├── .github └── workflows │ └── docker_image.yml ├── .gitignore ├── CMakeLists.txt ├── Dockerfile ├── LICENSE ├── README.md ├── include ├── action_mgr.hpp ├── drone_base.hpp ├── drone_pose.hpp ├── flight_controller_basic.hpp ├── flight_controller_interface.hpp ├── flight_controller_simple.hpp ├── flock_base.hpp ├── joystick.hpp ├── pid.hpp ├── planner_node.hpp └── simple_planner.hpp ├── launch ├── gazebo_launch.py ├── launch_one.py ├── launch_two.py ├── one.rviz └── two.rviz ├── package.xml └── src ├── action_mgr.cpp ├── drone_base.cpp ├── flight_controller_basic.cpp ├── flight_controller_simple.cpp ├── flock_base.cpp ├── flock_simple_path.py ├── planner_node.cpp ├── simple_planner.cpp ├── smooth_path_4poly_2min.py └── util.py /.github/workflows/docker_image.yml: -------------------------------------------------------------------------------- 1 | name: Docker Image CI 2 | 3 | on: 4 | push: 5 | branches: [ master ] 6 | pull_request: 7 | branches: [ master ] 8 | 9 | jobs: 10 | 11 | build: 12 | 13 | runs-on: ubuntu-20.04 14 | 15 | steps: 16 | - uses: actions/checkout@v2 17 | - name: Build the Docker image 18 | run: docker build . --file Dockerfile --tag flock2:$(date +%s) 19 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | .idea/ 3 | cmake-build-debug/ -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(flock2) 3 | 4 | #============= 5 | # Setup 6 | #============= 7 | 8 | # Default to C99 9 | if(NOT CMAKE_C_STANDARD) 10 | set(CMAKE_C_STANDARD 99) 11 | endif() 12 | 13 | # Default to C++14 14 | if(NOT CMAKE_CXX_STANDARD) 15 | set(CMAKE_CXX_STANDARD 14) 16 | endif() 17 | 18 | # Debugging: set _dump_all_variables to true 19 | set(_dump_all_variables false) 20 | if(_dump_all_variables) 21 | get_cmake_property(_variable_names VARIABLES) 22 | list(SORT _variable_names) 23 | foreach(_variable_name ${_variable_names}) 24 | message(STATUS "${_variable_name}=${${_variable_name}}") 25 | endforeach() 26 | endif() 27 | 28 | # Find packages 29 | find_package(ament_cmake REQUIRED) 30 | find_package(builtin_interfaces REQUIRED) 31 | find_package(geometry_msgs REQUIRED) 32 | find_package(nav_msgs REQUIRED) 33 | find_package(rclcpp REQUIRED) 34 | find_package(rclpy REQUIRED) 35 | find_package(ros2_shared REQUIRED) 36 | find_package(sensor_msgs REQUIRED) 37 | find_package(std_msgs REQUIRED) 38 | find_package(tello_msgs REQUIRED) 39 | find_package(tf2_msgs REQUIRED) 40 | find_package(visualization_msgs REQUIRED) 41 | 42 | # Local includes 43 | include_directories( 44 | include 45 | ${geometry_msgs_INCLUDE_DIRS} 46 | ${nav_msgs_INCLUDE_DIRS} 47 | ${rclcpp_INCLUDE_DIRS} 48 | ${ros2_shared_INCLUDE_DIRS} 49 | ${sensor_msgs_INCLUDE_DIRS} 50 | ${tello_msgs_INCLUDE_DIRS} 51 | ${tf2_msgs_INCLUDE_DIRS} 52 | ${visualization_msgs_INCLUDE_DIRS} 53 | ) 54 | 55 | #============= 56 | # Flock base node 57 | #============= 58 | 59 | add_executable( 60 | flock_base 61 | src/flock_base.cpp 62 | ) 63 | 64 | ament_target_dependencies( 65 | flock_base 66 | rclcpp 67 | ros2_shared 68 | sensor_msgs 69 | std_msgs 70 | ) 71 | 72 | #============= 73 | # Drone base node 74 | #============= 75 | 76 | add_executable( 77 | drone_base 78 | src/drone_base.cpp 79 | src/action_mgr.cpp 80 | src/flight_controller_basic.cpp 81 | src/flight_controller_simple.cpp 82 | ) 83 | 84 | ament_target_dependencies( 85 | drone_base 86 | geometry_msgs 87 | nav_msgs 88 | rclcpp 89 | ros2_shared 90 | sensor_msgs 91 | std_msgs 92 | tello_msgs 93 | ) 94 | 95 | #============= 96 | # Planner node 97 | #============= 98 | 99 | add_executable( 100 | planner_node 101 | src/planner_node.cpp 102 | src/simple_planner.cpp 103 | ) 104 | 105 | ament_target_dependencies( 106 | planner_node 107 | nav_msgs 108 | rclcpp 109 | ros2_shared 110 | std_msgs 111 | ) 112 | 113 | #============= 114 | # Install 115 | #============= 116 | 117 | # Install targets 118 | install( 119 | TARGETS 120 | flock_base 121 | drone_base 122 | planner_node 123 | DESTINATION lib/${PROJECT_NAME} 124 | ) 125 | 126 | # Install all Python programs 127 | install( 128 | PROGRAMS # PROGRAMS sets execute bits, FILES clears them 129 | src/flock_simple_path.py 130 | src/smooth_path_4poly_2min.py 131 | src/util.py 132 | DESTINATION lib/${PROJECT_NAME} 133 | ) 134 | 135 | # Install various directories 136 | install( 137 | DIRECTORY 138 | launch 139 | DESTINATION share/${PROJECT_NAME} 140 | ) 141 | 142 | ament_package() -------------------------------------------------------------------------------- /Dockerfile: -------------------------------------------------------------------------------- 1 | # To build: 2 | # docker build --pull --no-cache --tag flock2:foxy . 3 | 4 | # To run: 5 | # docker run -it flock2:foxy bash 6 | 7 | # I'm using this for smoke tests 8 | # To run flock2 in a docker container you will need to set up ports, x-windows, etc. 9 | 10 | FROM osrf/ros:foxy-desktop 11 | 12 | RUN apt-get update 13 | RUN apt-get upgrade -y 14 | 15 | RUN apt-get install -y libasio-dev 16 | RUN apt-get install -y python3-pip 17 | RUN yes | pip3 install 'transformations==2018.9.5' 18 | 19 | WORKDIR /work/flock2_ws/src 20 | 21 | RUN git clone https://github.com/clydemcqueen/flock2.git 22 | RUN git clone https://github.com/clydemcqueen/tello_ros.git 23 | RUN git clone https://github.com/ptrmu/ros2_shared.git 24 | RUN git clone https://github.com/ptrmu/fiducial_vlam.git 25 | 26 | WORKDIR /work/flock2_ws 27 | 28 | RUN rosdep install -y --from-paths . --ignore-src 29 | 30 | RUN /bin/bash -c "source /opt/ros/foxy/setup.bash && colcon build" 31 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2018, Clyde McQueen and Peter Mullen 2 | 3 | Redistribution and use in source and binary forms, with or without 4 | modification, are permitted provided that the following conditions are met: 5 | 6 | * Redistributions of source code must retain the above copyright notice, 7 | this list of conditions and the following disclaimer. 8 | 9 | * Redistributions in binary form must reproduce the above copyright notice, 10 | this list of conditions and the following disclaimer in the 11 | documentation and/or other materials provided with the distribution. 12 | 13 | * Neither the name of the copyright holder nor the names of its contributors may 14 | be used to endorse or promote products derived from this software without 15 | specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | POSSIBILITY OF SUCH DAMAGE. -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # `flock2` 2 | 3 | `flock2` can fly a swarm of [DJI Tello](https://store.dji.com/product/tello) drones. 4 | `flock2` is built on top of [ROS2](https://index.ros.org/doc/ros2/), 5 | [fiducial_vlam](https://github.com/ptrmu/fiducial_vlam), 6 | and [tello_ros](https://github.com/clydemcqueen/tello_ros). 7 | 8 | ## Installation 9 | 10 | ### 1. Set up your Linux environment 11 | 12 | Set up a Ubuntu 20.04 box or VM. 13 | 14 | ### 2. Set up your Python environment 15 | 16 | Use your favorite Python package manager to set up Python 3.6+ and the following packages: 17 | 18 | * numpy 1.15.2 19 | * transformations 2018.9.5 20 | 21 | ### 3. Set up your ROS environment 22 | 23 | [Install ROS2 Foxy](https://docs.ros.org/en/foxy/index.html) with the `ros-foxy-desktop` option. 24 | 25 | If you install binaries, be sure to also install the 26 | [development tools and ROS tools](https://docs.ros.org/en/foxy/Installation/Ubuntu-Development-Setup.html) 27 | from the source installation instructions. 28 | 29 | Install these additional packages: 30 | ~~~ 31 | sudo apt install ros-foxy-cv-bridge ros-foxy-camera-calibration-parsers ros-foxy-gazebo-ros 32 | ~~~ 33 | 34 | ### 4. Install dependencies 35 | 36 | Download, compile and install the following packages: 37 | ~~~ 38 | mkdir -p ~/flock2_ws/src 39 | cd ~/flock2_ws/src 40 | git clone https://github.com/clydemcqueen/flock2.git 41 | git clone https://github.com/clydemcqueen/tello_ros.git 42 | git clone https://github.com/ptrmu/fiducial_vlam.git 43 | git clone https://github.com/ptrmu/ros2_shared.git 44 | cd .. 45 | source /opt/ros/foxy/setup.bash 46 | # If you didn't install Gazebo, avoid building tello_gazebo: 47 | colcon build --event-handlers console_direct+ --packages-skip tello_gazebo 48 | ~~~ 49 | 50 | ## Running 51 | 52 | ### Flying a single drone 53 | 54 | `launch_one.py` will allow you to fly a drone using a wired XBox One gamepad. 55 | 56 | Turn on the drone, connect to `TELLO-XXXXX` via wifi, and launch ROS2: 57 | ~~~ 58 | cd ~/flock2_ws 59 | source install/setup.bash 60 | ros2 launch flock2 launch_one.py 61 | ~~~ 62 | 63 | Gamepad controls: 64 | * menu button to take off 65 | * view button to land 66 | * B to start mission 67 | * A to stop mission 68 | 69 | ### Flying multiple drones 70 | 71 | `launch_two.py` provides an example for flying multiple drones. 72 | 73 | Key elements of multi-drone missions: 74 | 75 | * All drones must be networked together. One way to do this is to connect to each drone's wifi 76 | using a Raspberry Pi 3 or similar device, and forward all UDP packets from the Pi to the host computer. 77 | See [udp_forward](https://github.com/clydemcqueen/udp_forward) for an example using 2 Tello drones. 78 | * Global nodes such as `flock_base` should have exactly 1 instance running. 79 | Per-drone nodes such as `drone_base` should have 1 instance running per drone. 80 | * Each drone has it's own ROS topic namespace. The default namespace for 1 drone is `solo`. 81 | * Each drone must have it's own URDF file with the appropriate coordinate frames. 82 | * The joystick controls one drone at a time. Hit the right bumper to select a different drone. 83 | * All drones participate in the mission. 84 | 85 | ## Design 86 | 87 | ### Coordinate frames 88 | 89 | [ROS world coordinate frames](http://www.ros.org/reps/rep-0103.html) are ENU (East, North, Up). 90 | 91 | There are 3 significant coordinate frames in `flock2`: 92 | * The world frame is `map` 93 | * Each drone has a base coordinate frame. The default for 1 drone is `base_link` 94 | * Each drone has a camera coordinate frame. The default for 1 drone is `camera_frame` 95 | 96 | ### The arena 97 | 98 | An arena is a right rectangular prism defined by 2 points: (x1=0, y1=0, z1=0) and (x2, y2, z2). 99 | z1 defines the ground, so z2 must be positive. 100 | The ground must be flat. 101 | Drones will never fly outside of the arena. 102 | 103 | There must be at least one 6x6 ArUco marker, with id 1, associated with the arena. 104 | Marker 1's pose is known in advance, the other ArUco marker poses are estimated during flight. 105 | The drones will use ArUco marker poses to estimate their current pose. 106 | 107 | ### The mission (under development) 108 | 109 | A mission is defined as autonomous flight by all drones. 110 | A mission is initiated when the user hits the _start mission_ button on the gamepad. 111 | A mission will end on it's own, or when the user hits the _stop mission_ button. 112 | 113 | All drones must be able to localize on the ground to start a mission. 114 | In practice this means that all drones must be able to see marker 1 while sitting on the ground, 115 | or at least one drone has to be flown around manually to build a good map before the mission starts. 116 | 117 | The overall mission dataflow looks like this: 118 | 119 | 1. `flock_base` publishes a message on the `/start_mission` topic 120 | 2. `planner_node` generates an overall pattern of flight for all drones, and publishes a 121 | sequence waypoints for each drone on `/[prefix]/plan` 122 | 3. `drone_base` subscribes to `~plan` and `~base_odom`, runs a PID controller, 123 | and sends commands to `tello_ros` 124 | 125 | If odometry stops arriving `drone_base` will execute a series of recovery tasks, which might include landing. 126 | 127 | If flight indicates that a drone has a low battery `drone_base` will land the drone. 128 | 129 | ### Simulating 4 drones in Gazebo 130 | 131 | [Install Gazebo 9 and build tello_gazebo](https://github.com/clydemcqueen/tello_ros/tree/master/tello_gazebo), 132 | if you haven't already. 133 | 134 | ~~~ 135 | cd ~/flock2_ws 136 | source install/setup.bash 137 | export GAZEBO_MODEL_PATH=${PWD}/install/tello_gazebo/share/tello_gazebo/models 138 | source /usr/share/gazebo/setup.sh 139 | ros2 launch flock2 gazebo_launch.py 140 | ~~~ 141 | 142 | Hit the "B" button on the XBox controller to start the mission. 143 | You should see 4 drones take off, rotate through 4 positions, then land. 144 | 145 | ### Node details 146 | 147 | #### flock_base 148 | 149 | Orchestrates the flight of one or more Tello drones. 150 | 151 | ##### Subscribed topics 152 | 153 | * `~joy` [sensor_msgs/Joy](http://docs.ros.org/api/sensor_msgs/html/msg/Joy.html) 154 | 155 | ##### Published topics 156 | 157 | * `/start_mission` [std_msgs/Empty](http://docs.ros.org/api/std_msgs/html/msg/Empty.html) 158 | * `/stop_mission` [std_msgs/Empty](http://docs.ros.org/api/std_msgs/html/msg/Empty.html) 159 | * `~[prefix]/joy` [sensor_msgs/Joy](http://docs.ros.org/api/sensor_msgs/html/msg/Joy.html) 160 | 161 | ##### Parameters 162 | 163 | * `drones` is an array of strings, where each string is a topic prefix 164 | 165 | #### drone_base 166 | 167 | Controls a single Tello drone. Akin to `move_base` in the ROS navigation stack. 168 | 169 | ##### Subscribed topics 170 | 171 | * `/start_mission` [std_msgs/Empty](http://docs.ros.org/api/std_msgs/html/msg/Empty.html) 172 | * `/stop_mission` [std_msgs/Empty](http://docs.ros.org/api/std_msgs/html/msg/Empty.html) 173 | * `~joy` [sensor_msgs/Joy](http://docs.ros.org/api/sensor_msgs/html/msg/Joy.html) 174 | * `~tello_response` tello_msgs/TelloResponse 175 | * `~flight_data` tello_msgs/FlightData 176 | * `~base_odom` [nav_msgs/Odometry](http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html) 177 | 178 | ##### Published topics 179 | 180 | * `~cmd_vel` [geometry_msgs/Twist](http://docs.ros.org/api/geometry_msgs/html/msg/Twist.html) 181 | 182 | ##### Published services 183 | 184 | * `~tello_command` tello_msgs/TelloCommand 185 | 186 | #### planner_node 187 | 188 | Compute and publish a set of waypoints for each drone in a flock. 189 | 190 | ##### Subscribed topics 191 | 192 | * `/start_mission` [std_msgs/Empty](http://docs.ros.org/api/std_msgs/html/msg/Empty.html) 193 | * `/stop_mission` [std_msgs/Empty](http://docs.ros.org/api/std_msgs/html/msg/Empty.html) 194 | * `~[prefix]/base_odom` [nav_msgs/Odometry](http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html) 195 | 196 | ##### Published topics 197 | 198 | * `~[prefix]/plan` [nav_msgs/Path](http://docs.ros.org/api/nav_msgs/html/msg/Path.html) 199 | 200 | ##### Parameters 201 | 202 | * `drones` is an array of strings, where the number of strings is the number of drones and each string is the topic prefix for a drone. 203 | For example, `['d1', 'd2']` refers to 2 drones, and the flight data topic for the first drone is `/d1/flight_data`. 204 | The default is `['solo']`. 205 | * `arena_x` defines the X extent of the arena, in meters. The default is 2. 206 | * `arena_y` defines the Y extent of the arena, in meters. The default is 2. 207 | * `arena_z` defines the Z extent of the arena, in meters. Must be greater than 1.5. The default is 2. 208 | 209 | ## Versions and branches 210 | 211 | `flock2` was developed along with several other projects while ROS2 was rapidly changing. 212 | All of the related projects adopted similar conventions around branch names: 213 | * the `master` branch works with the latest ROS2 release (Foxy as of this writing) 214 | * there may be branches for older ROS2 versions, such as `crystal`, `dashing` or `eloquent` 215 | 216 | The following projects and branches were tested together: 217 | 218 | * ROS2 Foxy with fiducial_vlam: 219 | * git clone https://github.com/ptrmu/ros2_shared.git 220 | * git clone https://github.com/ptrmu/fiducial_vlam.git 221 | * git clone https://github.com/clydemcqueen/tello_ros.git 222 | * git clone https://github.com/clydemcqueen/flock2.git 223 | 224 | -------------------------------------------------------------------------------- /include/action_mgr.hpp: -------------------------------------------------------------------------------- 1 | #ifndef ACTION_MGR_H 2 | #define ACTION_MGR_H 3 | 4 | #include "rclcpp/rclcpp.hpp" 5 | #include "tello_msgs/srv/tello_action.hpp" 6 | #include "tello_msgs/msg/tello_response.hpp" 7 | 8 | namespace drone_base 9 | { 10 | 11 | //========================== 12 | // tello_driver uses a ROS service plus a ROS topic to simulate an action. 13 | // 14 | // Overall flow: 15 | // 1. drone_base sends a TelloAction::Request on the tello_action service 16 | // 2. the drone responds with a TelloAction::Response 17 | // 3. later, the drone sends a TelloResponse message on the tello_response topic 18 | // 19 | // Only one action can be active at a time. 20 | //========================== 21 | 22 | enum class Action; 23 | 24 | class ActionMgr 25 | { 26 | public: 27 | 28 | enum class State 29 | { 30 | not_sent, // TelloAction::Request not sent yet 31 | waiting_for_future, // Waiting for TelloAction::Response 32 | waiting_for_response, // Waiting for TelloResponse 33 | succeeded, // Action succeeded, see the result string 34 | failed, // Action failed, see the result string 35 | failed_lost_connection, // Action failed, there's no connection to the drone 36 | }; 37 | 38 | private: 39 | 40 | // Init by constructor 41 | rclcpp::Logger logger_; 42 | rclcpp::Client::SharedPtr client_; 43 | State state_ = State::not_sent; 44 | 45 | // Init by send() 46 | Action action_; 47 | std::string action_str_; 48 | std::shared_future future_; 49 | std::string result_str_; 50 | 51 | public: 52 | 53 | explicit ActionMgr(rclcpp::Logger logger, rclcpp::Client::SharedPtr client) : 54 | logger_{logger}, client_{client} 55 | {} 56 | 57 | ~ActionMgr() 58 | {} 59 | 60 | State send(Action action, std::string action_str); 61 | 62 | State spin_once(); 63 | 64 | State complete(tello_msgs::msg::TelloResponse::SharedPtr msg); 65 | 66 | Action action() 67 | { return action_; } 68 | 69 | std::string action_str() 70 | { return action_str_; } 71 | 72 | std::string result_str() 73 | { return result_str_; } 74 | 75 | bool busy() 76 | { return state_ == State::waiting_for_future || state_ == State::waiting_for_response; } 77 | }; 78 | 79 | } // namespace drone_base 80 | 81 | #endif // ACTION_MGR_H 82 | 83 | -------------------------------------------------------------------------------- /include/drone_base.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DRONE_BASE_H 2 | #define DRONE_BASE_H 3 | 4 | #include "rclcpp/rclcpp.hpp" 5 | #include "geometry_msgs/msg/twist.hpp" 6 | #include "nav_msgs/msg/odometry.hpp" 7 | #include "nav_msgs/msg/path.hpp" 8 | #include "sensor_msgs/msg/joy.hpp" 9 | #include "std_msgs/msg/empty.hpp" 10 | #include "tello_msgs/msg/flight_data.hpp" 11 | 12 | #include "action_mgr.hpp" 13 | #include "ros2_shared/context_macros.hpp" 14 | #include "joystick.hpp" 15 | 16 | namespace drone_base 17 | { 18 | 19 | //============================================================================= 20 | // States, events and actions 21 | //============================================================================= 22 | 23 | // Flight states 24 | enum class State 25 | { 26 | unknown, // No flight data 27 | ready, // Ready for manual flight 28 | flight, // Flying (autonomous operation not available) 29 | ready_odom, // Ready for manual or autonomous flight 30 | flight_odom, // Flying (autonomous operation available) 31 | low_battery, // Low battery (must be swapped) 32 | }; 33 | 34 | // Events happen 35 | enum class Event 36 | { 37 | connected, // Connected to the drone 38 | disconnected, // Lost the connection 39 | odometry_started, // Odometry started 40 | odometry_stopped, // Odometry stopped 41 | low_battery, // Low battery detected 42 | }; 43 | 44 | // The user or autonomous controller can take actions 45 | enum class Action 46 | { 47 | takeoff, 48 | land, 49 | }; 50 | 51 | //============================================================================= 52 | // DroneBase Context 53 | //============================================================================= 54 | 55 | #define DRONE_BASE_ALL_PARAMS \ 56 | CXT_MACRO_MEMBER( /* Error if no additional flight_data message within this duration */ \ 57 | flight_data_timeout_sec, \ 58 | double, 1.5) \ 59 | CXT_MACRO_MEMBER( /* Error if no additional odom message within this duration */ \ 60 | odom_timeout_sec, \ 61 | double, 1.5) \ 62 | /* End of list */ 63 | 64 | #define DRONE_BASE_ALL_OTHERS \ 65 | CXT_MACRO_MEMBER( /* Error if no additional flight_data message within this duration */ \ 66 | flight_data_timeout, \ 67 | rclcpp::Duration, 0) \ 68 | CXT_MACRO_MEMBER( /* Error if no additional odom message within this duration */ \ 69 | odom_timeout, \ 70 | rclcpp::Duration, 0) \ 71 | /* End of list */ 72 | 73 | 74 | #undef CXT_MACRO_MEMBER 75 | #define CXT_MACRO_MEMBER(n, t, d) CXT_MACRO_DEFINE_MEMBER(n, t, d) 76 | 77 | struct DroneBaseContext 78 | { 79 | CXT_MACRO_DEFINE_MEMBERS(DRONE_BASE_ALL_PARAMS) 80 | DRONE_BASE_ALL_OTHERS 81 | }; 82 | 83 | //============================================================================= 84 | // DroneBase node 85 | //============================================================================= 86 | 87 | class FlightControllerInterface; 88 | 89 | class DroneBase : public rclcpp::Node 90 | { 91 | // DroneBase context containing parameters 92 | DroneBaseContext cxt_; 93 | 94 | // Drone state 95 | State state_ = State::unknown; 96 | 97 | // Flight data 98 | rclcpp::Time flight_data_time_; 99 | 100 | // Odom data 101 | rclcpp::Time odom_time_; 102 | 103 | // Drone action manager 104 | std::unique_ptr action_mgr_; 105 | 106 | // Mission state 107 | bool mission_ = false; // We're in a mission (flying autonomously) 108 | std::unique_ptr fc_{}; 109 | 110 | // Joystick assignments 111 | int joy_axis_throttle_ = JOY_AXIS_RIGHT_FB; 112 | int joy_axis_strafe_ = JOY_AXIS_RIGHT_LR; 113 | int joy_axis_vertical_ = JOY_AXIS_LEFT_FB; 114 | int joy_axis_yaw_ = JOY_AXIS_LEFT_LR; 115 | int joy_button_takeoff_ = JOY_BUTTON_MENU; 116 | int joy_button_land_ = JOY_BUTTON_VIEW; 117 | int joy_button_shift_ = JOY_BUTTON_LEFT_BUMPER; 118 | int joy_axis_trim_lr_ = JOY_AXIS_TRIM_LR; 119 | int joy_axis_trim_fb_ = JOY_AXIS_TRIM_FB; 120 | 121 | // Publications 122 | rclcpp::Publisher::SharedPtr cmd_vel_pub_; 123 | 124 | // Subscriptions 125 | rclcpp::Subscription::SharedPtr start_mission_sub_; 126 | rclcpp::Subscription::SharedPtr stop_mission_sub_; 127 | rclcpp::Subscription::SharedPtr joy_sub_; 128 | rclcpp::Subscription::SharedPtr tello_response_sub_; 129 | rclcpp::Subscription::SharedPtr flight_data_sub_; 130 | rclcpp::Subscription::SharedPtr odom_sub_; 131 | rclcpp::Subscription::SharedPtr plan_sub_; 132 | 133 | public: 134 | 135 | explicit DroneBase(); 136 | 137 | void spin_once(); 138 | 139 | private: 140 | void validate_parameters(); 141 | 142 | // Callbacks 143 | void joy_callback(sensor_msgs::msg::Joy::SharedPtr msg); 144 | 145 | void start_mission_callback(std_msgs::msg::Empty::SharedPtr msg); 146 | 147 | void stop_mission_callback(std_msgs::msg::Empty::SharedPtr msg); 148 | 149 | void tello_response_callback(tello_msgs::msg::TelloResponse::SharedPtr msg); 150 | 151 | void flight_data_callback(tello_msgs::msg::FlightData::SharedPtr msg); 152 | 153 | void odom_callback(nav_msgs::msg::Odometry::SharedPtr msg); 154 | 155 | void plan_callback(nav_msgs::msg::Path::SharedPtr msg); 156 | 157 | // State transition 158 | void start_action(Action action); 159 | 160 | void transition_state(Action action); 161 | 162 | void transition_state(Event event); 163 | 164 | void transition_state(State next_state); 165 | 166 | // All stop: set twist_ to 0, 0, 0, 0 and publish 167 | void all_stop(); 168 | 169 | // Stop mission 170 | void stop_mission(); 171 | }; 172 | 173 | } // namespace drone_base 174 | 175 | #endif // DRONE_BASE_H 176 | -------------------------------------------------------------------------------- /include/drone_pose.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DRONE_POSE_H 2 | #define DRONE_POSE_H 3 | 4 | #include "geometry_msgs/msg/pose_stamped.hpp" 5 | #include "geometry_msgs/msg/vector3.hpp" 6 | #include "nav_msgs/msg/odometry.hpp" 7 | #include "tf2_geometry_msgs/tf2_geometry_msgs.h" 8 | 9 | namespace drone_base 10 | { 11 | 12 | //============================================================================= 13 | // Utilities 14 | //============================================================================= 15 | 16 | struct PoseUtil 17 | { 18 | // Move an angle to the region [-M_PI, M_PI] 19 | static double norm_angle(double a) 20 | { 21 | while (a < -M_PI) { 22 | a += 2 * M_PI; 23 | } 24 | while (a > M_PI) { 25 | a -= 2 * M_PI; 26 | } 27 | 28 | return a; 29 | } 30 | 31 | // Compute a 2d point in a rotated frame (v' = R_transpose * v) 32 | static void rotate_frame(const double x, const double y, const double theta, double &x_r, double &y_r) 33 | { 34 | x_r = x * cos(theta) + y * sin(theta); 35 | y_r = y * cos(theta) - x * sin(theta); 36 | } 37 | 38 | static double clamp(const double v, const double min, const double max) 39 | { 40 | return v > max ? max : (v < min ? min : v); 41 | } 42 | 43 | // rclcpp::Time t() initializes nanoseconds to 0 44 | static bool is_valid_time(rclcpp::Time &t) 45 | { 46 | return t.nanoseconds() > 0; 47 | } 48 | }; 49 | 50 | //===================================================================================== 51 | // 4 DoF drone pose, in the world frame 52 | //===================================================================================== 53 | 54 | struct DronePose 55 | { 56 | double x; 57 | double y; 58 | double z; 59 | double yaw; 60 | 61 | constexpr DronePose() : x(0), y(0), z(0), yaw(0) 62 | {} 63 | 64 | void fromMsg(const geometry_msgs::msg::Pose &msg) 65 | { 66 | x = msg.position.x; 67 | y = msg.position.y; 68 | z = msg.position.z; 69 | 70 | // Quaternion to yaw 71 | tf2::Quaternion q; 72 | tf2::fromMsg(msg.orientation, q); 73 | double roll = 0, pitch = 0; 74 | tf2::Matrix3x3(q).getRPY(roll, pitch, yaw); 75 | } 76 | 77 | void toMsg(geometry_msgs::msg::Pose &msg) const 78 | { 79 | msg.position.x = x; 80 | msg.position.y = y; 81 | msg.position.z = z; 82 | 83 | // Yaw to quaternion 84 | tf2::Quaternion q; 85 | q.setRPY(0, 0, yaw); 86 | msg.orientation = tf2::toMsg(q); 87 | } 88 | 89 | // Within some epsilon 90 | bool close_enough(const DronePose &that, double EPSILON_XYZ = 0.15, double EPSILON_YAW = 0.15) const 91 | { 92 | return 93 | std::abs(x - that.x) < EPSILON_XYZ && 94 | std::abs(y - that.y) < EPSILON_XYZ && 95 | std::abs(z - that.z) < EPSILON_XYZ && 96 | std::abs(yaw - that.yaw) < EPSILON_YAW; 97 | } 98 | }; 99 | 100 | } // namespace drone_base 101 | 102 | #endif // DRONE_POSE_H 103 | -------------------------------------------------------------------------------- /include/flight_controller_basic.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FLIGHT_CONTROLLER_BASIC_HPP 2 | #define FLIGHT_CONTROLLER_BASIC_HPP 3 | 4 | #include "ros2_shared/context_macros.hpp" 5 | #include "flight_controller_interface.hpp" 6 | #include "pid.hpp" 7 | 8 | namespace drone_base 9 | { 10 | 11 | #define BASIC_CONTROLLER_ALL_PARAMS \ 12 | CXT_MACRO_MEMBER( /* Allow drone to stabilize for this duration */ \ 13 | stabilize_time_sec, \ 14 | double, 5.) \ 15 | /* End of list */ 16 | 17 | #define BASIC_CONTROLLER_ALL_OTHERS \ 18 | CXT_MACRO_MEMBER( /* Allow drone to stabilize for this duration */ \ 19 | stabilize_time, \ 20 | rclcpp::Duration, 0) \ 21 | /* End of list */ 22 | 23 | class FlightControllerBasic : public FlightControllerInterface 24 | { 25 | 26 | #undef CXT_MACRO_MEMBER 27 | #define CXT_MACRO_MEMBER(n, t, d) CXT_MACRO_DEFINE_MEMBER(n, t, d) 28 | CXT_MACRO_DEFINE_MEMBERS(BASIC_CONTROLLER_ALL_PARAMS) 29 | BASIC_CONTROLLER_ALL_OTHERS 30 | 31 | DronePose last_pose_; // Last pose from odom 32 | rclcpp::Time last_odom_time_; // Last pose from odom 33 | 34 | DronePose prev_target_; // Previous target pose 35 | DronePose curr_target_; // Current target pose 36 | rclcpp::Time prev_target_time_; // Time we left the previous target 37 | rclcpp::Time curr_target_time_; // Deadline to hit the current target 38 | double vx_{}, vy_{}, vz_{}, vyaw_{}; // Velocity required to hit the current target 39 | 40 | // PID controllers 41 | pid::Controller x_controller_{false, 0.1, 0, 0}; 42 | pid::Controller y_controller_{false, 0.1, 0, 0}; 43 | pid::Controller z_controller_{false, 0.1, 0, 0}; 44 | pid::Controller yaw_controller_{true, 0.2, 0, 0}; 45 | 46 | void validate_parameters(); 47 | 48 | public: 49 | FlightControllerBasic(rclcpp::Node &node, rclcpp::Publisher::SharedPtr &cmd_vel_pub); 50 | 51 | void _reset() override; 52 | 53 | void _set_target(int target) override; 54 | 55 | bool _odom_callback(const nav_msgs::msg::Odometry::SharedPtr &msg) override; 56 | }; 57 | 58 | } 59 | 60 | #endif //FLIGHT_CONTROLLER_BASIC_HPP 61 | -------------------------------------------------------------------------------- /include/flight_controller_interface.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FLIGHT_CONTROLLER_INTERFACE_HPP 2 | #define FLIGHT_CONTROLLER_INTERFACE_HPP 3 | 4 | #include "geometry_msgs/msg/twist.hpp" 5 | #include "nav_msgs/msg/odometry.hpp" 6 | #include "nav_msgs/msg/path.hpp" 7 | 8 | #include "drone_pose.hpp" 9 | 10 | namespace drone_base 11 | { 12 | class FlightControllerInterface 13 | { 14 | protected: 15 | int target_{}; // Current target (index into plan_) 16 | nav_msgs::msg::Path plan_{}; // The flight plan 17 | 18 | rclcpp::Node &node_; 19 | rclcpp::Publisher::SharedPtr &cmd_vel_pub_; 20 | 21 | // Implemented by the overriding class. 22 | virtual void _reset() = 0; 23 | 24 | virtual void _set_target(int target) = 0; 25 | 26 | virtual bool _odom_callback(const nav_msgs::msg::Odometry::SharedPtr &msg) = 0; 27 | 28 | public: 29 | explicit FlightControllerInterface(rclcpp::Node &node, 30 | rclcpp::Publisher::SharedPtr &cmd_vel_pub) 31 | : node_{node}, cmd_vel_pub_{cmd_vel_pub} 32 | {} 33 | 34 | FlightControllerInterface() = delete; 35 | 36 | virtual ~FlightControllerInterface() = default; 37 | 38 | void reset() 39 | { 40 | plan_ = nav_msgs::msg::Path(); 41 | _reset(); 42 | } 43 | 44 | void set_target(int target) 45 | { 46 | _set_target(target); 47 | } 48 | 49 | void set_plan(const nav_msgs::msg::Path::SharedPtr &msg) 50 | { 51 | _reset(); 52 | plan_ = *msg; 53 | _set_target(0); 54 | } 55 | 56 | bool odom_callback(const nav_msgs::msg::Odometry::SharedPtr &msg) 57 | { 58 | return _odom_callback(msg); 59 | } 60 | 61 | void publish_velocity(double throttle, double strafe, double vertical, double yaw) 62 | { 63 | geometry_msgs::msg::Twist twist; 64 | twist.linear.x = PoseUtil::clamp(throttle, -1.0, 1.0); 65 | twist.linear.y = PoseUtil::clamp(strafe, -1.0, 1.0); 66 | twist.linear.z = PoseUtil::clamp(vertical, -1.0, 1.0); 67 | twist.angular.z = PoseUtil::clamp(yaw, -1.0, 1.0); 68 | cmd_vel_pub_->publish(twist); 69 | } 70 | 71 | bool is_plan_complete() 72 | { 73 | return target_ >= plan_.poses.size(); 74 | } 75 | 76 | bool have_plan() 77 | { 78 | return !plan_.poses.empty(); 79 | } 80 | }; 81 | } 82 | 83 | #endif //FLIGHT_CONTROLLER_INTERFACE_HPP 84 | -------------------------------------------------------------------------------- /include/flight_controller_simple.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FLIGHT_CONTROLLER_SIMPLE_HPP 2 | #define FLIGHT_CONTROLLER_SIMPLE_HPP 3 | 4 | #include "rclcpp/rclcpp.hpp" 5 | 6 | #include "geometry_msgs/msg/twist.hpp" 7 | #include "nav_msgs/msg/odometry.hpp" 8 | #include "nav_msgs/msg/path.hpp" 9 | 10 | #include "ros2_shared/context_macros.hpp" 11 | #include "flight_controller_interface.hpp" 12 | #include "pid.hpp" 13 | 14 | namespace drone_base 15 | { 16 | #define SIMPLE_CONTROLLER_ALL_PARAMS \ 17 | CXT_MACRO_MEMBER( /* Allow drone to stabilize for this duration */ \ 18 | stabilize_time_sec, \ 19 | double, 5.) \ 20 | CXT_MACRO_MEMBER( /* PID coefficients */ \ 21 | pid_x_kp, \ 22 | double, 1.0) \ 23 | CXT_MACRO_MEMBER( /* PID coefficients */ \ 24 | pid_x_kd, \ 25 | double, 0.15) \ 26 | CXT_MACRO_MEMBER( /* PID coefficients */ \ 27 | pid_x_ki, \ 28 | double, 0.0) \ 29 | CXT_MACRO_MEMBER( /* PID coefficients */ \ 30 | pid_y_kp, \ 31 | double, 1.0) \ 32 | CXT_MACRO_MEMBER( /* PID coefficients */ \ 33 | pid_y_kd, \ 34 | double, 0.15) \ 35 | CXT_MACRO_MEMBER( /* PID coefficients */ \ 36 | pid_y_ki, \ 37 | double, 0.0) \ 38 | CXT_MACRO_MEMBER( /* PID coefficients */ \ 39 | pid_z_kp, \ 40 | double, 0.1) \ 41 | CXT_MACRO_MEMBER( /* PID coefficients */ \ 42 | pid_z_kd, \ 43 | double, 0.0) \ 44 | CXT_MACRO_MEMBER( /* PID coefficients */ \ 45 | pid_z_ki, \ 46 | double, 0.0) \ 47 | CXT_MACRO_MEMBER( /* PID coefficients */ \ 48 | pid_yaw_kp, \ 49 | double, 0.1) \ 50 | CXT_MACRO_MEMBER( /* PID coefficients */ \ 51 | pid_yaw_kd, \ 52 | double, 0.0) \ 53 | CXT_MACRO_MEMBER( /* PID coefficients */ \ 54 | pid_yaw_ki, \ 55 | double, 0.0) \ 56 | CXT_MACRO_MEMBER( /* Waypoint reached criterion */ \ 57 | close_enough_xyz, \ 58 | double, 0.15) \ 59 | CXT_MACRO_MEMBER( /* Waypoint reached criterion */ \ 60 | close_enough_yaw, \ 61 | double, 0.15) \ 62 | /* End of list */ 63 | 64 | #define SIMPLE_CONTROLLER_ALL_OTHERS \ 65 | CXT_MACRO_MEMBER( /* Allow drone to stabilize for this duration */ \ 66 | stabilize_time, \ 67 | rclcpp::Duration, 0) \ 68 | /* End of list */ 69 | 70 | class FlightControllerSimple : public FlightControllerInterface 71 | { 72 | 73 | #undef CXT_MACRO_MEMBER 74 | #define CXT_MACRO_MEMBER(n, t, d) CXT_MACRO_DEFINE_MEMBER(n, t, d) 75 | CXT_MACRO_DEFINE_MEMBERS(SIMPLE_CONTROLLER_ALL_PARAMS) 76 | SIMPLE_CONTROLLER_ALL_OTHERS 77 | 78 | rclcpp::Time last_odom_time_; // time of last odometry message 79 | DronePose last_pose_; // pose from last odometry message 80 | 81 | rclcpp::Time curr_target_time_; // Deadline to hit the current target 82 | DronePose curr_target_; // Current target pose 83 | 84 | // PID controllers 85 | pid::Controller2 x_controller_{false, 0.1, 0}; 86 | pid::Controller2 y_controller_{false, 0.1, 0}; 87 | pid::Controller2 z_controller_{false, 0.1, 0}; 88 | pid::Controller2 yaw_controller_{true, 0.2, 0}; 89 | 90 | void validate_parameters(); 91 | 92 | public: 93 | FlightControllerSimple(rclcpp::Node &node, rclcpp::Publisher::SharedPtr &cmd_vel_pub); 94 | 95 | void _reset() override; 96 | 97 | void _set_target(int target) override; 98 | 99 | bool _odom_callback(const nav_msgs::msg::Odometry::SharedPtr &msg) override; 100 | }; 101 | } 102 | #endif //FLIGHT_CONTROLLER_SIMPLE_HPP 103 | -------------------------------------------------------------------------------- /include/flock_base.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FLOCK_BASE_H 2 | #define FLOCK_BASE_H 3 | 4 | #include "rclcpp/rclcpp.hpp" 5 | #include "sensor_msgs/msg/joy.hpp" 6 | #include "std_msgs/msg/empty.hpp" 7 | 8 | #include "ros2_shared/context_macros.hpp" 9 | #include "joystick.hpp" 10 | 11 | namespace flock_base 12 | { 13 | 14 | #define FLOCK_BASE_ALL_PARAMS \ 15 | CXT_MACRO_MEMBER( /* */ \ 16 | drones, \ 17 | std::vector, "solo") \ 18 | /* End of list */ 19 | 20 | class FlockBase : public rclcpp::Node 21 | { 22 | #undef CXT_MACRO_MEMBER 23 | #define CXT_MACRO_MEMBER(n, t, d) CXT_MACRO_DEFINE_MEMBER(n, t, d) 24 | CXT_MACRO_DEFINE_MEMBERS(FLOCK_BASE_ALL_PARAMS) 25 | 26 | // Global state 27 | bool mission_; 28 | 29 | // Users can use the joystick to manually control one drone at a time 30 | int manual_control_{0}; 31 | 32 | // Joystick assignments 33 | int joy_button_stop_mission_ = JOY_BUTTON_A; 34 | int joy_button_start_mission_ = JOY_BUTTON_B; 35 | int joy_button_next_drone_ = JOY_BUTTON_RIGHT_BUMPER; 36 | 37 | // Subscriptions 38 | rclcpp::Subscription::SharedPtr joy_sub_; 39 | 40 | // Publications 41 | std::vector::SharedPtr> joy_pubs_; 42 | rclcpp::Publisher::SharedPtr start_mission_pub_; 43 | rclcpp::Publisher::SharedPtr stop_mission_pub_; 44 | 45 | public: 46 | 47 | explicit FlockBase(); 48 | 49 | ~FlockBase() 50 | {} 51 | 52 | private: 53 | void joy_callback(const sensor_msgs::msg::Joy::SharedPtr msg); 54 | 55 | void validate_parameters(); 56 | }; 57 | 58 | } // namespace flock_base 59 | 60 | #endif // FLOCK_BASE_H 61 | -------------------------------------------------------------------------------- /include/joystick.hpp: -------------------------------------------------------------------------------- 1 | #ifndef JOYSTICK_H 2 | #define JOYSTICK_H 3 | 4 | // XBox One joystick axes and buttons 5 | const int JOY_AXIS_LEFT_LR = 0; // Left stick left/right; 1.0 is left and -1.0 is right 6 | const int JOY_AXIS_LEFT_FB = 1; // Left stick forward/back; 1.0 is forward and -1.0 is back 7 | const int JOY_AXIS_LEFT_TRIGGER = 2; // Left trigger 8 | const int JOY_AXIS_RIGHT_LR = 3; // Right stick left/right; 1.0 is left and -1.0 is right 9 | const int JOY_AXIS_RIGHT_FB = 4; // Right stick forward/back; 1.0 is forward and -1.0 is back 10 | const int JOY_AXIS_RIGHT_TRIGGER = 5; // Right trigger 11 | const int JOY_AXIS_TRIM_LR = 6; // Trim left/right; 1.0 for left and -1.0 for right 12 | const int JOY_AXIS_TRIM_FB = 7; // Trim forward/back; 1.0 for forward and -1.0 for back 13 | const int JOY_BUTTON_A = 0; // A button 14 | const int JOY_BUTTON_B = 1; // B button 15 | const int JOY_BUTTON_X = 2; // X button 16 | const int JOY_BUTTON_Y = 3; // Y button 17 | const int JOY_BUTTON_LEFT_BUMPER = 4; // Left bumper 18 | const int JOY_BUTTON_RIGHT_BUMPER = 5; // Right bumper 19 | const int JOY_BUTTON_VIEW = 6; // View button 20 | const int JOY_BUTTON_MENU = 7; // Menu button 21 | const int JOY_BUTTON_LOGO = 8; // XBox logo button 22 | const int JOY_BUTTON_LEFT_STICK = 9; // Left stick button 23 | const int JOY_BUTTON_RIGHT_STICK = 10; // Right stick button 24 | 25 | #endif // JOYSTICK_H -------------------------------------------------------------------------------- /include/pid.hpp: -------------------------------------------------------------------------------- 1 | #ifndef PID_H 2 | #define PID_H 3 | 4 | #include 5 | 6 | namespace pid 7 | { 8 | 9 | class Controller 10 | { 11 | private: 12 | 13 | bool angle_; // True if we're controlling an angle [-pi, pi] 14 | double target_ = 0; 15 | double prev_error_ = 0; 16 | double integral_ = 0; 17 | double Kp_; 18 | double Ki_; 19 | double Kd_; 20 | 21 | public: 22 | 23 | // Standard constructor 24 | Controller(bool angle, double Kp, double Ki, double Kd) 25 | { 26 | angle_ = angle; 27 | Kp_ = Kp; 28 | Ki_ = Ki; 29 | Kd_ = Kd; 30 | } 31 | 32 | // Intuitive constructor 33 | Controller(bool angle, double damping_ratio, double natural_frequency) 34 | { 35 | angle_ = angle; 36 | Kp_ = natural_frequency * natural_frequency * (1 + 2 * damping_ratio); 37 | Ki_ = natural_frequency * natural_frequency * natural_frequency; 38 | Kd_ = natural_frequency * (1 + 2 * damping_ratio); 39 | } 40 | 41 | // Set target 42 | void set_target(double target) 43 | { 44 | target_ = target; 45 | prev_error_ = 0; 46 | integral_ = 0; 47 | } 48 | 49 | void set_coefficients(double Kp, double Ki, double Kd) 50 | { 51 | Kp_ = Kp; 52 | Ki_ = Ki; 53 | Kd_ = Kd; 54 | } 55 | 56 | auto target() const 57 | { 58 | return target_; 59 | } 60 | 61 | // Run one calculation 62 | double calc(double state, double dt, double bias) 63 | { 64 | double error = target_ - state; 65 | 66 | if (angle_) { 67 | // Deal with discontinuity 68 | while (error < -M_PI) { 69 | error += 2 * M_PI; 70 | } 71 | while (error > M_PI) { 72 | error -= 2 * M_PI; 73 | } 74 | } 75 | 76 | integral_ = integral_ + (error * dt); 77 | double derivative = (error - prev_error_) / dt; 78 | prev_error_ = error; 79 | 80 | return Kp_ * error + Ki_ * integral_ + Kd_ * derivative + bias; 81 | } 82 | }; 83 | 84 | class Controller2 85 | { 86 | private: 87 | 88 | bool angle_; // True if we're controlling an angle [-pi, pi] 89 | double Kp_; 90 | double Kd_; 91 | 92 | public: 93 | 94 | // Standard constructor 95 | Controller2(bool angle, double Kp, double Kd) 96 | { 97 | angle_ = angle; 98 | Kp_ = Kp; 99 | Kd_ = Kd; 100 | } 101 | 102 | void set_coefficients(double Kp, double Kd) 103 | { 104 | Kp_ = Kp; 105 | Kd_ = Kd; 106 | } 107 | 108 | // Run one calculation 109 | double calc(double y_target, double y_actual, double y_dot_target, double y_dot_actual) 110 | { 111 | double y_error = y_target - y_actual; 112 | double y_dot_error = y_dot_target - y_dot_actual; 113 | 114 | if (angle_) { 115 | // Deal with discontinuity 116 | while (y_error < -M_PI) { 117 | y_error += 2 * M_PI; 118 | } 119 | while (y_error > M_PI) { 120 | y_error -= 2 * M_PI; 121 | } 122 | } 123 | 124 | return Kp_ * y_error + Kd_ * y_dot_error; 125 | } 126 | }; 127 | 128 | } // namespace pid 129 | 130 | #endif // PID_H -------------------------------------------------------------------------------- /include/planner_node.hpp: -------------------------------------------------------------------------------- 1 | #ifndef PLANNER_H 2 | #define PLANNER_H 3 | 4 | #include "rclcpp/rclcpp.hpp" 5 | #include "nav_msgs/msg/odometry.hpp" 6 | #include "nav_msgs/msg/path.hpp" 7 | #include "std_msgs/msg/empty.hpp" 8 | 9 | #include "ros2_shared/context_macros.hpp" 10 | 11 | namespace planner_node 12 | { 13 | 14 | //============================================================================= 15 | // DroneInfo 16 | //============================================================================= 17 | 18 | class DroneInfo 19 | { 20 | std::string ns_; 21 | 22 | // Pose for takeoff and landing 23 | bool valid_landing_pose_; 24 | geometry_msgs::msg::PoseStamped landing_pose_; 25 | 26 | // At the moment, odometry is only used to capture the landing pad location 27 | // In the future the plan might be updated based on current drone locations 28 | rclcpp::Subscription::SharedPtr odom_sub_; 29 | 30 | // Publish a plan at 1Hz 31 | rclcpp::Publisher::SharedPtr plan_pub_; 32 | 33 | void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg); 34 | 35 | public: 36 | 37 | explicit DroneInfo(rclcpp::Node *node, std::string ns); 38 | 39 | ~DroneInfo() 40 | {}; 41 | 42 | std::string ns() const 43 | { return ns_; } 44 | 45 | bool valid_landing_pose() const 46 | { return valid_landing_pose_; } 47 | 48 | const geometry_msgs::msg::PoseStamped &landing_pose() const 49 | { return landing_pose_; } 50 | 51 | const rclcpp::Publisher::SharedPtr plan_pub() const 52 | { return plan_pub_; } 53 | }; 54 | 55 | //============================================================================= 56 | // PlannerNode parameters 57 | //============================================================================= 58 | 59 | #define PLANNER_NODE_ALL_PARAMS \ 60 | CXT_MACRO_MEMBER( /* */ \ 61 | arena_x, \ 62 | double, 2.0) \ 63 | CXT_MACRO_MEMBER( /* */ \ 64 | arena_y, \ 65 | double, 2.0) \ 66 | CXT_MACRO_MEMBER( /* */ \ 67 | arena_z, \ 68 | double, 2.0) \ 69 | CXT_MACRO_MEMBER( /* */ \ 70 | drones, \ 71 | std::vector, "solo") \ 72 | /* End of list */ 73 | 74 | struct PlannerNodeContext 75 | { 76 | #undef CXT_MACRO_MEMBER 77 | #define CXT_MACRO_MEMBER(n, t, d) CXT_MACRO_DEFINE_MEMBER(n, t, d) 78 | CXT_MACRO_DEFINE_MEMBERS(PLANNER_NODE_ALL_PARAMS) 79 | }; 80 | 81 | //============================================================================= 82 | // PlannerNode 83 | //============================================================================= 84 | 85 | class PlannerNode : public rclcpp::Node 86 | { 87 | PlannerNodeContext cxt_{}; 88 | 89 | // Per-drone info 90 | std::vector> drones_; 91 | 92 | // Global subscriptions 93 | rclcpp::Subscription::SharedPtr start_mission_sub_; 94 | rclcpp::Subscription::SharedPtr stop_mission_sub_; 95 | 96 | public: 97 | 98 | explicit PlannerNode(); 99 | 100 | ~PlannerNode() 101 | {} 102 | 103 | private: 104 | 105 | void start_mission_callback(const std_msgs::msg::Empty::SharedPtr msg); 106 | 107 | void stop_mission_callback(const std_msgs::msg::Empty::SharedPtr msg); 108 | 109 | void create_and_publish_plans(); 110 | 111 | void validate_parameters(); 112 | }; 113 | 114 | } // namespace planner_node 115 | 116 | #endif // PLANNER_H 117 | -------------------------------------------------------------------------------- /include/simple_planner.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SIMPLE_PLANNER_H 2 | #define SIMPLE_PLANNER_H 3 | 4 | #include "rclcpp/rclcpp.hpp" 5 | #include "nav_msgs/msg/odometry.hpp" 6 | #include "nav_msgs/msg/path.hpp" 7 | #include "std_msgs/msg/empty.hpp" 8 | 9 | namespace simple_planner 10 | { 11 | 12 | class SimplePlanner 13 | { 14 | int num_drones_; 15 | std::vector waypoints_; 16 | 17 | public: 18 | 19 | explicit SimplePlanner(const std::vector &landing_poses); 20 | 21 | ~SimplePlanner() 22 | {} 23 | 24 | std::vector plans(const rclcpp::Time &now) const; 25 | }; 26 | 27 | } // namespace simple_planner 28 | 29 | #endif // SIMPLE_PLANNER_H 30 | -------------------------------------------------------------------------------- /launch/gazebo_launch.py: -------------------------------------------------------------------------------- 1 | """Simulate one or more Tello drones in Gazebo, using ArUco markers and fiducial_vlam for localization""" 2 | 3 | import os 4 | 5 | from ament_index_python.packages import get_package_share_directory 6 | from launch import LaunchDescription 7 | from launch_ros.actions import Node 8 | from launch.actions import ExecuteProcess 9 | 10 | 11 | def generate_launch_description(): 12 | # 1 or more drones: 13 | drones = ['drone1', 'drone2', 'drone3', 'drone4'] 14 | 15 | # Starting locations: 16 | starting_locations = [ 17 | # Face the wall of markers in fiducial.world 18 | ['-2.5', '1.5', '1', '0'], 19 | ['-1.5', '0.5', '1', '0.785'], 20 | ['-0.5', '1.5', '1', '0'], 21 | ['-1.5', '2.5', '1', '-0.785'] 22 | 23 | # Face all 4 directions in f2.world 24 | # ['-2.5', '1.5', '1', '0'], 25 | # ['-1.5', '0.5', '1', '1.57'], 26 | # ['-0.5', '1.5', '1', '3.14'], 27 | # ['-1.5', '2.5', '1', '-1.57'] 28 | ] 29 | 30 | tello_gazebo_path = get_package_share_directory('tello_gazebo') 31 | tello_description_path = get_package_share_directory('tello_description') 32 | 33 | world_path = os.path.join(tello_gazebo_path, 'worlds', 'fiducial.world') 34 | map_path = os.path.join(tello_gazebo_path, 'worlds', 'fiducial_map.yaml') 35 | 36 | # Global entities 37 | entities = [ 38 | # Launch Gazebo, loading tello.world 39 | ExecuteProcess(cmd=[ 40 | 'gazebo', 41 | '--verbose', 42 | '-s', 'libgazebo_ros_init.so', # Publish /clock 43 | '-s', 'libgazebo_ros_factory.so', # Provide gazebo_ros::Node 44 | world_path 45 | ], output='screen'), 46 | 47 | # Load and publish a known map 48 | Node(package='fiducial_vlam', executable='vmap_main', output='screen', 49 | name='vmap_main', parameters=[{ 50 | 'use_sim_time': True, # Use /clock if available 51 | 'publish_tfs': 1, # Publish marker /tf 52 | 'marker_length': 0.1778, # Marker length 53 | 'marker_map_load_full_filename': map_path, # Load a pre-built map from disk 54 | 'make_not_use_map': 0 # Don't save a map to disk 55 | }]), 56 | 57 | # Joystick driver, generates /namespace/joy messages 58 | Node(package='joy', executable='joy_node', output='screen', 59 | name='joy_node', parameters=[{ 60 | 'use_sim_time': True, # Use /clock if available 61 | }]), 62 | 63 | # Flock controller (basically a joystick multiplexer, also starts/stops missions) 64 | Node(package='flock2', executable='flock_base', output='screen', 65 | name='flock_base', parameters=[{ 66 | 'use_sim_time': True, # Use /clock if available 67 | 'drones': drones 68 | }]), 69 | 70 | # WIP: planner 71 | Node(package='flock2', executable='planner_node', output='screen', 72 | name='planner_node', parameters=[{ 73 | 'use_sim_time': True, # Use /clock if available 74 | 'drones': drones, 75 | 'arena_x': -5.0, 76 | 'arena_y': -5.0, 77 | 'arena_z': 10.0, 78 | }]), 79 | ] 80 | 81 | # Per-drone entities 82 | for idx, namespace in enumerate(drones): 83 | suffix = '_' + str(idx + 1) 84 | urdf_path = os.path.join(tello_description_path, 'urdf', 'tello' + suffix + '.urdf') 85 | 86 | entities.extend([ 87 | # Add a drone to the simulation 88 | Node(package='tello_gazebo', executable='inject_entity.py', output='screen', 89 | arguments=[urdf_path]+starting_locations[idx]), 90 | 91 | # Publish base_link to camera_link tf 92 | # Node(package='robot_state_publisher', executable='robot_state_publisher', output='screen', 93 | # name=namespace+'_tf_pub', arguments=[urdf_path], parameters=[{ 94 | # 'use_sim_time': True, # Use /clock if available 95 | # }]), 96 | 97 | # Localize this drone against the map 98 | # Future: need odometry for base_link, not camera_link 99 | Node(package='fiducial_vlam', executable='vloc_main', output='screen', 100 | name='vloc_main', namespace=namespace, parameters=[{ 101 | 'use_sim_time': True, # Use /clock if available 102 | 'publish_tfs': 1, # Publish drone and camera /tf 103 | 'stamp_msgs_with_current_time': 0, # Use incoming message time, not now() 104 | 'base_frame_id': 'base_link' + suffix, 105 | 'map_init_pose_z': -0.035, 106 | 'camera_frame_id': 'camera_link' + suffix, 107 | 'base_odometry_pub_topic': 'base_odom', 108 | 'sub_camera_info_best_effort_not_reliable': 1, # Gazebo camera uses 'best effort' 109 | }]), 110 | 111 | # Drone controller 112 | Node(package='flock2', executable='drone_base', output='screen', 113 | name='drone_base', namespace=namespace, parameters=[{ 114 | 'use_sim_time': True, # Use /clock if available 115 | }]), 116 | ]) 117 | 118 | return LaunchDescription(entities) 119 | -------------------------------------------------------------------------------- /launch/launch_one.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | from launch import LaunchDescription 5 | from launch_ros.actions import Node 6 | from launch.actions import ExecuteProcess 7 | 8 | # Launch a single drone. 9 | 10 | 11 | def generate_launch_description(): 12 | urdf = os.path.join(get_package_share_directory('tello_description'), 'urdf', 'tello.urdf') 13 | return LaunchDescription([ 14 | # Rviz 15 | ExecuteProcess(cmd=['rviz2', '-d', 'install/flock2/share/flock2/launch/one.rviz'], output='screen'), 16 | 17 | # Publish static transforms 18 | Node(package='robot_state_publisher', executable='robot_state_publisher', output='screen', 19 | arguments=[urdf]), 20 | 21 | # Driver 22 | Node(package='tello_driver', executable='tello_driver_main', output='screen', 23 | name='tello_driver', namespace='solo'), 24 | 25 | # Joystick 26 | Node(package='joy', executable='joy_node', output='screen'), 27 | 28 | # Flock controller 29 | Node(package='flock2', executable='flock_base', output='screen'), 30 | 31 | # Drone controller 32 | Node(package='flock2', executable='drone_base', output='screen', 33 | name='drone_base', namespace='solo'), 34 | 35 | # Mapper 36 | Node(package='fiducial_vlam', executable='vmap_main', output='screen'), 37 | 38 | # Visual localizer 39 | Node(package='fiducial_vlam', executable='vloc_main', output='screen', 40 | name='vloc_main', namespace='solo'), 41 | 42 | # WIP: planner 43 | Node(package='flock2', executable='planner_node', output='screen'), 44 | ]) 45 | -------------------------------------------------------------------------------- /launch/launch_two.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | from launch import LaunchDescription 5 | from launch_ros.actions import Node 6 | from launch.actions import ExecuteProcess 7 | 8 | # Launch a flock of drones 9 | 10 | 11 | def generate_launch_description(): 12 | urdf1 = os.path.join(get_package_share_directory('tello_description'), 'urdf', 'tello_1.urdf') 13 | urdf2 = os.path.join(get_package_share_directory('tello_description'), 'urdf', 'tello_2.urdf') 14 | 15 | dr1_ns = 'drone1' 16 | dr2_ns = 'drone2' 17 | 18 | dr1_params = [{ 19 | 'drone_ip': '192.168.86.206', 20 | 'command_port': 11001, 21 | 'drone_port': 12001, 22 | 'data_port': 13001, 23 | 'video_port': 14001 24 | }] 25 | 26 | dr2_params = [{ 27 | 'drone_ip': '192.168.86.212', 28 | 'command_port': 11002, 29 | 'drone_port': 12002, 30 | 'data_port': 13002, 31 | 'video_port': 14002 32 | }] 33 | 34 | base_params = [{ 35 | 'drones': [dr1_ns, dr2_ns] 36 | }] 37 | 38 | filter1_params = [{ 39 | 'map_frame': 'map', 40 | 'base_frame': 'base1' 41 | }] 42 | 43 | filter2_params = [{ 44 | 'map_frame': 'map', 45 | 'base_frame': 'base2' 46 | }] 47 | 48 | return LaunchDescription([ 49 | # Rviz 50 | ExecuteProcess(cmd=['rviz2', '-d', 'install/flock2/share/flock2/launch/two.rviz'], output='screen'), 51 | 52 | # Publish N sets of static transforms 53 | Node(package='robot_state_publisher', executable='robot_state_publisher', output='screen', 54 | arguments=[urdf1]), 55 | Node(package='robot_state_publisher', executable='robot_state_publisher', output='screen', 56 | arguments=[urdf2]), 57 | 58 | # N drivers 59 | Node(package='tello_driver', executable='tello_driver_main', output='screen', 60 | name='driver1', namespace=dr1_ns, parameters=dr1_params), 61 | Node(package='tello_driver', executable='tello_driver_main', output='screen', 62 | name='driver2', namespace=dr2_ns, parameters=dr2_params), 63 | 64 | # Joystick 65 | Node(package='joy', executable='joy_node', output='screen'), 66 | 67 | # Flock controller 68 | Node(package='flock2', executable='flock_base', output='screen', 69 | name='flock_base', parameters=base_params), 70 | 71 | # N drone controllers 72 | Node(package='flock2', executable='drone_base', output='screen', 73 | name='base1', namespace=dr1_ns), 74 | Node(package='flock2', executable='drone_base', output='screen', 75 | name='base2', namespace=dr2_ns), 76 | 77 | # Mapper 78 | Node(package='fiducial_vlam', executable='vmap_main', output='screen'), 79 | 80 | # N visual localizers 81 | Node(package='fiducial_vlam', executable='vloc_main', output='screen', 82 | name='vloc1', namespace=dr1_ns), 83 | Node(package='fiducial_vlam', executable='vloc_main', output='screen', 84 | name='vloc2', namespace=dr2_ns), 85 | ]) 86 | -------------------------------------------------------------------------------- /launch/one.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 128 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /TF1 8 | - /TF1/Frames1 9 | - /TF1/Tree1 10 | - /Path1 11 | - /Odometry1 12 | - /Odometry1/Shape1 13 | Splitter Ratio: 0.40940526127815247 14 | Tree Height: 897 15 | - Class: rviz_common/Selection 16 | Name: Selection 17 | - Class: rviz_common/Tool Properties 18 | Expanded: 19 | - /2D Nav Goal1 20 | - /Publish Point1 21 | Name: Tool Properties 22 | Splitter Ratio: 0.5886790156364441 23 | - Class: rviz_common/Views 24 | Expanded: 25 | - /Current View1 26 | Name: Views 27 | Splitter Ratio: 0.5 28 | Visualization Manager: 29 | Class: "" 30 | Displays: 31 | - Alpha: 0.5 32 | Cell Size: 1 33 | Class: rviz_default_plugins/Grid 34 | Color: 160; 160; 164 35 | Enabled: true 36 | Line Style: 37 | Line Width: 0.029999999329447746 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_default_plugins/RobotModel 51 | Collision Enabled: false 52 | Description File: install/tello_description/share/tello_description/urdf/tello.urdf 53 | Description Source: File 54 | Description Topic: /robot_description 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: Links in Alphabetic Order 62 | base_link: 63 | Alpha: 1 64 | Show Axes: false 65 | Show Trail: false 66 | Value: true 67 | camera_frame: 68 | Alpha: 1 69 | Show Axes: false 70 | Show Trail: false 71 | Value: true 72 | Name: RobotModel 73 | TF Prefix: "" 74 | Unreliable: false 75 | Update Interval: 0 76 | Value: true 77 | Visual Enabled: true 78 | - Class: rviz_default_plugins/Image 79 | Enabled: true 80 | Max Value: 1 81 | Median window: 5 82 | Min Value: 0 83 | Name: Image 84 | Normalize Range: true 85 | Queue Size: 1 86 | Topic: /solo/image_marked 87 | Unreliable: false 88 | Value: true 89 | - Class: rviz_default_plugins/TF 90 | Enabled: true 91 | Frame Timeout: 15 92 | Frames: 93 | All Enabled: true 94 | base_link: 95 | Value: true 96 | camera_frame: 97 | Value: true 98 | map: 99 | Value: true 100 | Marker Scale: 1 101 | Name: TF 102 | Show Arrows: true 103 | Show Axes: true 104 | Show Names: true 105 | Tree: 106 | map: 107 | base_link: 108 | camera_frame: 109 | {} 110 | Update Interval: 0 111 | Value: true 112 | - Class: rviz_default_plugins/MarkerArray 113 | Enabled: true 114 | Name: MarkerArray 115 | Namespaces: 116 | "": true 117 | Queue Size: 10 118 | Topic: /rviz_markers 119 | Unreliable: false 120 | Value: true 121 | - Alpha: 1 122 | Buffer Length: 1 123 | Class: rviz_default_plugins/Path 124 | Color: 25; 255; 0 125 | Enabled: true 126 | Head Diameter: 0.30000001192092896 127 | Head Length: 0.20000000298023224 128 | Length: 0.30000001192092896 129 | Line Style: Lines 130 | Line Width: 0.029999999329447746 131 | Name: Path 132 | Offset: 133 | X: 0 134 | Y: 0 135 | Z: 0 136 | Pose Color: 255; 85; 255 137 | Pose Style: None 138 | Radius: 0.029999999329447746 139 | Shaft Diameter: 0.10000000149011612 140 | Shaft Length: 0.10000000149011612 141 | Topic: /solo/plan 142 | Unreliable: false 143 | Value: true 144 | - Angle Tolerance: 0.10000000149011612 145 | Class: rviz_default_plugins/Odometry 146 | Covariance: 147 | Orientation: 148 | Alpha: 0.5 149 | Color: 255; 255; 127 150 | Color Style: Unique 151 | Frame: Local 152 | Offset: 1 153 | Scale: 1 154 | Value: true 155 | Position: 156 | Alpha: 0.30000001192092896 157 | Color: 204; 51; 204 158 | Scale: 1 159 | Value: true 160 | Value: false 161 | Enabled: true 162 | Keep: 100 163 | Name: Odometry 164 | Position Tolerance: 0.10000000149011612 165 | Shape: 166 | Alpha: 1 167 | Axes Length: 0.10000000149011612 168 | Axes Radius: 0.009999999776482582 169 | Color: 255; 25; 0 170 | Head Length: 0.30000001192092896 171 | Head Radius: 0.10000000149011612 172 | Shaft Length: 1 173 | Shaft Radius: 0.05000000074505806 174 | Value: Axes 175 | Topic: /solo/base_odom 176 | Unreliable: false 177 | Value: true 178 | Enabled: true 179 | Global Options: 180 | Background Color: 48; 48; 48 181 | Fixed Frame: map 182 | Frame Rate: 30 183 | Name: root 184 | Tools: 185 | - Class: rviz_default_plugins/MoveCamera 186 | - Class: rviz_default_plugins/Select 187 | - Class: rviz_default_plugins/FocusCamera 188 | - Class: rviz_default_plugins/Measure 189 | Line color: 128; 128; 0 190 | - Class: rviz_default_plugins/SetGoal 191 | Topic: /move_base_simple/goal 192 | - Class: rviz_default_plugins/PublishPoint 193 | Single click: true 194 | Topic: /clicked_point 195 | Transformation: 196 | Current: 197 | Class: rviz_default_plugins/TF 198 | Value: true 199 | Views: 200 | Current: 201 | Class: rviz_default_plugins/Orbit 202 | Distance: 5.481599807739258 203 | Enable Stereo Rendering: 204 | Stereo Eye Separation: 0.05999999865889549 205 | Stereo Focal Distance: 1 206 | Swap Stereo Eyes: false 207 | Value: false 208 | Focal Point: 209 | X: -0.8537734746932983 210 | Y: -0.01974310912191868 211 | Z: 0.6969380974769592 212 | Focal Shape Fixed Size: true 213 | Focal Shape Size: 0.05000000074505806 214 | Invert Z Axis: false 215 | Name: Current View 216 | Near Clip Distance: 0.009999999776482582 217 | Pitch: 1.129796028137207 218 | Target Frame: 219 | Value: Orbit (rviz) 220 | Yaw: 3.1396431922912598 221 | Saved: ~ 222 | Window Geometry: 223 | Displays: 224 | collapsed: false 225 | Height: 1890 226 | Hide Left Dock: false 227 | Hide Right Dock: false 228 | Image: 229 | collapsed: false 230 | QMainWindow State: 000000ff00000000fd0000000400000000000002d5000006fafc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006e00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc000000470000046c0000011501000023fa000000010100000002fb0000000a005600690065007700730100000000ffffffff0000010200fffffffb000000100044006900730070006c0061007900730100000000000003670000016100fffffffb0000000a0049006d00610067006501000004ba000002870000002f00fffffffb0000000a0049006d00610067006501000004f0000002510000000000000000fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000549000001f800000000000000000000000100000335000006fafc0200000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000058f000006fa00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 231 | Selection: 232 | collapsed: false 233 | Tool Properties: 234 | collapsed: false 235 | Views: 236 | collapsed: false 237 | Width: 2155 238 | X: 1453 239 | Y: 142 240 | -------------------------------------------------------------------------------- /launch/two.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 85 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /TF1/Frames1 8 | - /TF1/Tree1 9 | - /MarkerArray1 10 | Splitter Ratio: 0.40940526127815247 11 | Tree Height: 508 12 | - Class: rviz_common/Selection 13 | Name: Selection 14 | - Class: rviz_common/Tool Properties 15 | Expanded: 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.5886790156364441 20 | - Class: rviz_common/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | Visualization Manager: 26 | Class: "" 27 | Displays: 28 | - Alpha: 0.5 29 | Cell Size: 1 30 | Class: rviz_default_plugins/Grid 31 | Color: 160; 160; 164 32 | Enabled: true 33 | Line Style: 34 | Line Width: 0.029999999329447746 35 | Value: Lines 36 | Name: Grid 37 | Normal Cell Count: 0 38 | Offset: 39 | X: 0 40 | Y: 0 41 | Z: 0 42 | Plane: XY 43 | Plane Cell Count: 10 44 | Reference Frame: 45 | Value: true 46 | - Class: rviz_default_plugins/Image 47 | Enabled: true 48 | Max Value: 1 49 | Median window: 5 50 | Min Value: 0 51 | Name: Image 52 | Normalize Range: true 53 | Queue Size: 1 54 | Topic: /drone1/image_marked 55 | Unreliable: false 56 | Value: true 57 | - Class: rviz_default_plugins/Image 58 | Enabled: true 59 | Max Value: 1 60 | Median window: 5 61 | Min Value: 0 62 | Name: Image 63 | Normalize Range: true 64 | Queue Size: 10 65 | Topic: /drone2/image_marked 66 | Unreliable: false 67 | Value: true 68 | - Class: rviz_default_plugins/TF 69 | Enabled: true 70 | Frame Timeout: 15 71 | Frames: 72 | All Enabled: false 73 | base1: 74 | Value: true 75 | base2: 76 | Value: true 77 | camera1: 78 | Value: true 79 | camera2: 80 | Value: true 81 | map: 82 | Value: true 83 | Marker Scale: 1 84 | Name: TF 85 | Show Arrows: true 86 | Show Axes: true 87 | Show Names: true 88 | Tree: 89 | map: 90 | base1: 91 | camera1: 92 | {} 93 | base2: 94 | camera2: 95 | {} 96 | Update Interval: 0 97 | Value: true 98 | - Class: rviz_default_plugins/MarkerArray 99 | Enabled: true 100 | Name: MarkerArray 101 | Namespaces: 102 | "": true 103 | Queue Size: 10 104 | Topic: /rviz_markers 105 | Unreliable: true 106 | Value: true 107 | - Alpha: 1 108 | Class: rviz_default_plugins/RobotModel 109 | Collision Enabled: false 110 | Description File: install/tello_description/share/tello_description/urdf/drone_1.urdf 111 | Description Source: File 112 | Description Topic: /robot_description 113 | Enabled: true 114 | Links: 115 | All Links Enabled: true 116 | Expand Joint Details: false 117 | Expand Link Details: false 118 | Expand Tree: false 119 | Link Tree Style: Links in Alphabetic Order 120 | base1: 121 | Alpha: 1 122 | Show Axes: false 123 | Show Trail: false 124 | Value: true 125 | camera1: 126 | Alpha: 1 127 | Show Axes: false 128 | Show Trail: false 129 | Value: true 130 | Name: RobotModel 131 | TF Prefix: "" 132 | Unreliable: false 133 | Update Interval: 0 134 | Value: true 135 | Visual Enabled: true 136 | - Alpha: 1 137 | Class: rviz_default_plugins/RobotModel 138 | Collision Enabled: false 139 | Description File: install/tello_description/share/tello_description/urdf/drone_2.urdf 140 | Description Source: File 141 | Description Topic: "" 142 | Enabled: true 143 | Links: 144 | All Links Enabled: true 145 | Expand Joint Details: false 146 | Expand Link Details: false 147 | Expand Tree: false 148 | Link Tree Style: Links in Alphabetic Order 149 | base2: 150 | Alpha: 1 151 | Show Axes: false 152 | Show Trail: false 153 | Value: true 154 | camera2: 155 | Alpha: 1 156 | Show Axes: false 157 | Show Trail: false 158 | Value: true 159 | Name: RobotModel 160 | TF Prefix: "" 161 | Unreliable: false 162 | Update Interval: 0 163 | Value: true 164 | Visual Enabled: true 165 | Enabled: true 166 | Global Options: 167 | Background Color: 48; 48; 48 168 | Fixed Frame: map 169 | Frame Rate: 30 170 | Name: root 171 | Tools: 172 | - Class: rviz_default_plugins/MoveCamera 173 | - Class: rviz_default_plugins/Select 174 | - Class: rviz_default_plugins/FocusCamera 175 | - Class: rviz_default_plugins/Measure 176 | Line color: 128; 128; 0 177 | - Class: rviz_default_plugins/SetGoal 178 | Topic: /move_base_simple/goal 179 | - Class: rviz_default_plugins/PublishPoint 180 | Single click: true 181 | Topic: /clicked_point 182 | Transformation: 183 | Current: 184 | Class: rviz_default_plugins/TF 185 | Value: true 186 | Views: 187 | Current: 188 | Class: rviz_default_plugins/Orbit 189 | Distance: 2.777148962020874 190 | Enable Stereo Rendering: 191 | Stereo Eye Separation: 0.05999999865889549 192 | Stereo Focal Distance: 1 193 | Swap Stereo Eyes: false 194 | Value: false 195 | Focal Point: 196 | X: -0.8537734746932983 197 | Y: -0.01974310912191868 198 | Z: 0.6969380974769592 199 | Focal Shape Fixed Size: true 200 | Focal Shape Size: 0.05000000074505806 201 | Invert Z Axis: false 202 | Name: Current View 203 | Near Clip Distance: 0.009999999776482582 204 | Pitch: 0.40479809045791626 205 | Target Frame: 206 | Value: Orbit (rviz) 207 | Yaw: 3.1746439933776855 208 | Saved: ~ 209 | Window Geometry: 210 | Displays: 211 | collapsed: false 212 | Height: 1890 213 | Hide Left Dock: false 214 | Hide Right Dock: false 215 | Image: 216 | collapsed: false 217 | QMainWindow State: 000000ff00000000fd0000000400000000000002d5000006fafc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006e00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc00000047000002bc0000011501000023fa000000010100000002fb0000000a005600690065007700730100000000ffffffff0000010200fffffffb000000100044006900730070006c0061007900730100000000000003670000016100fffffffb0000000a0049006d006100670065010000030a000001d90000002f00fffffffb0000000a0049006d00610067006501000004ea000002570000002f00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000549000001f800000000000000000000000100000335000006fafc0200000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000058f000006fa00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 218 | Selection: 219 | collapsed: false 220 | Tool Properties: 221 | collapsed: false 222 | Views: 223 | collapsed: false 224 | Width: 2155 225 | X: 1382 226 | Y: 127 227 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | flock2 6 | 0.1.0 7 | Flock for ROS2 8 | 9 | Clyde McQueen 10 | BSD 11 | 12 | https://github.com/clydemcqueen/flock2.git 13 | https://github.com/clydemcqueen/flock2/issues 14 | 15 | Clyde McQueen 16 | Peter Mullen 17 | 18 | ament_cmake 19 | 20 | builtin_interfaces 21 | geometry_msgs 22 | nav_msgs 23 | rclcpp 24 | rclpy 25 | ros2_shared 26 | sensor_msgs 27 | std_msgs 28 | tello_msgs 29 | visualization_msgs 30 | 31 | 32 | ament_cmake 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /src/action_mgr.cpp: -------------------------------------------------------------------------------- 1 | #include "action_mgr.hpp" 2 | 3 | // Future: time out if tello_driver hasn't responded in ~10s, this can happen if tello_driver was restarted 4 | 5 | namespace drone_base 6 | { 7 | 8 | ActionMgr::State ActionMgr::send(Action action, std::string action_str) 9 | { 10 | action_ = action; 11 | action_str_ = action_str; 12 | 13 | RCLCPP_DEBUG(logger_, "send %s to tello_driver", action_str.c_str()); 14 | 15 | auto request = std::make_shared(); 16 | request->cmd = action_str; 17 | future_ = client_->async_send_request(request); 18 | 19 | state_ = ActionMgr::State::waiting_for_future; 20 | return state_; 21 | } 22 | 23 | ActionMgr::State ActionMgr::spin_once() 24 | { 25 | if (state_ == ActionMgr::State::waiting_for_future && 26 | future_.wait_for(std::chrono::seconds(0)) == std::future_status::ready) { 27 | 28 | // Get the initial response 29 | tello_msgs::srv::TelloAction::Response::SharedPtr response = future_.get(); 30 | 31 | if (response->rc == response->OK) { 32 | RCLCPP_DEBUG(logger_, "%s accepted", action_str_.c_str()); 33 | state_ = ActionMgr::State::waiting_for_response; 34 | 35 | } else if (response->rc == response->ERROR_BUSY) { 36 | RCLCPP_ERROR(logger_, "%s failed, drone is busy", action_str_.c_str()); 37 | result_str_ = "drone is busy"; 38 | state_ = ActionMgr::State::failed; 39 | 40 | } else if (response->rc == response->ERROR_NOT_CONNECTED) { 41 | RCLCPP_ERROR(logger_, "%s failed, lost connection", action_str_.c_str()); 42 | result_str_ = "lost connection"; 43 | state_ = ActionMgr::State::failed_lost_connection; 44 | } 45 | } 46 | 47 | return state_; 48 | } 49 | 50 | ActionMgr::State ActionMgr::complete(tello_msgs::msg::TelloResponse::SharedPtr msg) 51 | { 52 | // The tello_response message may arrive before the future is ready -- that's OK 53 | if (!busy()) { 54 | RCLCPP_ERROR(logger_, "unexpected response %s", msg->str.c_str()); 55 | result_str_ = "unexpected response"; 56 | state_ = ActionMgr::State::failed; 57 | 58 | } else if (msg->rc == msg->OK) { 59 | RCLCPP_DEBUG(logger_, "%s succeeded with %s", action_str_.c_str(), msg->str.c_str()); 60 | result_str_ = msg->str; 61 | state_ = ActionMgr::State::succeeded; 62 | 63 | } else if (msg->rc == msg->ERROR) { 64 | RCLCPP_ERROR(logger_, "%s failed with %s", action_str_.c_str(), msg->str.c_str()); 65 | result_str_ = msg->str; 66 | state_ = ActionMgr::State::failed; 67 | 68 | } else if (msg->rc == msg->TIMEOUT) { 69 | RCLCPP_ERROR(logger_, "%s failed, drone timed out", msg->str.c_str()); 70 | result_str_ = "drone timed out"; 71 | state_ = ActionMgr::State::failed; 72 | } 73 | 74 | return state_; 75 | } 76 | 77 | } // namespace drone_base -------------------------------------------------------------------------------- /src/drone_base.cpp: -------------------------------------------------------------------------------- 1 | #include "drone_base.hpp" 2 | 3 | #include 4 | 5 | #include "flight_controller_basic.hpp" 6 | #include "flight_controller_simple.hpp" 7 | 8 | namespace drone_base 9 | { 10 | 11 | //============================================================================= 12 | // Constants 13 | //============================================================================= 14 | 15 | const int SPIN_RATE = 20; 16 | 17 | const int MIN_BATTERY{20}; // Percent 18 | 19 | //============================================================================= 20 | // Utilities 21 | //============================================================================= 22 | 23 | bool button_down(const sensor_msgs::msg::Joy::SharedPtr &curr, const sensor_msgs::msg::Joy &prev, int index) 24 | { 25 | return curr->buttons[index] && !prev.buttons[index]; 26 | } 27 | 28 | //============================================================================= 29 | // States, events and actions 30 | //============================================================================= 31 | 32 | std::map g_states{ 33 | {State::unknown, "unknown"}, 34 | {State::ready, "ready"}, 35 | {State::flight, "flight"}, 36 | {State::ready_odom, "ready_odom"}, 37 | {State::flight_odom, "flight_odom"}, 38 | {State::low_battery, "low_battery"}, 39 | }; 40 | 41 | std::map g_events{ 42 | {Event::connected, "connected"}, 43 | {Event::disconnected, "disconnected"}, 44 | {Event::odometry_started, "odometry_started"}, 45 | {Event::odometry_stopped, "odometry_stopped"}, 46 | {Event::low_battery, "low_battery"}, 47 | }; 48 | 49 | std::map g_actions{ 50 | {Action::takeoff, "takeoff"}, 51 | {Action::land, "land"}, 52 | }; 53 | 54 | struct EventTransition 55 | { 56 | State curr_state_; 57 | Event event_; 58 | State next_state_; 59 | 60 | EventTransition(State curr_state, Event event, State next_state) : 61 | curr_state_{curr_state}, event_{event}, next_state_{next_state} 62 | {} 63 | }; 64 | 65 | struct ActionTransition 66 | { 67 | State curr_state_; 68 | Action action_; 69 | State next_state_; 70 | 71 | ActionTransition(State curr_state, Action action, State next_state) : 72 | curr_state_{curr_state}, action_{action}, next_state_{next_state} 73 | {} 74 | }; 75 | 76 | bool valid_event_transition(const State state, const Event event, State &next_state) 77 | { 78 | const static std::vector valid_transitions{ 79 | EventTransition{State::unknown, Event::connected, State::ready}, 80 | 81 | EventTransition{State::ready, Event::disconnected, State::unknown}, 82 | EventTransition{State::ready, Event::odometry_started, State::ready_odom}, 83 | EventTransition{State::ready, Event::low_battery, State::low_battery}, 84 | 85 | EventTransition{State::flight, Event::disconnected, State::unknown}, 86 | EventTransition{State::flight, Event::odometry_started, State::flight_odom}, 87 | EventTransition{State::flight, Event::low_battery, State::low_battery}, 88 | 89 | EventTransition{State::ready_odom, Event::disconnected, State::unknown}, 90 | EventTransition{State::ready_odom, Event::odometry_stopped, State::ready}, 91 | EventTransition{State::ready_odom, Event::low_battery, State::low_battery}, 92 | 93 | EventTransition{State::flight_odom, Event::disconnected, State::unknown}, 94 | EventTransition{State::flight_odom, Event::odometry_stopped, State::flight}, 95 | EventTransition{State::flight_odom, Event::low_battery, State::low_battery}, 96 | 97 | EventTransition{State::low_battery, Event::disconnected, State::unknown}, 98 | }; 99 | 100 | for (auto i = valid_transitions.begin(); i != valid_transitions.end(); i++) { 101 | if (i->curr_state_ == state && i->event_ == event) { 102 | next_state = i->next_state_; 103 | return true; 104 | } 105 | } 106 | 107 | return false; 108 | } 109 | 110 | bool valid_action_transition(const State state, const Action action, State &next_state) 111 | { 112 | // Allow for emergency landing in all states 113 | const static std::vector valid_transitions{ 114 | ActionTransition{State::unknown, Action::land, State::unknown}, 115 | 116 | ActionTransition{State::ready, Action::takeoff, State::flight}, 117 | ActionTransition{State::ready, Action::land, State::ready}, 118 | 119 | ActionTransition{State::flight, Action::land, State::ready}, 120 | 121 | ActionTransition{State::ready_odom, Action::takeoff, State::flight_odom}, 122 | ActionTransition{State::ready_odom, Action::land, State::ready_odom}, 123 | 124 | ActionTransition{State::flight_odom, Action::land, State::ready_odom}, 125 | 126 | ActionTransition{State::low_battery, Action::land, State::low_battery}, 127 | }; 128 | 129 | for (auto i = valid_transitions.begin(); i != valid_transitions.end(); i++) { 130 | if (i->curr_state_ == state && i->action_ == action) { 131 | next_state = i->next_state_; 132 | return true; 133 | } 134 | } 135 | 136 | return false; 137 | } 138 | 139 | //============================================================================= 140 | // DroneBase node 141 | //============================================================================= 142 | 143 | DroneBase::DroneBase() : Node{"drone_base"} 144 | { 145 | // Suppress CLion warnings 146 | (void) cmd_vel_pub_; 147 | (void) start_mission_sub_; 148 | (void) stop_mission_sub_; 149 | (void) joy_sub_; 150 | (void) tello_response_sub_; 151 | (void) flight_data_sub_; 152 | (void) odom_sub_; 153 | (void) plan_sub_; 154 | 155 | #undef CXT_MACRO_MEMBER 156 | #define CXT_MACRO_MEMBER(n, t, d) CXT_MACRO_LOAD_PARAMETER((*this), cxt_, n, t, d) 157 | CXT_MACRO_INIT_PARAMETERS(DRONE_BASE_ALL_PARAMS, validate_parameters) 158 | 159 | #undef CXT_MACRO_MEMBER 160 | #define CXT_MACRO_MEMBER(n, t, d) CXT_MACRO_PARAMETER_CHANGED(n, t) 161 | CXT_MACRO_REGISTER_PARAMETERS_CHANGED((*this), cxt_, DRONE_BASE_ALL_PARAMS, validate_parameters) 162 | 163 | fc_ = std::make_unique(*this, cmd_vel_pub_); 164 | // fc_ = std::make_unique(*this, cmd_vel_pub_); 165 | 166 | action_mgr_ = std::make_unique(get_logger(), 167 | create_client("tello_action")); 168 | 169 | cmd_vel_pub_ = create_publisher("cmd_vel", 1); 170 | 171 | using std::placeholders::_1; 172 | auto joy_cb = std::bind(&DroneBase::joy_callback, this, _1); 173 | auto start_mission_cb = std::bind(&DroneBase::start_mission_callback, this, _1); 174 | auto stop_mission_cb = std::bind(&DroneBase::stop_mission_callback, this, _1); 175 | auto tello_response_cb = std::bind(&DroneBase::tello_response_callback, this, _1); 176 | auto flight_data_cb = std::bind(&DroneBase::flight_data_callback, this, _1); 177 | auto odom_cb = std::bind(&DroneBase::odom_callback, this, _1); 178 | auto plan_cb = std::bind(&DroneBase::plan_callback, this, _1); 179 | 180 | start_mission_sub_ = create_subscription("/start_mission", 10, start_mission_cb); 181 | stop_mission_sub_ = create_subscription("/stop_mission", 10, stop_mission_cb); 182 | joy_sub_ = create_subscription("joy", 10, joy_cb); 183 | tello_response_sub_ = create_subscription("tello_response", 10, tello_response_cb); 184 | flight_data_sub_ = create_subscription("flight_data", 10, flight_data_cb); 185 | odom_sub_ = create_subscription("base_odom", 10, odom_cb); 186 | plan_sub_ = create_subscription("plan", 10, plan_cb); 187 | 188 | RCLCPP_INFO(get_logger(), "drone initialized"); 189 | } 190 | 191 | void DroneBase::spin_once() 192 | { 193 | rclcpp::Time ros_time = now(); 194 | 195 | // Check for flight data timeout 196 | if (PoseUtil::is_valid_time(flight_data_time_) && ros_time - flight_data_time_ > cxt_.flight_data_timeout_) { 197 | RCLCPP_ERROR(get_logger(), "flight data timeout, now %ld, last %ld", 198 | RCL_NS_TO_MS(ros_time.nanoseconds()), RCL_NS_TO_MS(flight_data_time_.nanoseconds())); 199 | transition_state(Event::disconnected); 200 | flight_data_time_ = rclcpp::Time(); // Zero time is invalid 201 | odom_time_ = rclcpp::Time(); 202 | } 203 | 204 | // Check for odometry timeout 205 | if (PoseUtil::is_valid_time(odom_time_) && ros_time - odom_time_ > cxt_.odom_timeout_) { 206 | RCLCPP_ERROR(get_logger(), "odom timeout, now %ld, last %ld", 207 | RCL_NS_TO_MS(ros_time.nanoseconds()), RCL_NS_TO_MS(odom_time_.nanoseconds())); 208 | transition_state(Event::odometry_stopped); 209 | odom_time_ = rclcpp::Time(); 210 | } 211 | 212 | // Process any actions 213 | action_mgr_->spin_once(); 214 | 215 | // Automated flight 216 | if (mission_ && fc_->have_plan()) { 217 | // We have a plan 218 | if (!fc_->is_plan_complete()) { 219 | // There's more to do 220 | if (state_ == State::ready_odom) { 221 | if (!action_mgr_->busy()) { 222 | RCLCPP_INFO(get_logger(), "start mission, taking off"); 223 | start_action(Action::takeoff); 224 | } 225 | } else if (state_ == State::flight) { 226 | // Future: try to recover 227 | RCLCPP_ERROR(get_logger(), "lost odometry during mission"); 228 | stop_mission(); 229 | } 230 | } else { 231 | // We're done 232 | if (state_ == State::flight || state_ == State::flight_odom) { 233 | RCLCPP_INFO(get_logger(), "mission complete"); 234 | stop_mission(); 235 | } 236 | } 237 | } 238 | } 239 | 240 | void DroneBase::validate_parameters() 241 | { 242 | cxt_.flight_data_timeout_ = rclcpp::Duration(static_cast(RCL_S_TO_NS(cxt_.flight_data_timeout_sec_))); 243 | cxt_.odom_timeout_ = rclcpp::Duration(static_cast(RCL_S_TO_NS(cxt_.odom_timeout_sec_))); 244 | 245 | RCLCPP_INFO(get_logger(), "DroneBase Parameters"); 246 | 247 | #undef CXT_MACRO_MEMBER 248 | #define CXT_MACRO_MEMBER(n, t, d) CXT_MACRO_LOG_PARAMETER(RCLCPP_INFO, get_logger(), cxt_, n, t, d) 249 | DRONE_BASE_ALL_PARAMS 250 | } 251 | 252 | void DroneBase::start_mission_callback(std_msgs::msg::Empty::SharedPtr msg) 253 | { 254 | (void) msg; 255 | RCLCPP_INFO(get_logger(), "start mission"); 256 | mission_ = true; 257 | } 258 | 259 | void DroneBase::stop_mission_callback(std_msgs::msg::Empty::SharedPtr msg) 260 | { 261 | (void) msg; 262 | RCLCPP_INFO(get_logger(), "stop mission"); 263 | stop_mission(); 264 | } 265 | 266 | void DroneBase::stop_mission() 267 | { 268 | mission_ = false; 269 | all_stop(); 270 | if (state_ == State::flight || state_ == State::flight_odom) { 271 | // Future: queue action if busy 272 | start_action(Action::land); 273 | } 274 | } 275 | 276 | void DroneBase::joy_callback(sensor_msgs::msg::Joy::SharedPtr msg) 277 | { 278 | static sensor_msgs::msg::Joy prev_msg; 279 | 280 | // Ignore the joystick if we're in a mission 281 | if (mission_) { 282 | prev_msg = *msg; 283 | return; 284 | } 285 | 286 | // Takeoff/land 287 | if (button_down(msg, prev_msg, joy_button_takeoff_)) { 288 | start_action(Action::takeoff); 289 | } else if (button_down(msg, prev_msg, joy_button_land_)) { 290 | start_action(Action::land); 291 | } 292 | 293 | // Manual flight 294 | if ((state_ == State::flight || state_ == State::flight_odom) && !action_mgr_->busy()) { 295 | // Trim (slow, steady) mode vs. joystick mode 296 | if (msg->axes[joy_axis_trim_lr_] != 0. || msg->axes[joy_axis_trim_fb_] != 0.) { 297 | const static double TRIM_SPEED{0.2}; 298 | double throttle{0}, strafe{0}, vertical{0}, yaw{0}; 299 | if (msg->axes[joy_axis_trim_lr_] != 0.) { 300 | if (msg->buttons[joy_button_shift_]) { 301 | yaw = TRIM_SPEED * msg->axes[joy_axis_trim_lr_]; 302 | } else { 303 | strafe = TRIM_SPEED * msg->axes[joy_axis_trim_lr_]; 304 | } 305 | } 306 | if (msg->axes[joy_axis_trim_fb_] != 0.) { 307 | if (msg->buttons[joy_button_shift_]) { 308 | throttle = TRIM_SPEED * msg->axes[joy_axis_trim_fb_]; 309 | } else { 310 | vertical = TRIM_SPEED * msg->axes[joy_axis_trim_fb_]; 311 | } 312 | } 313 | fc_->publish_velocity(throttle, strafe, vertical, yaw); 314 | } else { 315 | fc_->publish_velocity( 316 | msg->axes[joy_axis_throttle_], 317 | msg->axes[joy_axis_strafe_], 318 | msg->axes[joy_axis_vertical_], 319 | msg->axes[joy_axis_yaw_]); 320 | } 321 | } 322 | 323 | prev_msg = *msg; 324 | } 325 | 326 | void DroneBase::tello_response_callback(tello_msgs::msg::TelloResponse::SharedPtr msg) 327 | { 328 | ActionMgr::State result = action_mgr_->complete(std::move(msg)); 329 | if (result == ActionMgr::State::succeeded) { 330 | transition_state(action_mgr_->action()); 331 | } 332 | } 333 | 334 | void DroneBase::flight_data_callback(tello_msgs::msg::FlightData::SharedPtr msg) 335 | { 336 | if (!PoseUtil::is_valid_time(flight_data_time_)) { 337 | transition_state(Event::connected); 338 | } 339 | 340 | if (msg->bat < MIN_BATTERY && state_ != State::low_battery) { 341 | RCLCPP_ERROR(get_logger(), "low battery (%d)", msg->bat); 342 | transition_state(Event::low_battery); 343 | if (mission_) { 344 | stop_mission(); 345 | } 346 | } 347 | 348 | flight_data_time_ = msg->header.stamp; 349 | } 350 | 351 | void DroneBase::odom_callback(nav_msgs::msg::Odometry::SharedPtr msg) 352 | { 353 | // It's possible (but unlikely) to get an odom message before flight data 354 | if (PoseUtil::is_valid_time(flight_data_time_)) { 355 | if (!PoseUtil::is_valid_time(odom_time_)) { 356 | transition_state(Event::odometry_started); 357 | } else { 358 | // Automated flight 359 | if (mission_ && fc_->have_plan() && !fc_->is_plan_complete() && !action_mgr_->busy()) { 360 | if (fc_->odom_callback(msg)) { 361 | RCLCPP_ERROR(get_logger(), "didn't reach target"); 362 | stop_mission(); 363 | } 364 | } 365 | } 366 | 367 | odom_time_ = rclcpp::Time(msg->header.stamp); 368 | } 369 | } 370 | 371 | void DroneBase::plan_callback(nav_msgs::msg::Path::SharedPtr msg) 372 | { 373 | if (mission_) { 374 | RCLCPP_INFO(get_logger(), "Got plan with %d waypoints, plan msg time %ld, last odom time %ld, ros time %ld ", 375 | msg->poses.size(), RCL_NS_TO_MS(rclcpp::Time(msg->header.stamp).nanoseconds()), 376 | RCL_NS_TO_MS(odom_time_.nanoseconds()), RCL_NS_TO_MS(now().nanoseconds())); 377 | fc_->set_plan(msg); 378 | } 379 | } 380 | 381 | void DroneBase::start_action(Action action) 382 | { 383 | if (action_mgr_->busy()) { 384 | RCLCPP_INFO(get_logger(), "busy, dropping %s", g_actions[action]); 385 | return; 386 | } 387 | 388 | State next_state; 389 | if (!valid_action_transition(state_, action, next_state)) { 390 | RCLCPP_DEBUG(get_logger(), "%s not allowed in %s", g_actions[action], g_states[state_]); 391 | return; 392 | } 393 | 394 | RCLCPP_INFO(get_logger(), "in state '%s', initiating action '%s'", g_states[state_], g_actions[action]); 395 | action_mgr_->send(action, g_actions[action]); 396 | } 397 | 398 | void DroneBase::transition_state(Action action) 399 | { 400 | State next_state; 401 | if (!valid_action_transition(state_, action, next_state)) { 402 | RCLCPP_DEBUG(get_logger(), "%s not allowed in %s", g_actions[action], g_states[state_]); 403 | return; 404 | } 405 | 406 | transition_state(next_state); 407 | } 408 | 409 | void DroneBase::transition_state(Event event) 410 | { 411 | State next_state; 412 | if (!valid_event_transition(state_, event, next_state)) { 413 | RCLCPP_DEBUG(get_logger(), "%s not allowed in %s", g_events[event], g_states[state_]); 414 | return; 415 | } 416 | 417 | transition_state(next_state); 418 | } 419 | 420 | void DroneBase::transition_state(State next_state) 421 | { 422 | if (state_ != next_state) { 423 | RCLCPP_INFO(get_logger(), "transition from '%s' to '%s'", g_states[state_], g_states[next_state]); 424 | state_ = next_state; 425 | } 426 | } 427 | 428 | void DroneBase::all_stop() 429 | { 430 | RCLCPP_DEBUG(get_logger(), "ALL STOP"); 431 | fc_->publish_velocity(0, 0, 0, 0); 432 | } 433 | 434 | } // namespace drone_base 435 | 436 | //============================================================================= 437 | // main 438 | //============================================================================= 439 | 440 | int main(int argc, char **argv) 441 | { 442 | // Force flush of the stdout buffer 443 | setvbuf(stdout, nullptr, _IONBF, BUFSIZ); 444 | 445 | // Init ROS 446 | rclcpp::init(argc, argv); 447 | 448 | // Create node 449 | auto node = std::make_shared(); 450 | //auto result = rcutils_logging_set_logger_level(node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_INFO); 451 | 452 | // rclcpp::Rate uses std::chrono::system_clock, so doesn't honor use_sim_time 453 | rclcpp::Rate r(drone_base::SPIN_RATE); 454 | while (rclcpp::ok()) { 455 | // Do our work 456 | node->spin_once(); 457 | 458 | // Respond to incoming messages 459 | rclcpp::spin_some(node); 460 | 461 | // Wait 462 | r.sleep(); 463 | } 464 | 465 | // Shut down ROS 466 | rclcpp::shutdown(); 467 | 468 | return 0; 469 | } 470 | -------------------------------------------------------------------------------- /src/flight_controller_basic.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "flight_controller_basic.hpp" 3 | 4 | namespace drone_base 5 | { 6 | 7 | //============================================================================= 8 | // FlightControllerBasic 9 | //============================================================================= 10 | 11 | FlightControllerBasic::FlightControllerBasic(rclcpp::Node &node, 12 | rclcpp::Publisher::SharedPtr &cmd_vel_pub) : 13 | FlightControllerInterface(node, cmd_vel_pub) 14 | { 15 | #undef CXT_MACRO_MEMBER 16 | #define CXT_MACRO_MEMBER(n, t, d) CXT_MACRO_LOAD_PARAMETER(node_, (*this), n, t, d) 17 | CXT_MACRO_INIT_PARAMETERS(BASIC_CONTROLLER_ALL_PARAMS, validate_parameters) 18 | 19 | #undef CXT_MACRO_MEMBER 20 | #define CXT_MACRO_MEMBER(n, t, d) CXT_MACRO_PARAMETER_CHANGED(n, t) 21 | CXT_MACRO_REGISTER_PARAMETERS_CHANGED(node, (*this), BASIC_CONTROLLER_ALL_PARAMS, validate_parameters) 22 | 23 | _reset(); 24 | } 25 | 26 | void FlightControllerBasic::validate_parameters() 27 | { 28 | stabilize_time_ = rclcpp::Duration(static_cast(RCL_S_TO_NS(stabilize_time_sec_))); 29 | 30 | RCLCPP_INFO(node_.get_logger(), "FlightControllerSimple Parameters"); 31 | 32 | #undef CXT_MACRO_MEMBER 33 | #define CXT_MACRO_MEMBER(n, t, d) CXT_MACRO_LOG_PARAMETER(RCLCPP_INFO, node_.get_logger(), (*this), n, t, d) 34 | BASIC_CONTROLLER_ALL_PARAMS 35 | } 36 | 37 | void FlightControllerBasic::_reset() 38 | { 39 | last_odom_time_ = rclcpp::Time(); 40 | } 41 | 42 | void FlightControllerBasic::_set_target(int target) 43 | { 44 | target_ = target; 45 | 46 | // Handle "done" case 47 | if (target_ < 0 || target_ >= plan_.poses.size()) { 48 | return; 49 | } 50 | 51 | // Set current target 52 | curr_target_.fromMsg(plan_.poses[target_].pose); 53 | curr_target_time_ = rclcpp::Time(plan_.poses[target_].header.stamp) - stabilize_time_; 54 | 55 | RCLCPP_INFO(node_.get_logger(), "target %d position: (%g, %g, %g), yaw %g, time %12ld", 56 | target_, 57 | curr_target_.x, 58 | curr_target_.y, 59 | curr_target_.z, 60 | curr_target_.yaw, 61 | RCL_NS_TO_MS(curr_target_time_.nanoseconds())); 62 | 63 | // Set previous target, as well as velocity 64 | if (target_ == 0) { 65 | // Takeoff case 66 | prev_target_ = curr_target_; 67 | prev_target_time_ = node_.now(); 68 | vx_ = vy_ = vz_ = vyaw_ = 0; 69 | } else { 70 | // Typical case 71 | prev_target_.fromMsg(plan_.poses[target_ - 1].pose); 72 | prev_target_time_ = rclcpp::Time(plan_.poses[target_ - 1].header.stamp); 73 | 74 | auto flight_time = (curr_target_time_ - prev_target_time_).seconds(); 75 | assert(flight_time > 0); 76 | 77 | // Velocity vector from previous target to this target 78 | vx_ = (curr_target_.x - prev_target_.x) / flight_time; 79 | vy_ = (curr_target_.y - prev_target_.y) / flight_time; 80 | vz_ = (curr_target_.z - prev_target_.z) / flight_time; 81 | vyaw_ = PoseUtil::norm_angle(curr_target_.yaw - prev_target_.yaw) / flight_time; 82 | 83 | RCLCPP_INFO(node_.get_logger(), "target %d velocity: (%g, %g, %g), yaw %g", target_, vx_, vy_, vz_, vyaw_); 84 | } 85 | 86 | // Initialize PID controllers to previous target, these will be updated in the odom callback 87 | x_controller_.set_target(prev_target_.x); 88 | y_controller_.set_target(prev_target_.y); 89 | z_controller_.set_target(prev_target_.z); 90 | yaw_controller_.set_target(prev_target_.yaw); 91 | } 92 | 93 | bool FlightControllerBasic::_odom_callback(const nav_msgs::msg::Odometry::SharedPtr &msg) 94 | { 95 | bool retVal = false; 96 | 97 | rclcpp::Time msg_time(msg->header.stamp); 98 | 99 | if (PoseUtil::is_valid_time(last_odom_time_)) { 100 | if (msg_time > curr_target_time_ + stabilize_time_) { 101 | if (curr_target_.close_enough(last_pose_)) { 102 | // Advance to the next target 103 | set_target(target_ + 1); 104 | } else { 105 | // Timeout 106 | retVal = true; 107 | } 108 | } else { 109 | // Compute expected position and set PID targets 110 | // The odom pipeline has a lag, so ignore messages that are older than prev_target_time_ 111 | if (msg_time < curr_target_time_ && msg_time > prev_target_time_) { 112 | auto elapsed_time = (msg_time - prev_target_time_).seconds(); 113 | x_controller_.set_target(prev_target_.x + vx_ * elapsed_time); 114 | y_controller_.set_target(prev_target_.y + vy_ * elapsed_time); 115 | z_controller_.set_target(prev_target_.z + vz_ * elapsed_time); 116 | yaw_controller_.set_target(PoseUtil::norm_angle(prev_target_.yaw + vyaw_ * elapsed_time)); 117 | } 118 | 119 | // Compute velocity 120 | auto dt = (msg_time - last_odom_time_).seconds(); 121 | double ubar_x = x_controller_.calc(last_pose_.x, dt, 0); 122 | double ubar_y = y_controller_.calc(last_pose_.y, dt, 0); 123 | double ubar_z = z_controller_.calc(last_pose_.z, dt, 0); 124 | double ubar_yaw = yaw_controller_.calc(last_pose_.yaw, dt, 0); 125 | 126 | // Rotate ubar_x and ubar_y into the body frame 127 | double throttle, strafe; 128 | PoseUtil::rotate_frame(ubar_x, ubar_y, last_pose_.yaw, throttle, strafe); 129 | 130 | // RCLCPP_INFO(node_.get_logger(), "%12ld " 131 | // "x controller: target %7.3f, curr %7.3f, throttle %7.3f " 132 | // "y controller: target %7.3f, curr %7.3f, strafe %7.3f", 133 | // RCL_NS_TO_MS(msg_time.nanoseconds()), 134 | // x_controller_.target(), last_pose_.x, throttle, 135 | // y_controller_.target(), last_pose_.y, strafe); 136 | 137 | // Publish velocity 138 | publish_velocity(throttle, strafe, ubar_z, ubar_yaw); 139 | } 140 | } 141 | 142 | last_odom_time_ = msg_time; 143 | last_pose_.fromMsg(msg->pose.pose); 144 | return retVal; 145 | } 146 | 147 | 148 | } -------------------------------------------------------------------------------- /src/flight_controller_simple.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "flight_controller_simple.hpp" 3 | 4 | namespace drone_base 5 | { 6 | FlightControllerSimple::FlightControllerSimple(rclcpp::Node &node, 7 | rclcpp::Publisher::SharedPtr &cmd_vel_pub) : 8 | FlightControllerInterface(node, cmd_vel_pub) 9 | { 10 | #undef CXT_MACRO_MEMBER 11 | #define CXT_MACRO_MEMBER(n, t, d) CXT_MACRO_LOAD_PARAMETER(node_, (*this), n, t, d) 12 | CXT_MACRO_INIT_PARAMETERS(SIMPLE_CONTROLLER_ALL_PARAMS, validate_parameters) 13 | 14 | #undef CXT_MACRO_MEMBER 15 | #define CXT_MACRO_MEMBER(n, t, d) CXT_MACRO_PARAMETER_CHANGED(n, t) 16 | CXT_MACRO_REGISTER_PARAMETERS_CHANGED(node, (*this), SIMPLE_CONTROLLER_ALL_PARAMS, validate_parameters) 17 | 18 | _reset(); 19 | } 20 | 21 | void FlightControllerSimple::validate_parameters() 22 | { 23 | stabilize_time_ = rclcpp::Duration(static_cast(RCL_S_TO_NS(stabilize_time_sec_))); 24 | x_controller_.set_coefficients(pid_x_kp_, pid_x_kd_); 25 | y_controller_.set_coefficients(pid_y_kp_, pid_y_kd_); 26 | z_controller_.set_coefficients(pid_z_kp_, pid_z_kd_); 27 | yaw_controller_.set_coefficients(pid_yaw_kp_, pid_yaw_kd_); 28 | 29 | RCLCPP_INFO(node_.get_logger(), "FlightControllerSimple Parameters"); 30 | 31 | #undef CXT_MACRO_MEMBER 32 | #define CXT_MACRO_MEMBER(n, t, d) CXT_MACRO_LOG_PARAMETER(RCLCPP_INFO, node_.get_logger(), (*this), n, t, d) 33 | SIMPLE_CONTROLLER_ALL_PARAMS 34 | } 35 | 36 | void FlightControllerSimple::_reset() 37 | { 38 | last_odom_time_ = rclcpp::Time(); 39 | } 40 | 41 | void FlightControllerSimple::_set_target(int target) 42 | { 43 | target_ = target; 44 | 45 | // Handle "done" case 46 | if (target_ < 0 || target_ >= plan_.poses.size()) { 47 | return; 48 | } 49 | 50 | // Set current target 51 | curr_target_.fromMsg(plan_.poses[target_].pose); 52 | curr_target_time_ = rclcpp::Time(plan_.poses[target_].header.stamp); 53 | 54 | RCLCPP_INFO(node_.get_logger(), "target %d position: (%g, %g, %g), yaw %g, time %12ld", 55 | target_, 56 | curr_target_.x, 57 | curr_target_.y, 58 | curr_target_.z, 59 | curr_target_.yaw, 60 | RCL_NS_TO_MS(curr_target_time_.nanoseconds())); 61 | } 62 | 63 | bool FlightControllerSimple::_odom_callback(const nav_msgs::msg::Odometry::SharedPtr &msg) 64 | { 65 | bool retVal = false; 66 | 67 | rclcpp::Time msg_time(msg->header.stamp); 68 | DronePose pose; 69 | pose.fromMsg(msg->pose.pose); 70 | 71 | if (PoseUtil::is_valid_time(last_odom_time_) && msg_time > last_odom_time_) { 72 | 73 | // Check if we have reached the target or exceeded the stabilize time. 74 | if (msg_time > curr_target_time_) { 75 | 76 | // For now ignore yaw and z. 77 | DronePose test_pose{pose}; 78 | test_pose.yaw = curr_target_.yaw; 79 | test_pose.z = curr_target_.z; 80 | 81 | if (curr_target_.close_enough(test_pose, close_enough_xyz_, close_enough_yaw_)) { 82 | // Advance to the next target 83 | set_target(target_ + 1); 84 | } else if (msg_time > curr_target_time_ + stabilize_time_) { 85 | // Timeout 86 | retVal = true; 87 | } 88 | } 89 | 90 | // Send move command as generated by the controller. 91 | if (!retVal) { 92 | auto dt = (msg_time - last_odom_time_).seconds(); 93 | auto x_dot_actual = (pose.x - last_pose_.x) / dt; 94 | auto y_dot_actual = (pose.y - last_pose_.y) / dt; 95 | auto z_dot_actual = (pose.z - last_pose_.z) / dt; 96 | auto yaw_dot_actual = (pose.yaw - last_pose_.yaw) / dt; 97 | double ubar_x = x_controller_.calc(curr_target_.x, pose.x, 0., x_dot_actual); 98 | double ubar_y = y_controller_.calc(curr_target_.y, pose.y, 0., y_dot_actual); 99 | double ubar_z = z_controller_.calc(curr_target_.z, pose.z, 0., z_dot_actual); 100 | double ubar_yaw = yaw_controller_.calc(curr_target_.yaw, pose.yaw, 0., yaw_dot_actual); 101 | 102 | // Rotate ubar_x and ubar_y into the body frame 103 | double throttle, strafe; 104 | PoseUtil::rotate_frame(ubar_x, ubar_y, pose.yaw, throttle, strafe); 105 | 106 | RCLCPP_INFO(node_.get_logger(), "%12ld " 107 | "x: targ %7.3f, curr %7.3f, throttle %7.3f " 108 | "y: targ %7.3f, curr %7.3f, strafe %7.3f", 109 | RCL_NS_TO_MS(msg_time.nanoseconds()), 110 | curr_target_.x, pose.x, throttle, 111 | curr_target_.y, pose.y, strafe); 112 | 113 | // Publish velocity 114 | publish_velocity(throttle, strafe, ubar_z, ubar_yaw); 115 | } 116 | } 117 | 118 | last_odom_time_ = msg_time; 119 | last_pose_ = pose; 120 | 121 | return retVal; 122 | } 123 | } 124 | 125 | -------------------------------------------------------------------------------- /src/flock_base.cpp: -------------------------------------------------------------------------------- 1 | #include "flock_base.hpp" 2 | 3 | namespace flock_base 4 | { 5 | 6 | FlockBase::FlockBase() : Node{"flock_base"} 7 | { 8 | #undef CXT_MACRO_MEMBER 9 | #define CXT_MACRO_MEMBER(n, t, d) CXT_MACRO_LOAD_PARAMETER((*this), (*this), n, t, d) 10 | CXT_MACRO_INIT_PARAMETERS(FLOCK_BASE_ALL_PARAMS, validate_parameters) 11 | 12 | #undef CXT_MACRO_MEMBER 13 | #define CXT_MACRO_MEMBER(n, t, d) CXT_MACRO_PARAMETER_CHANGED(n, t) 14 | CXT_MACRO_REGISTER_PARAMETERS_CHANGED((*this), (*this), FLOCK_BASE_ALL_PARAMS, validate_parameters) 15 | 16 | if (drones_.size() > 1) { 17 | RCLCPP_INFO(get_logger(), "%d drones, joystick controls %s, right bumper to change", 18 | drones_.size(), drones_[manual_control_].c_str()); 19 | } else { 20 | // A single drone always has the namespace "solo" 21 | RCLCPP_INFO(get_logger(), "1 drone"); 22 | } 23 | 24 | auto joy_cb = std::bind(&FlockBase::joy_callback, this, std::placeholders::_1); 25 | joy_sub_ = create_subscription("joy", 10, joy_cb); 26 | 27 | start_mission_pub_ = create_publisher("/start_mission", 1); 28 | stop_mission_pub_ = create_publisher("/stop_mission", 1); 29 | 30 | // Create N joy publishers 31 | for (auto i = drones_.begin(); i != drones_.end(); i++) { 32 | joy_pubs_.push_back(create_publisher((*i) + "/joy", 1)); 33 | } 34 | } 35 | 36 | inline bool button_down(const sensor_msgs::msg::Joy::SharedPtr curr, const sensor_msgs::msg::Joy &prev, int index) 37 | { 38 | return curr->buttons[index] && !prev.buttons[index]; 39 | } 40 | 41 | void FlockBase::joy_callback(const sensor_msgs::msg::Joy::SharedPtr msg) 42 | { 43 | static sensor_msgs::msg::Joy prev_msg; 44 | 45 | // Stop/start a mission 46 | if (mission_ && button_down(msg, prev_msg, joy_button_stop_mission_)) { 47 | stop_mission_pub_->publish(std_msgs::msg::Empty()); 48 | mission_ = false; 49 | } else if (!mission_ && button_down(msg, prev_msg, joy_button_start_mission_)) { 50 | start_mission_pub_->publish(std_msgs::msg::Empty()); 51 | mission_ = true; 52 | } 53 | 54 | // Ignore further input if we're in a mission 55 | if (mission_) { 56 | prev_msg = *msg; 57 | return; 58 | } 59 | 60 | // Toggle between drones 61 | if (button_down(msg, prev_msg, joy_button_next_drone_)) { 62 | if (drones_.size() < 2) { 63 | RCLCPP_WARN(get_logger(), "there's only 1 drone"); 64 | } else { 65 | if (++manual_control_ >= drones_.size()) { 66 | manual_control_ = 0; 67 | } 68 | RCLCPP_INFO(get_logger(), "joystick controls %s", drones_[manual_control_].c_str()); 69 | } 70 | } 71 | 72 | // Send joy message to the drone 73 | joy_pubs_[manual_control_]->publish(*msg); 74 | 75 | prev_msg = *msg; 76 | } 77 | 78 | void FlockBase::validate_parameters() 79 | { 80 | RCLCPP_INFO(get_logger(), "FlockBase Parameters"); 81 | 82 | #undef CXT_MACRO_MEMBER 83 | #define CXT_MACRO_MEMBER(n, t, d) CXT_MACRO_LOG_PARAMETER(RCLCPP_INFO, get_logger(), (*this), n, t, d) 84 | FLOCK_BASE_ALL_PARAMS 85 | } 86 | } // namespace flock_base 87 | 88 | int main(int argc, char **argv) 89 | { 90 | // Force flush of the stdout buffer 91 | setvbuf(stdout, NULL, _IONBF, BUFSIZ); 92 | 93 | // Init ROS 94 | rclcpp::init(argc, argv); 95 | 96 | // Create node 97 | auto node = std::make_shared(); 98 | auto result = rcutils_logging_set_logger_level(node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_INFO); 99 | 100 | rclcpp::Rate r(20); 101 | while (rclcpp::ok()) { 102 | // Respond to incoming messages 103 | rclcpp::spin_some(node); 104 | 105 | // Wait 106 | r.sleep(); 107 | } 108 | 109 | // Shut down ROS 110 | rclcpp::shutdown(); 111 | 112 | return 0; 113 | } -------------------------------------------------------------------------------- /src/flock_simple_path.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import time 4 | from enum import Enum 5 | 6 | import numpy as np 7 | import transformations as xf 8 | 9 | import rclpy 10 | from rclpy.node import Node 11 | from builtin_interfaces.msg import Time 12 | from geometry_msgs.msg import PoseStamped, Twist, TransformStamped 13 | from nav_msgs.msg import Path 14 | from std_msgs.msg import Empty 15 | from tf2_msgs.msg import TFMessage 16 | 17 | import smooth_path_4poly_2min 18 | 19 | 20 | def now(): 21 | cpu_time = time.time() 22 | sec = int(cpu_time) 23 | nanosec = int((cpu_time - sec) * 1000000) 24 | return Time(sec=sec, nanosec=nanosec) 25 | 26 | 27 | def ros_time_to_time(ros_time: Time) -> float: 28 | return ros_time.sec + ros_time.nanosec / 1000000.0 29 | 30 | 31 | class TrajectoryHandler(object): 32 | 33 | def __init__(self): 34 | 35 | # set up the variables of interest 36 | self._rel_times = np.array([]) # list of the relative times for the trajectory in decimal seconds 37 | self._positions = np.array([]) # list of the positions for the trajectory in 38 | 39 | self._use_waypoints = False 40 | self._path_3d = None 41 | 42 | self._repeat = False 43 | self._stabilize_sec = 0. 44 | 45 | def set_waypoints(self, data, repeat, stabilize_sec): 46 | # - each row contains a trajectory point 47 | # - each row consists of the following 4 comma separated values: 48 | # - relative time (seconds) 49 | # - x position 50 | # - y position 51 | # - z position 52 | d = np.array(data) 53 | self._rel_times = d[:, 0] 54 | self._positions = d[:, 1:4] 55 | 56 | self._path_3d = smooth_path_4poly_2min.Path3d(self._rel_times, self._positions) 57 | 58 | self._repeat = repeat 59 | self._stabilize_sec = stabilize_sec 60 | 61 | def is_trajectory_completed(self, inflight_time): 62 | return (inflight_time + self._stabilize_sec > self._rel_times[-1]) if not self._repeat else False 63 | 64 | def get_point(self, inflight_time): 65 | # check time range 66 | inflight_time -= self._stabilize_sec 67 | 68 | if inflight_time <= 0: 69 | return self._positions[0], np.zeros(3) 70 | 71 | elif self._repeat: 72 | _, inflight_time = divmod(inflight_time, self._rel_times[-1]) 73 | 74 | elif inflight_time >= self._rel_times[-1]: 75 | return self._positions[-1], np.zeros(3) 76 | 77 | return self._get_waypoint(inflight_time) if self._use_waypoints else self._get_smoothpoint(inflight_time) 78 | 79 | def _get_waypoint(self, inflight_time): 80 | # get the index of the trajectory with the closest time 81 | ind_min = np.argmin(np.abs(self._rel_times - inflight_time)) 82 | 83 | # get the time of this point 84 | time_ref = self._rel_times[ind_min] 85 | 86 | # create the position and velocity commands, 87 | # which depends on if current time is before or after the trajectory point 88 | if inflight_time < time_ref: # before the current point 89 | 90 | p0 = self._positions[ind_min - 1] 91 | p1 = self._positions[ind_min] 92 | 93 | t0 = self._rel_times[ind_min - 1] 94 | t1 = self._rel_times[ind_min] 95 | 96 | else: # after the current point 97 | 98 | # now need to check if we are at the end of the file 99 | if ind_min >= len(self._positions) - 1: 100 | 101 | p0 = self._positions[ind_min] 102 | p1 = self._positions[ind_min] 103 | 104 | t0 = 0.0 105 | t1 = 1.0 106 | 107 | else: 108 | 109 | p0 = self._positions[ind_min] 110 | p1 = self._positions[ind_min + 1] 111 | 112 | t0 = self._rel_times[ind_min] 113 | t1 = self._rel_times[ind_min + 1] 114 | 115 | # compute the position target (interpolating between points) 116 | position_target = (p1 - p0) * (inflight_time - t0) / (t1 - t0) + p0 117 | 118 | # compute the velocity target based on the desired points and time 119 | velocity_target = (p1 - p0) / (t1 - t0) 120 | 121 | return position_target, velocity_target 122 | 123 | def _get_smoothpoint(self, inflight_time): 124 | return self._path_3d.calc_y_and_y_dot(inflight_time) 125 | 126 | 127 | class VelocityController(object): 128 | 129 | def __init__(self): 130 | 131 | # define all the gains that will be needed 132 | self._kp_pos = 1.0 # gain for lateral position error 133 | self._kp_alt = 1.0 # gain for altitude error 134 | self._kp_yaw = 1.0 # gain for yaw error 135 | 136 | # some limits to use 137 | self._v_max = 0.4 # the maximum horizontal velocity 138 | self._hdot_max = 0.4 # the maximum vertical velocity 139 | self._yawdot_max = 0.8 # the maximum yaw rate 140 | 141 | def lateral_position_control(self, pos_cmd, pos, vel_target=np.array((0., 0.))): 142 | pos_error = pos_cmd - pos 143 | lateral_vel_cmd = self._kp_pos * pos_error + vel_target 144 | lateral_vel_cmd = np.clip(lateral_vel_cmd, -self._v_max, self._v_max) 145 | print( 146 | ("pos_cmd:{0:7.3f},{1:7.3f}; pos:{2:7.3f},{3:7.3f}; pos_error:{4:7.3f},{5:7.3f}; " + 147 | "vel_target:{6:7.3f},{7:7.3f}; lateral_vel_cmd:{8:7.3f},{9:7.3f}") 148 | .format(pos_cmd[0], pos_cmd[1], pos[0], pos[1], pos_error[0], pos_error[1], 149 | vel_target[0], vel_target[1], lateral_vel_cmd[0], lateral_vel_cmd[1])) 150 | return lateral_vel_cmd 151 | 152 | def altitude_control(self, alt_cmd, alt, hdot_target=0.0): 153 | alt_error = alt_cmd - alt 154 | hdot_cmd = self._kp_alt * alt_error + hdot_target 155 | hdot_cmd = np.clip(hdot_cmd, -self._hdot_max, self._hdot_max) 156 | return hdot_cmd 157 | 158 | def yaw_control(self, yaw_cmd, yaw, yawdot_target=0.0): 159 | yaw_error = yaw_cmd - yaw 160 | if yaw_error > np.pi: 161 | yaw_error -= 2.0 * np.pi 162 | elif yaw_error < -np.pi: 163 | yaw_error += 2.0 * np.pi 164 | 165 | yawdot_cmd = self._kp_yaw * yaw_error + yawdot_target 166 | yawdot_cmd = np.clip(yawdot_cmd, -self._yawdot_max, self._yawdot_max) 167 | # print( 168 | # ("yaw_cmd:{0:7.3f}; yaw:{1:7.3f}; yaw_error:{2:7.3f}; " + 169 | # "yawdot_target:{3:7.3f}; yawdot_cmd:{4:7.3f}") 170 | # .format(yaw_cmd, yaw, yaw_error, 171 | # yawdot_target, yawdot_cmd)) 172 | return yawdot_cmd 173 | 174 | 175 | class TrajectoryVelocityFlyer: 176 | class States(Enum): 177 | INIT = 0 178 | WAITING = 1 179 | LAUNCHING = 2 180 | RUNNING = 3 181 | LANDING = 4 182 | DONE = 5 183 | 184 | def __init__(self): 185 | self._controller = VelocityController() 186 | self._trajectory = TrajectoryHandler() 187 | self._callback_node = None 188 | self._state = self.States.INIT 189 | 190 | self._lag_limit_sec = 0.25 191 | self._launch_limit_sec = 10.0 192 | self._already_flying = True # don't launch and land 193 | 194 | self._start_msg_time = 0.0 195 | self._start_drone_state = np.zeros(4) 196 | self._watchdog_clear = False 197 | 198 | def start(self): 199 | if self._state == self.States.INIT: 200 | self._state = self.States.WAITING 201 | 202 | def stop(self): 203 | assert self._callback_node 204 | 205 | if self._state == self.States.LAUNCHING or \ 206 | self._state == self.States.RUNNING: 207 | # for now just tell the drone to stop (do not try to land) if it is flying. 208 | self._call_flyer_cmd_callback(np.zeros(4)) 209 | 210 | last_state = self._state 211 | self._state = self.States.DONE 212 | 213 | # tell the node that we are stopping. 214 | if last_state != self.States.DONE: 215 | self._call_flyer_stoping_callback() 216 | 217 | def set_waypoints(self, waypoints, repeat=False, stabilize_sec=0.0): 218 | self._trajectory.set_waypoints(waypoints, repeat, stabilize_sec) 219 | 220 | def set_callback_node(self, callback_node): 221 | self._callback_node = callback_node 222 | 223 | def new_drone_state(self, msg_time, drone_state): 224 | self._watchdog_clear = True 225 | 226 | # Certain states don't process this message 227 | if self._state == self.States.INIT or \ 228 | self._state == self.States.LANDING or \ 229 | self._state == self.States.DONE: 230 | return 231 | 232 | if self._state == self.States.WAITING: 233 | # Wait for the lag to get small before starting 234 | if self._no_lag(msg_time): 235 | assert self._callback_node 236 | self._launch(msg_time, drone_state) 237 | return 238 | 239 | # if we stop getting state messages, then stop. 240 | if not self._no_lag(msg_time): 241 | self._call_flyer_log_info_callback('too much lag in state messages') 242 | self.stop() 243 | return 244 | 245 | # Launching rises to the height of the first way point or a fixed amount of time 246 | if self._state == self.States.LAUNCHING: 247 | pos_target, _ = self._trajectory.get_point(0.0) 248 | if drone_state[2] >= pos_target[2] or \ 249 | msg_time - self._start_msg_time > self._launch_limit_sec: 250 | self._start_msg_time = msg_time 251 | self._start_drone_state = drone_state 252 | self._state = self.States.RUNNING 253 | return 254 | 255 | # process the RUNNING state: 256 | if self._process_drone_state(msg_time, drone_state): 257 | self._land() 258 | 259 | def timer_fired(self): 260 | # if the clear flag is false, then we haven't had 261 | # a state message in a while so stop 262 | if not self._watchdog_clear: 263 | if self._state == self.States.LAUNCHING or \ 264 | self._state == self.States.RUNNING: 265 | self._call_flyer_log_info_callback('too much time between state messages') 266 | self.stop() 267 | return 268 | self._watchdog_clear = False 269 | 270 | def _no_lag(self, msg_time: float) -> bool: 271 | # Return True if the lag between when the message was sent and 272 | # now is not too long 273 | now_time = time.time() 274 | return now_time - msg_time < self._lag_limit_sec 275 | 276 | def _process_drone_state(self, msg_time, drone_state): 277 | # get the current in flight time 278 | rel_time = msg_time - self._start_msg_time 279 | 280 | # get the target position and velocity from the trajectory (in world frame) 281 | pos_target, vel_target = self._trajectory.get_point(rel_time) 282 | 283 | # run the controller for position (position controller -> to velocity command) 284 | vel_cmd = np.zeros(4) 285 | vel_cmd[0:2] = self._controller.lateral_position_control(pos_target[0:2], drone_state[0:2], vel_target[0:2]) 286 | vel_cmd[2] = self._controller.altitude_control(pos_target[2], drone_state[2], vel_target[2]) 287 | 288 | # The drone should yaw toward the origin. Figure out the yaw and raw_dot targets and run the yaw controller 289 | yaw_target = np.arctan2(-pos_target[1], -pos_target[0]) 290 | vel_target_half = 0.5 * vel_target 291 | yaw_dot_target = np.arctan2(-pos_target[1] + vel_target_half[1], -pos_target[0] + vel_target_half[0]) - \ 292 | np.arctan2(-pos_target[1] - vel_target_half[1], -pos_target[0] - vel_target_half[0]) 293 | vel_cmd[3] = self._controller.yaw_control(yaw_target, drone_state[3], -yaw_dot_target) 294 | 295 | # The vel_cmd should be in the drone frame. Rotate the command velocities by the yaw. 296 | yaw = drone_state[3] 297 | vel_cmd_drone = np.copy(vel_cmd) 298 | vel_cmd_drone[0] = np.cos(yaw) * vel_cmd[0] + np.sin(yaw) * vel_cmd[1] 299 | vel_cmd_drone[1] = -np.sin(yaw) * vel_cmd[0] + np.cos(yaw) * vel_cmd[1] 300 | 301 | # print('pos_target:', pos_target, 'vel_target:', 302 | # vel_target, 'vel_cmd', vel_cmd, 'vel_cmd_drone', vel_cmd_drone) 303 | 304 | # send the velocity command to the drone 305 | self._call_flyer_cmd_callback(vel_cmd_drone) 306 | 307 | # check for the end of the trajectory 308 | return self._trajectory.is_trajectory_completed(rel_time) 309 | 310 | def _launch(self, msg_time, drone_state): 311 | self._call_flyer_log_info_callback('launching') 312 | self._start_msg_time = msg_time 313 | self._start_drone_state = drone_state 314 | self._state = self.States.RUNNING if self._already_flying else self.States.LAUNCHING 315 | if not self._already_flying: 316 | self._call_flyer_takeoff_callback() 317 | 318 | def _land(self): 319 | self._call_flyer_log_info_callback('landing') 320 | if not self._already_flying: 321 | self._state = self.States.LANDING 322 | self._call_flyer_land_callback() 323 | self.stop() 324 | 325 | def _call_flyer_cmd_callback(self, vel_cmd): 326 | self._callback_node.flyer_cmd_callback(vel_cmd) 327 | 328 | def _call_flyer_stoping_callback(self): 329 | self._callback_node.flyer_stopping_callback() 330 | 331 | def _call_flyer_takeoff_callback(self): 332 | self._callback_node.flyer_takeoff_callback() 333 | 334 | def _call_flyer_land_callback(self): 335 | self._callback_node.flyer_land_callback() 336 | 337 | def _call_flyer_log_info_callback(self, msg): 338 | self._callback_node.flyer_log_info_callback(msg) 339 | 340 | 341 | class FlockSimplePath(Node): 342 | class States(Enum): 343 | INIT = 0 344 | RUNNING = 1 345 | DONE = 2 346 | 347 | def __init__(self, flyer): 348 | super().__init__('flock_simple_path') 349 | self._flyer = flyer 350 | flyer.set_callback_node(self) 351 | 352 | # Parameters 353 | self._map_frame = self.get_parameter_or('base_frame', 'base_link') 354 | self._base_frame = self.get_parameter_or('map_frame', 'map') 355 | 356 | self._state = self.States.INIT 357 | 358 | # ROS publishers 359 | self._cmd_vel_pub = self.create_publisher(Twist, 'cmd_vel') 360 | self._takeoff_pub = self.create_publisher(Empty, 'takeoff') 361 | self._land_pub = self.create_publisher(Empty, 'land') 362 | self._path_pub = self.create_publisher(Path, 'planned_path') 363 | 364 | # ROS subscriptions 365 | self.create_subscription(Empty, '/start_mission', self._ros_start_callback) 366 | self.create_subscription(Empty, '/stop_mission', self._ros_stop_callback) 367 | self.create_subscription(TFMessage, '/tf', self._ros_tf_callback) 368 | 369 | # Timer 370 | timer_period_sec = 1. 371 | self.create_timer(timer_period_sec, self._ros_timer_callback) 372 | 373 | self.get_logger().info('init complete') 374 | 375 | def get_parameter_or(self, name, default): 376 | value = self.get_parameter(name).value 377 | if value is None: 378 | value = default 379 | return value 380 | 381 | def publish_path(self, waypoints): 382 | """Publish a list of waypoints as a ROS path""" 383 | path = Path() 384 | path.header.stamp = now() 385 | path.header.frame_id = self._map_frame 386 | for waypoint in waypoints: 387 | pose = PoseStamped() 388 | pose.header.frame_id = self._base_frame 389 | pose.header.stamp = now() 390 | pose.pose.position.x = waypoint[1] 391 | pose.pose.position.y = waypoint[2] 392 | pose.pose.position.z = waypoint[3] 393 | pose.pose.orientation.w = 1. 394 | path.poses.append(pose) 395 | self._path_pub.publish(path) 396 | 397 | def start(self): 398 | if self._state == self.States.INIT: 399 | self._state = self.States.RUNNING 400 | self._flyer.start() 401 | 402 | def stop(self): 403 | if self._state == self.States.RUNNING: 404 | self._flyer.stop() 405 | self._state = self.States.DONE 406 | 407 | def _ros_start_callback(self, msg): 408 | self.get_logger().info('starting mission') 409 | self.start() 410 | 411 | def _ros_stop_callback(self, msg): 412 | self.get_logger().info('stopping mission') 413 | self.stop() 414 | 415 | def _ros_tf_callback(self, msg): 416 | if self._state == self.States.RUNNING: 417 | # only process tf messages for the drone position 418 | for transform in msg.transforms: # type: TransformStamped 419 | if transform.header.frame_id == self._map_frame and transform.child_frame_id == self._base_frame: 420 | xlat = transform.transform.translation 421 | quat = transform.transform.rotation 422 | _, _, yaw = xf.euler_from_quaternion([quat.w, quat.x, quat.y, quat.z]) 423 | state = np.array([xlat.x, xlat.y, xlat.z, yaw]) 424 | self._flyer.new_drone_state(ros_time_to_time(transform.header.stamp), state) 425 | 426 | def _ros_timer_callback(self): 427 | if self._state == self.States.RUNNING: 428 | self._flyer.timer_fired() 429 | 430 | def flyer_cmd_callback(self, vel_cmd): 431 | if self._state == self.States.RUNNING: 432 | # The commands coming from the flyer are a rate. How these translate 433 | # into drone commands is a mystery. For now pass the values on through. 434 | twist = Twist() 435 | twist.linear.x = vel_cmd[0] 436 | twist.linear.y = vel_cmd[1] 437 | twist.linear.z = vel_cmd[2] 438 | twist.angular.z = vel_cmd[3] 439 | 440 | self._cmd_vel_pub.publish(twist) 441 | 442 | def flyer_stopping_callback(self): 443 | # flyer has decided to stop. 444 | self._state = self.States.DONE 445 | 446 | def flyer_takeoff_callback(self): 447 | if self._state == self.States.RUNNING: 448 | self._takeoff_pub.publish(Empty()) 449 | 450 | def flyer_land_callback(self): 451 | if self._state == self.States.RUNNING: 452 | self._land_pub.publish(Empty()) 453 | 454 | def flyer_log_info_callback(self, msg): 455 | self.get_logger().info(msg) 456 | 457 | 458 | class Figure: 459 | def __init__(self, speed, origin, scale): 460 | self._speed = speed 461 | self._origin = np.array(origin) 462 | self._scale = scale 463 | 464 | def _abs_pos(self, rel_pos): 465 | return rel_pos * self._scale + self._origin 466 | 467 | @staticmethod 468 | def _elem(t, abs_pos): 469 | return [t, abs_pos[0], abs_pos[1], abs_pos[2]] 470 | 471 | def _gen(self, func): 472 | curr_t = 0.0 473 | curr_pos = np.zeros(3) 474 | last_abs_pos = self._abs_pos(curr_pos) 475 | yield self._elem(0., last_abs_pos) 476 | for e in func(self): 477 | this_pos = curr_pos 478 | for s, pos in e: 479 | rel_pos = np.array(pos) + this_pos 480 | abs_pos = self._abs_pos(rel_pos) 481 | delta_t = -s if s <= 0.0 else np.linalg.norm(abs_pos - last_abs_pos) / s 482 | curr_t = curr_t + delta_t 483 | yield self._elem(curr_t, abs_pos) 484 | curr_pos = rel_pos 485 | last_abs_pos = abs_pos 486 | 487 | def generate(self, func): 488 | return np.array([p for p in self._gen(func)]) 489 | 490 | @staticmethod 491 | def pause(sec): 492 | yield -sec, (0., 0., 0.) 493 | 494 | def _line_by(self, p): 495 | return self._speed, (p[0], p[1], p[2]) 496 | 497 | def line_by(self, p): 498 | yield self._line_by(p) 499 | 500 | def lines_by(self, ps): 501 | for p in ps: 502 | yield self._line_by(p) 503 | 504 | 505 | class WaypointGenerator: 506 | 507 | @staticmethod 508 | def line_y(figure: Figure): 509 | yield figure.pause(4) 510 | yield figure.line_by([0., 0.5, 0.]) 511 | yield figure.line_by([0., -1., 0.]) 512 | yield figure.line_by([0., 0.5, 0.]) 513 | 514 | @staticmethod 515 | def stationary(figure: Figure): 516 | yield figure.pause(20) 517 | 518 | @staticmethod 519 | def generate(style, speed=0.15, origin=(0., 0., 0.), scale=1.0): 520 | assert style in WaypointGenerator.__dict__ and isinstance(WaypointGenerator.__dict__[style], staticmethod) 521 | return Figure(speed, origin, scale).generate(WaypointGenerator.__dict__[style].__func__) 522 | 523 | 524 | def main(args=None): 525 | rclpy.init(args=args) 526 | 527 | flyer = TrajectoryVelocityFlyer() 528 | wp = WaypointGenerator.generate('line_y', origin=[-1.3, 0.0, 1.2]) 529 | flyer.set_waypoints(wp, stabilize_sec=3.0, repeat=True) 530 | 531 | node = FlockSimplePath(flyer) 532 | node.publish_path(wp) 533 | 534 | try: 535 | rclpy.spin(node) 536 | except KeyboardInterrupt: 537 | node.get_logger().info("Ctrl-C detected, shutting down") 538 | finally: 539 | flyer.stop() 540 | node.destroy_node() 541 | rclpy.shutdown() 542 | 543 | 544 | if __name__ == '__main__': 545 | main() 546 | -------------------------------------------------------------------------------- /src/planner_node.cpp: -------------------------------------------------------------------------------- 1 | #include "planner_node.hpp" 2 | 3 | #include "simple_planner.hpp" 4 | 5 | namespace planner_node 6 | { 7 | 8 | //==================== 9 | // Constants 10 | //==================== 11 | 12 | // Spin rate 13 | const int SPIN_RATE = 20; 14 | 15 | // Arena constraints 16 | const double MIN_ARENA_Z = 1.5; 17 | const double MIN_ARENA_XY = 2.0; 18 | const double GROUND_EPSILON = 1.2; 19 | 20 | //==================== 21 | // Utilities 22 | //==================== 23 | 24 | // Are we on the ground? 25 | inline bool on_the_ground(nav_msgs::msg::Odometry::SharedPtr msg) 26 | { 27 | return std::abs(msg->pose.pose.position.z) < GROUND_EPSILON; 28 | } 29 | 30 | //==================== 31 | // DroneInfo 32 | //==================== 33 | 34 | DroneInfo::DroneInfo(rclcpp::Node *node, std::string ns) : ns_{ns}, valid_landing_pose_{false} 35 | { 36 | auto odom_cb = std::bind(&DroneInfo::odom_callback, this, std::placeholders::_1); 37 | 38 | // TODO move topics to cxt 39 | odom_sub_ = node->create_subscription(ns + "/base_odom", 10, odom_cb); 40 | plan_pub_ = node->create_publisher(ns + "/plan", 1); 41 | } 42 | 43 | void DroneInfo::odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg) 44 | { 45 | if (!valid_landing_pose_ && on_the_ground(msg)) { 46 | landing_pose_.header = msg->header; 47 | landing_pose_.pose = msg->pose.pose; 48 | landing_pose_.pose.position.z = 0; 49 | valid_landing_pose_ = true; 50 | } 51 | } 52 | 53 | //==================== 54 | // PlannerNode 55 | //==================== 56 | 57 | PlannerNode::PlannerNode() : Node{"planner_node"} 58 | { 59 | #undef CXT_MACRO_MEMBER 60 | #define CXT_MACRO_MEMBER(n, t, d) CXT_MACRO_LOAD_PARAMETER((*this), cxt_, n, t, d) 61 | CXT_MACRO_INIT_PARAMETERS(PLANNER_NODE_ALL_PARAMS, validate_parameters) 62 | 63 | #undef CXT_MACRO_MEMBER 64 | #define CXT_MACRO_MEMBER(n, t, d) CXT_MACRO_PARAMETER_CHANGED(n, t) 65 | CXT_MACRO_REGISTER_PARAMETERS_CHANGED((*this), cxt_, PLANNER_NODE_ALL_PARAMS, validate_parameters) 66 | 67 | // Get drone namespaces 68 | if (cxt_.drones_.size() > 1) { 69 | RCLCPP_INFO(get_logger(), "%d drones", cxt_.drones_.size()); 70 | } else { 71 | RCLCPP_INFO(get_logger(), "1 drone"); 72 | } 73 | 74 | // Get arena 75 | RCLCPP_INFO(get_logger(), "arena is (0, 0, 0) to (%g, %g, %g)", cxt_.arena_x_, cxt_.arena_y_, cxt_.arena_z_); 76 | 77 | auto start_mission_cb = std::bind(&PlannerNode::start_mission_callback, this, std::placeholders::_1); 78 | auto stop_mission_cb = std::bind(&PlannerNode::stop_mission_callback, this, std::placeholders::_1); 79 | 80 | start_mission_sub_ = create_subscription("/start_mission", 10, start_mission_cb); 81 | stop_mission_sub_ = create_subscription("/stop_mission", 10, stop_mission_cb); 82 | 83 | for (auto i = cxt_.drones_.begin(); i != cxt_.drones_.end(); i++) { 84 | drones_.push_back(std::make_shared(this, *i)); 85 | } 86 | } 87 | 88 | void PlannerNode::create_and_publish_plans() 89 | { 90 | // Check arena volume 91 | if (std::abs(cxt_.arena_x_) < MIN_ARENA_XY || std::abs(cxt_.arena_y_) < MIN_ARENA_XY || 92 | cxt_.arena_z_ < MIN_ARENA_Z) { 93 | RCLCPP_ERROR(get_logger(), "arena must be at least (%g, %g, %g)", MIN_ARENA_XY, MIN_ARENA_XY, MIN_ARENA_Z); 94 | return; 95 | } 96 | 97 | // Make sure we have landing poses 98 | for (auto i = drones_.begin(); i != drones_.end(); i++) { 99 | if (!(*i)->valid_landing_pose()) { 100 | RCLCPP_ERROR(get_logger(), "waiting for landing pose for %s (and possibly more)", (*i)->ns().c_str()); 101 | return; 102 | } 103 | } 104 | 105 | // Create N plans 106 | std::vector landing_poses; 107 | for (auto i = drones_.begin(); i != drones_.end(); i++) { 108 | landing_poses.push_back((*i)->landing_pose()); 109 | } 110 | simple_planner::SimplePlanner planner(landing_poses); 111 | std::vector plans = planner.plans(now()); 112 | RCLCPP_INFO(get_logger(), "plan(s) created"); 113 | 114 | // Publish N plans 115 | for (int i = 0; i < drones_.size(); i++) { 116 | drones_[i]->plan_pub()->publish(plans[i]); 117 | } 118 | } 119 | 120 | void PlannerNode::start_mission_callback(const std_msgs::msg::Empty::SharedPtr msg) 121 | { 122 | (void) msg; 123 | RCLCPP_INFO(get_logger(), "start mission"); 124 | create_and_publish_plans(); 125 | } 126 | 127 | void PlannerNode::stop_mission_callback(const std_msgs::msg::Empty::SharedPtr msg) 128 | { 129 | (void) msg; 130 | RCLCPP_INFO(get_logger(), "stop mission"); 131 | } 132 | 133 | void PlannerNode::validate_parameters() 134 | { 135 | RCLCPP_INFO(get_logger(), "PlannerNode Parameters"); 136 | 137 | #undef CXT_MACRO_MEMBER 138 | #define CXT_MACRO_MEMBER(n, t, d) CXT_MACRO_LOG_PARAMETER(RCLCPP_INFO, get_logger(), cxt_, n, t, d) 139 | PLANNER_NODE_ALL_PARAMS 140 | } 141 | 142 | } // namespace planner_node 143 | 144 | //==================== 145 | // main 146 | //==================== 147 | 148 | int main(int argc, char **argv) 149 | { 150 | // Force flush of the stdout buffer 151 | setvbuf(stdout, NULL, _IONBF, BUFSIZ); 152 | 153 | // Init ROS 154 | rclcpp::init(argc, argv); 155 | 156 | // Create node 157 | auto node = std::make_shared(); 158 | auto result = rcutils_logging_set_logger_level(node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG); 159 | 160 | // Spin until rclcpp::ok() returns false 161 | rclcpp::spin(node); 162 | 163 | // Shut down ROS 164 | rclcpp::shutdown(); 165 | 166 | return 0; 167 | } -------------------------------------------------------------------------------- /src/simple_planner.cpp: -------------------------------------------------------------------------------- 1 | #include "simple_planner.hpp" 2 | 3 | #include 4 | 5 | #include "tf2_geometry_msgs/tf2_geometry_msgs.h" 6 | 7 | namespace simple_planner 8 | { 9 | 10 | // Plan: 11 | // The collection of takeoff positions define the waypoints. 12 | // Each drone flies through all takeoff positions back to it's own takeoff position, then lands. 13 | // Example: if there are 4 drones, each drone flies 4 line segments. 14 | // If the number of drones is less than 3 then additional waypoints are added. 15 | // Each drone maintains the same orientation throughout the flight. 16 | 17 | // Timestamps: 18 | // Mission time starts at node->now(). 19 | // Plan will include a time buffer at each waypoint (TAKEOFF_NS, STABILIZE_NS). 20 | // Drone must get to waypoint by the indicated timestamp or earlier. 21 | // Drone should start moving to the first waypoint as soon as the plan is received. 22 | // Drone should start moving to the next waypoint at the timestamp of the previous waypoint. 23 | // Drone controller can compute speed from the timestamp with knowledge of the time buffer values. 24 | 25 | // Future improvements: 26 | // Create good plans for a small number of drones (1, 2, 3) 27 | // Check for crossing paths 28 | // Check arena bounds 29 | // Adjust yaw so that that drones can see markers 30 | 31 | const double CRUISING_Z = 1.0; // m 32 | const double SEPARATION = 4.0; // m 33 | const double SPEED = 0.2; // m/s 34 | 35 | const rclcpp::Duration TAKEOFF{9000000000}; // includes time to send plan and send takeoff command and takeoff. 36 | const rclcpp::Duration STABILIZE{9000000000}; 37 | 38 | #define SUPER_SIMPLE 39 | 40 | #ifdef SUPER_SIMPLE 41 | // x, y, z, yaw 42 | double starting_locations[4][4] = { 43 | // Face the wall of markers in fiducial.world 44 | // -2.5, 1.5, 1.0, 0.0, 45 | // -1.5, 0.5, 1.0, 0.785, 46 | // -0.5, 1.5, 1.0, 0.0, 47 | // -1.5, 2.5, 1.0, -0.785 48 | 49 | -1.0, 1.0, 1.0, 0.0, 50 | -1.0, 0.5, 1.0, 0.0, 51 | -1.0, 0.0, 1.0, 0.0, 52 | -1.0, -0.5, 1.0, -0.0 53 | 54 | // Face all 4 directions in f2.world 55 | // -2.5, 1.5, 1.0, 0.0, 56 | // -1.5, 0.5, 1.0, 1.57, 57 | // -0.5, 1.5, 1.0, 3.14, 58 | // -1.5, 3.0, 1.0, -1.57 59 | }; 60 | #endif 61 | 62 | double distance(const geometry_msgs::msg::Point &p1, const geometry_msgs::msg::Point &p2) 63 | { 64 | return sqrt(pow(p2.x - p1.x, 2) + pow(p2.y - p1.y, 2) + pow(p2.z - p1.z, 2)); 65 | } 66 | 67 | SimplePlanner::SimplePlanner(const std::vector &landing_poses) 68 | { 69 | assert(landing_poses.size() > 0); 70 | 71 | num_drones_ = landing_poses.size(); 72 | 73 | // Waypoints are directly above landing poses 74 | waypoints_ = landing_poses; 75 | for (auto i = waypoints_.begin(); i != waypoints_.end(); i++) { 76 | i->pose.position.z = CRUISING_Z; 77 | } 78 | 79 | #ifdef SUPER_SIMPLE 80 | for (int i = waypoints_.size(); i < 4; i++) { 81 | geometry_msgs::msg::PoseStamped p = waypoints_[0]; 82 | p.pose.position.x = starting_locations[i][0]; 83 | p.pose.position.y = starting_locations[i][1]; 84 | p.pose.position.z = starting_locations[i][2]; 85 | tf2::Quaternion q; 86 | q.setRPY(0, 0, starting_locations[i][3]); 87 | p.pose.orientation = tf2::toMsg(q); 88 | waypoints_.push_back(p); 89 | } 90 | #else 91 | if (waypoints_.size() == 1) { 92 | // Add a 2nd waypoint 93 | geometry_msgs::msg::PoseStamped p = waypoints_[0]; 94 | p.pose.position.y += SEPARATION; 95 | waypoints_.push_back(p); 96 | } 97 | 98 | if (waypoints_.size() == 2) { 99 | // Add a 3rd waypoint to make an equilateral triangle 100 | geometry_msgs::msg::PoseStamped p = waypoints_[0]; 101 | const auto x1 = waypoints_[0].pose.position.x; 102 | const auto y1 = waypoints_[0].pose.position.y; 103 | const auto x2 = waypoints_[1].pose.position.x; 104 | const auto y2 = waypoints_[1].pose.position.y; 105 | p.pose.position.x = (x1 + x2 + sqrt(3) * (y1 - y2)) / 2; 106 | p.pose.position.y = (y1 + y2 + sqrt(3) * (x2 - x1)) / 2; 107 | waypoints_.push_back(p); 108 | } 109 | #endif 110 | } 111 | 112 | std::vector SimplePlanner::plans(const rclcpp::Time &now) const 113 | { 114 | std::vector plans; 115 | 116 | // Compute time to fly from this waypoint to next waypoint 117 | // Assume instant acceleration and constant speed 118 | std::vector flight_time; 119 | for (int i = 0; i < waypoints_.size(); i++) { 120 | int next = (i + 1) % waypoints_.size(); 121 | flight_time.emplace_back(distance(waypoints_[next].pose.position, waypoints_[i].pose.position) / SPEED * 1e9); 122 | } 123 | 124 | // Create num_drones_ plans, each with waypoints_.size() waypoints_ 125 | for (int i = 0; i < num_drones_; i++) { 126 | // Init path 127 | nav_msgs::msg::Path path; 128 | path.header.stamp = now; 129 | plans.push_back(path); 130 | 131 | // Timestamp for waypoint 0 132 | rclcpp::Time timestamp = now + TAKEOFF; 133 | 134 | // Add waypoints to path 135 | for (int j = 0; j < waypoints_.size(); j++) { 136 | int curr = (i + j) % waypoints_.size(); 137 | plans[i].poses.push_back(waypoints_[curr]); 138 | plans[i].poses[j].header.stamp = timestamp; 139 | 140 | // Timestamp for waypoint j + 1 141 | timestamp = timestamp + flight_time[curr]; 142 | } 143 | 144 | // Last waypoint returns to the spot just above the landing pose 145 | plans[i].poses.push_back(waypoints_[i]); 146 | plans[i].poses[waypoints_.size()].header.stamp = timestamp; 147 | } 148 | 149 | return plans; 150 | } 151 | 152 | } // namespace simple_planner 153 | -------------------------------------------------------------------------------- /src/smooth_path_4poly_2min.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import numpy as np 4 | 5 | 6 | class SmoothPath4Poly2Min: 7 | 8 | def __init__(self, ys, ts, v0, vn): 9 | self.ys = np.array(ys) 10 | self.ts = np.array(ts) 11 | 12 | self.tbeg = self.ts[0] 13 | self.tend = self.ts[self.ts.shape[0] - 1] 14 | 15 | self.ps = self.generate_smooth_trajectory(v0, vn) 16 | self.p_dots = self.ps @ np.array([[0, 0, 0], 17 | [1, 0, 0], 18 | [0, 2, 0], 19 | [0, 0, 3]]) 20 | self.p_dotdots = self.ps @ np.array([[0, 0], 21 | [0, 0], 22 | [2, 0], 23 | [0, 6]]) 24 | 25 | self.last_t_idx = 0 26 | 27 | def generate_smooth_trajectory(self, v0, vn): 28 | n = self.ys.shape[0] 29 | 30 | # create the taus 31 | taus = self.ts[1:] - self.ts[:-1] 32 | 33 | # create the Q matrix 34 | Q = np.zeros([4 * (n - 1), 4 * (n - 1)]) 35 | for i in range(n - 1): 36 | tau = taus[i] 37 | ib = i * 4 + 2 38 | Q[ib:ib + 2, ib:ib + 2] = np.array([[4 * tau, 6 * tau ** 2], [6 * tau ** 2, 12 * tau ** 3]]) 39 | 40 | # create the A and b matrices 41 | A = np.zeros([4 * (n - 1), 4 * (n - 1)]) 42 | b = np.zeros(4 * (n - 1)) 43 | 44 | # add the start and end velocity constraints 45 | A[0, 1] = 1 46 | b[0] = v0 47 | tau = taus[n - 2] 48 | A[1, 4 * (n - 2):4 * (n - 2) + 4] = np.array([0, 1, 2 * tau, 3 * tau ** 2]) 49 | b[1] = vn 50 | 51 | rb = 2 52 | 53 | # add the position constraints 54 | for i in range(n - 1): 55 | tau = taus[i] 56 | A[rb:rb + 2, i * 4:i * 4 + 4] = np.array([[1, 0, 0, 0], [1, tau, tau ** 2, tau ** 3]]) 57 | b[rb:rb + 2] = self.ys[i:i + 2] 58 | rb += 2 59 | 60 | # add the velocity continuity constraints 61 | for i in range(n - 2): 62 | tau = taus[i] 63 | A[rb:rb + 1, i * 4:i * 4 + 8] = np.array([0, 1, 2 * tau, 3 * tau ** 2, 0, -1, 0, 0]) 64 | rb += 1 65 | 66 | # add the unknown velocities constraints 67 | for i in range(1, n - 1): 68 | A[rb:rb + 1, i * 4:i * 4 + 4] = np.array([0, 1, 0, 0]) 69 | rb += 1 70 | 71 | # compute R 72 | Ainv = np.linalg.inv(A) 73 | R = Ainv.T @ Q @ Ainv 74 | 75 | # partition R 76 | ip = 4 * (n - 1) - (n - 2) 77 | Rvv = R[ip:, ip:] 78 | Rvc = R[ip:, 0:ip] 79 | bc = b[0:ip] 80 | 81 | # solve for bv 82 | bv = -np.linalg.inv(Rvv) @ Rvc @ bc 83 | 84 | # plug the optimum bv into b and compute the polynomial coefficients 85 | b[ip:] = bv 86 | ps = Ainv @ b 87 | 88 | return ps.reshape(-1, 4) 89 | 90 | def pick_pt(self, t): 91 | if self.ts[self.last_t_idx] <= t <= self.ts[self.last_t_idx + 1]: 92 | return self.last_t_idx, t - self.ts[self.last_t_idx] 93 | 94 | if self.ts[self.last_t_idx] > t: 95 | while self.last_t_idx > 0: 96 | self.last_t_idx -= 1 97 | if self.ts[self.last_t_idx] <= t: 98 | return self.last_t_idx, t - self.ts[self.last_t_idx] 99 | return 0, -1.0 100 | 101 | while self.last_t_idx < self.ts.shape[0] - 2: 102 | self.last_t_idx += 1 103 | if t <= self.ts[self.last_t_idx + 1]: 104 | return self.last_t_idx, t - self.ts[self.last_t_idx] 105 | return 0, -1.0 106 | 107 | def calc_pt_y(self, pt): 108 | t = pt[1] 109 | if t < 0.: 110 | return 0.0 111 | return self.ps[pt[0], :] @ np.array([1, t, t ** 2, t ** 3]) 112 | 113 | def calc_pt_y_dot(self, pt): 114 | t = pt[1] 115 | if t < 0.: 116 | return 0.0 117 | return self.p_dots[pt[0], :] @ np.array([1, t, t ** 2]) 118 | 119 | def calc_pt_y_dotdot(self, pt): 120 | t = pt[1] 121 | if t < 0.: 122 | return 0.0 123 | return self.p_dotdots[pt[0], :] @ np.array([1, t]) 124 | 125 | def calc_y(self, t): 126 | return self.calc_pt_y(self.pick_pt(t)) 127 | 128 | def calc_y_dot(self, t): 129 | return self.calc_pt_y_dot(self.pick_pt(t)) 130 | 131 | def calc_y_dotdot(self, t): 132 | return self.calc_pt_y_dotdot(self.pick_pt(t)) 133 | 134 | 135 | class Path3d: 136 | def __init__(self, ts, waypoints): 137 | wps = np.array(waypoints) 138 | self.x = SmoothPath4Poly2Min(wps[:, 0], ts, 0.0, 0.0) 139 | self.y = SmoothPath4Poly2Min(wps[:, 1], ts, 0.0, 0.0) 140 | self.z = SmoothPath4Poly2Min(wps[:, 2], ts, 0.0, 0.0) 141 | 142 | def calc_y_and_y_dot(self, t): 143 | pt = self.x.pick_pt(t) 144 | return \ 145 | np.array([self.x.calc_pt_y(pt), self.y.calc_pt_y(pt), self.z.calc_pt_y(pt)]), \ 146 | np.array([self.x.calc_pt_y_dot(pt), self.y.calc_pt_y_dot(pt), self.z.calc_pt_y_dot(pt)]) 147 | -------------------------------------------------------------------------------- /src/util.py: -------------------------------------------------------------------------------- 1 | import time 2 | 3 | from builtin_interfaces.msg import Time 4 | from geometry_msgs.msg import Pose, Transform, TransformStamped, Vector3 5 | 6 | 7 | def now() -> Time: 8 | """Return builtin_interfaces.msg.Time object with the current CPU time""" 9 | cpu_time = time.time() 10 | sec = int(cpu_time) 11 | nanosec = int((cpu_time - sec) * 1.e9) 12 | return Time(sec=sec, nanosec=nanosec) 13 | 14 | 15 | def duration(t1: Time, t2: Time) -> float: 16 | """Compute duration from t1 to t2, return seconds""" 17 | return float(t2.sec - t1.sec) + float(t2.nanosec - t1.nanosec) / 1.e9 18 | 19 | 20 | def copy_pose_to_transform(pose: Pose, transform: Transform): 21 | """Copy fields from geometry_msgs.msg.Pose to a geometry_msgs.msg.Transform""" 22 | transform.translation.x = pose.position.x 23 | transform.translation.y = pose.position.y 24 | transform.translation.z = pose.position.z 25 | transform.rotation.x = pose.orientation.x 26 | transform.rotation.y = pose.orientation.y 27 | transform.rotation.z = pose.orientation.z 28 | transform.rotation.w = pose.orientation.w 29 | 30 | 31 | def copy_pose_to_pose(pose1: Pose, pose2: Pose): 32 | """Copy fields from one geometry_msgs.msg.Pose to another""" 33 | pose2.position.x = pose1.position.x 34 | pose2.position.y = pose1.position.y 35 | pose2.position.z = pose1.position.z 36 | pose2.orientation.x = pose1.orientation.x 37 | pose2.orientation.y = pose1.orientation.y 38 | pose2.orientation.z = pose1.orientation.z 39 | pose2.orientation.w = pose1.orientation.w 40 | --------------------------------------------------------------------------------