├── .gitignore ├── LICENSE ├── README.md └── swarm_planner ├── CMakeLists.txt ├── config ├── catvehicle.rviz ├── catvehicle_control.yaml ├── map.yaml └── twocar.rviz ├── include ├── ecbs_planner.hpp ├── init_traj_planner.hpp ├── matplotlibcpp.h ├── mission.hpp ├── param.hpp ├── rapidjson │ ├── allocators.h │ ├── cursorstreamwrapper.h │ ├── document.h │ ├── encodedstream.h │ ├── encodings.h │ ├── error │ │ ├── en.h │ │ └── error.h │ ├── filereadstream.h │ ├── filewritestream.h │ ├── fwd.h │ ├── internal │ │ ├── biginteger.h │ │ ├── diyfp.h │ │ ├── dtoa.h │ │ ├── ieee754.h │ │ ├── itoa.h │ │ ├── meta.h │ │ ├── pow10.h │ │ ├── regex.h │ │ ├── stack.h │ │ ├── strfunc.h │ │ ├── strtod.h │ │ └── swap.h │ ├── istreamwrapper.h │ ├── memorybuffer.h │ ├── memorystream.h │ ├── msinttypes │ │ ├── inttypes.h │ │ └── stdint.h │ ├── ostreamwrapper.h │ ├── pointer.h │ ├── prettywriter.h │ ├── rapidjson.h │ ├── reader.h │ ├── schema.h │ ├── stream.h │ ├── stringbuffer.h │ └── writer.h ├── rbp_corridor.hpp ├── rbp_planner.hpp ├── rbp_plotter.hpp ├── rbp_publisher.hpp ├── scp_planner.hpp ├── scp_plotter.hpp ├── scp_publisher.hpp ├── sp_const.hpp └── timer.hpp ├── launch ├── create_octomap_world.launch ├── map_saver.launch ├── plan_rbp_empty_space.launch ├── plan_rbp_random_forest.launch ├── plan_rbp_test.launch ├── plan_scp_empty_space.launch ├── rqtplot_config │ └── rqtplot_config.perspective └── rviz_config │ ├── config_16agents.rviz │ ├── config_1agents.rviz │ ├── config_2agents.rviz │ ├── config_32agents.rviz │ ├── config_4agents.rviz │ ├── config_64agents.rviz │ ├── config_6agents.rviz │ ├── config_8agents.rviz │ └── config_8agents_side.rviz ├── log ├── QPmodel.lp ├── coef1.csv ├── coef10.csv ├── coef11.csv ├── coef12.csv ├── coef13.csv ├── coef14.csv ├── coef15.csv ├── coef16.csv ├── coef17.csv ├── coef18.csv ├── coef19.csv ├── coef2.csv ├── coef20.csv ├── coef21.csv ├── coef22.csv ├── coef23.csv ├── coef24.csv ├── coef25.csv ├── coef26.csv ├── coef27.csv ├── coef28.csv ├── coef29.csv ├── coef3.csv ├── coef30.csv ├── coef31.csv ├── coef32.csv ├── coef33.csv ├── coef34.csv ├── coef35.csv ├── coef36.csv ├── coef37.csv ├── coef38.csv ├── coef39.csv ├── coef4.csv ├── coef40.csv ├── coef41.csv ├── coef42.csv ├── coef43.csv ├── coef44.csv ├── coef45.csv ├── coef46.csv ├── coef47.csv ├── coef48.csv ├── coef49.csv ├── coef5.csv ├── coef50.csv ├── coef51.csv ├── coef52.csv ├── coef53.csv ├── coef54.csv ├── coef55.csv ├── coef56.csv ├── coef57.csv ├── coef58.csv ├── coef59.csv ├── coef6.csv ├── coef60.csv ├── coef61.csv ├── coef62.csv ├── coef63.csv ├── coef64.csv ├── coef7.csv ├── coef8.csv └── coef9.csv ├── missions ├── mission_16agents_15.json ├── mission_16agents_20.json ├── mission_16agents_25.json ├── mission_16agents_30.json ├── mission_16agents_35.json ├── mission_2agents_25.json ├── mission_2agents_50.json ├── mission_32agents_12.json ├── mission_32agents_15.json ├── mission_4agents_15.json ├── mission_4agents_15_2.json ├── mission_4agents_25_aty.json ├── mission_4agents_35_aty.json ├── mission_64agents_12.json ├── mission_64agents_15.json ├── mission_64agents_20.json ├── mission_8agents_10.json ├── mission_8agents_12.json ├── mission_8agents_120.json ├── mission_8agents_15.json ├── mission_8agents_30.json └── mission_8agents_60.json ├── package.xml ├── setting ├── src ├── random_map_generator.cpp ├── swarm_traj_planner_rbp.cpp ├── swarm_traj_planner_rbp_flat.cpp ├── swarm_traj_planner_rbp_test_all.cpp └── swarm_traj_planner_scp.cpp ├── third_party └── ecbs │ ├── .clang-format │ ├── .clang-tidy │ ├── .gitignore │ ├── CMakeLists.txt │ ├── LICENSE │ ├── doc │ ├── Doxyfile.in │ └── libMultiRobotPlanning.md │ ├── include │ ├── .a_star_epsilon.hpp.swp │ ├── .ecbs.hpp.swp │ ├── a_star.hpp │ ├── a_star_epsilon.hpp │ ├── cbs.hpp │ ├── ecbs.hpp │ ├── environment.hpp │ ├── neighbor.hpp │ └── planresult.hpp │ ├── package.xml │ └── src │ ├── a_star.cpp │ ├── a_star_epsilon.cpp │ ├── cbs.cpp │ └── ecbs.cpp └── worlds ├── ICRA2020_64agents_presentation.bt ├── IROS2019.bt ├── IROS2019_1.bt ├── IROS2019_2.bt ├── empty.bt ├── map1.bt ├── map10.bt ├── map11.bt ├── map12.bt ├── map13.bt ├── map14.bt ├── map15.bt ├── map16.bt ├── map17.bt ├── map18.bt ├── map19.bt ├── map2.bt ├── map20.bt ├── map21.bt ├── map22.bt ├── map23.bt ├── map24.bt ├── map25.bt ├── map26.bt ├── map27.bt ├── map28.bt ├── map29.bt ├── map3.bt ├── map30.bt ├── map31.bt ├── map32.bt ├── map33.bt ├── map34.bt ├── map35.bt ├── map36.bt ├── map37.bt ├── map38.bt ├── map39.bt ├── map4.bt ├── map40.bt ├── map41.bt ├── map42.bt ├── map43.bt ├── map44.bt ├── map45.bt ├── map46.bt ├── map47.bt ├── map48.bt ├── map49.bt ├── map5.bt ├── map50.bt ├── map6.bt ├── map7.bt ├── map8.bt ├── map9.bt └── map_reduced_tmp3.bt /.gitignore: -------------------------------------------------------------------------------- 1 | swarm_planner/cmake-build-debug/ 2 | swarm_planner/.idea/ 3 | html/ 4 | latex/ 5 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2020 Jungwon Park 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # swarm_simulator 2 | 3 | This package presents an efficient multi-agent trajectory planning algorithm which generates safe trajectories in obstacle-dense environments. 4 | Our algorithm combines the advantages of both grid-based and optimization-based approaches, and generates safe, dynamically feasible trajectory without suffering from an errorneous optimization setup such as imposing infeasible collision constraints. 5 | The details can be found at the following link. 6 | 7 | - **Authors:** Jungwon Park, Junha Kim, Inkyu Jang and H. Jin Kim from [LARR](http://larr.snu.ac.kr/), Seoul National Univ. 8 | - **Paper:** Efficient Multi-Agent Trajectory Planning with Feasibility Guarantee using Relative Bernstein Polynomial [PDF Link](https://arxiv.org/abs/1909.10219) 9 | - **Video:** [Youtube Link](https://www.youtube.com/watch?v=0koj-AlIbbI&list=PLdzwkGI22JhXa63sRb8zPTK3ZNFRmkdIu&index=2&t=0s) 10 | 11 | ## 0. Dependencies 12 | Following sources are used to implement this package. 13 | - [CPLEX](https://www.ibm.com/products/ilog-cplex-optimization-studio/resources) 14 | - [octomap](https://github.com/OctoMap/octomap) 15 | - [libMultiRobotPlanning](https://github.com/whoenig/libMultiRobotPlanning) 16 | - [matplotlib-cpp](https://github.com/lava/matplotlib-cpp) 17 | 18 | ## 1. Install 19 | (1) Install ROS Kinetic (for Ubuntu 16.04) or Melodic (for Ubuntu 18.04). 20 | 21 | (2) Install CPLEX and fix CMAKELIST depending on intallation location. For instance: 22 | ``` 23 | set(CPLEX_PREFIX_DIR /opt/ibm/ILOG/CPLEX_Studio129) 24 | ``` 25 | 26 | (3) At terminal: 27 | ``` 28 | sudo apt-get install ros--octomap 29 | sudo apt-get install ros--octomap-ros 30 | sudo apt-get install ros--dynamic-edt-3d 31 | sudo apt-get install python-matplotlib python-numpy python2.7-dev 32 | cd ~/catkin_ws/src 33 | git clone https://github.com/qwerty35/swarm_simulator.git 34 | cd ../ 35 | catkin_make 36 | source ~/catkin_ws/devel/setup.bash 37 | ``` 38 | (`````` is ```kinetic``` or ```melodic``` depending on your ROS version.) 39 | 40 | ## 2. Demo 41 | ``` 42 | roslaunch swarm_planner plan_rbp_random_forest.launch 43 | ``` 44 | 45 | ## 3. Simulation Configuration 46 | You can configure the simulation setting at the launch file, plan_rbp_random_forest.launch. 47 | 48 | - runsim: You can see the planning result at the rviz. 49 | 50 | - log: You can see more detail message, and QPmodel and planning results will be saved in "swarm_planner/log" 51 | 52 | - mission: You can deploy the mission by editing the json file in "swarm_planner/missions" directory. 53 | 54 | - replay: 55 | * false: It runs the simulation at the random forest. 56 | * true: It runs the simulation at the map specified at 'replay_map' tag. 57 | * Map files are located in "swarm_planner/worlds", and should be octomap bt files. 58 | 59 | - plan_time_scale: Execute time scale to match dynamic limits specified at mission file. 60 | 61 | - plan_time_step: You can execute time scale with this tag manually unless plan_time_step is true 62 | 63 | - plan_sequential: Execute seqeuntial planning. You can change the batch size at 'plan_batch_size' tag. 64 | 65 | - plan_batch_iter: 66 | * positive number: You can see the intermediate result of seqential planning. 67 | * 0: it shows initial trajectory. 68 | * -1: it means maximum batch iteration. 69 | 70 | 71 | ## 3. Notes 72 | (1) You may turn off 'runsim', 'log' arguments to check the correct computation time. 73 | (2) This code is not fully tested with various maps. Grid generation algorithm may not work with other maps. 74 | -------------------------------------------------------------------------------- /swarm_planner/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(swarm_planner) 3 | 4 | #set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 5 | #set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pg -g3 -O3 -m64 -Ofast -flto -msse2 -msse3 -march=native -mfpmath=sse") 6 | 7 | set(CMAKE_CXX_STANDARD 14) 8 | 9 | include_directories(${PROJECT_SOURCE_DIR}/include) 10 | #set(CMAKE_BUILD_TYPE Debug) 11 | set(CMAKE_BUILD_TYPE Release) 12 | 13 | #ECBS 14 | add_subdirectory(${PROJECT_SOURCE_DIR}/third_party/ecbs) 15 | include_directories(${PROJECT_SOURCE_DIR}/third_party/ecbs/include) 16 | 17 | #BOOST14 18 | find_package(Boost 1.58 REQUIRED COMPONENTS program_options) 19 | 20 | #EIGEN 21 | set(Eigen3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) 22 | 23 | #PCL 24 | find_package(PCL REQUIRED) 25 | include_directories(${PCL_INCLUDE_DIRS}) 26 | 27 | #OCTOMAP 28 | find_package(octomap REQUIRED) 29 | include_directories(${OCTOMAP_INCLUDE_DIRS}) 30 | link_libraries(${OCTOMAP_LIBRARIES}) 31 | add_definitions(-DOCTOMAP_NODEBUGOUT) 32 | 33 | #EDT 34 | find_package(dynamicEDT3D REQUIRED) 35 | include_directories(${DYNAMICEDT3D_INCLUDE_DIRS}) 36 | link_libraries(${DYNAMICEDT3D_LIBRARIES}) 37 | 38 | #CPLEX 39 | add_definitions(-DNDEBUG) 40 | add_definitions(-DIL_STD) 41 | set(CPLEX_PREFIX_DIR /opt/ibm/ILOG/CPLEX_Studio1210) 42 | set(CPLEX_INCLUDE_DIR ${CPLEX_PREFIX_DIR}/cplex/include) 43 | set(CPLEX_LIBRARIES_DIR ${CPLEX_PREFIX_DIR}/cplex/lib/x86-64_linux/static_pic) 44 | set(CONCERT_INCLUDE_DIR ${CPLEX_PREFIX_DIR}/concert/include) 45 | set(CONCERT_LIBRARIES_DIR ${CPLEX_PREFIX_DIR}/concert/lib/x86-64_linux/static_pic) 46 | include_directories(${CPLEX_INCLUDE_DIR} ${CONCERT_INCLUDE_DIR}) 47 | link_directories(${CPLEX_LIBRARIES_DIR} ${CONCERT_LIBRARIES_DIR}) 48 | 49 | #CATKIN 50 | find_package(catkin REQUIRED COMPONENTS 51 | roscpp 52 | roslib 53 | std_msgs 54 | nav_msgs 55 | geometry_msgs 56 | octomap_ros 57 | octomap_msgs 58 | ) 59 | catkin_package( 60 | INCLUDE_DIRS include 61 | CATKIN_DEPENDS roscpp roslib message_runtime 62 | ) 63 | 64 | #MATPLOTLIB-CPP 65 | find_package(PythonLibs 2.7) 66 | find_package(Threads) 67 | 68 | #BUILD 69 | add_executable(random_map_generator src/random_map_generator.cpp) 70 | target_link_libraries(random_map_generator 71 | ${catkin_LIBRARIES} 72 | ${PCL_LIBRARIES} 73 | ) 74 | 75 | add_executable(swarm_traj_planner_rbp src/swarm_traj_planner_rbp.cpp) 76 | target_include_directories(swarm_traj_planner_rbp PRIVATE ${PYTHON_INCLUDE_DIRS}) 77 | target_link_libraries(swarm_traj_planner_rbp 78 | ${catkin_LIBRARIES} 79 | ${OCTOMAP_INCLUDE_DIRS} 80 | ${SIPP_LINK_LIBS} 81 | ${PYTHON_LIBRARIES} 82 | m 83 | ilocplex 84 | cplex 85 | concert 86 | pthread 87 | dl 88 | ) 89 | 90 | add_executable(swarm_traj_planner_rbp_test_all src/swarm_traj_planner_rbp_test_all.cpp) 91 | target_include_directories(swarm_traj_planner_rbp_test_all PRIVATE ${PYTHON_INCLUDE_DIRS}) 92 | target_link_libraries(swarm_traj_planner_rbp_test_all 93 | ${catkin_LIBRARIES} 94 | ${OCTOMAP_INCLUDE_DIRS} 95 | ${SIPP_LINK_LIBS} 96 | ${PYTHON_LIBRARIES} 97 | m 98 | ilocplex 99 | cplex 100 | concert 101 | pthread 102 | dl 103 | ) 104 | 105 | add_executable(swarm_traj_planner_scp src/swarm_traj_planner_scp.cpp) 106 | target_include_directories(swarm_traj_planner_scp PRIVATE ${PYTHON_INCLUDE_DIRS}) 107 | target_link_libraries(swarm_traj_planner_scp 108 | ${catkin_LIBRARIES} 109 | ${PYTHON_LIBRARIES} 110 | m 111 | ilocplex 112 | cplex 113 | concert 114 | pthread 115 | dl 116 | ) 117 | 118 | add_executable(swarm_traj_planner_rbp_flat src/swarm_traj_planner_rbp_flat.cpp) 119 | target_include_directories(swarm_traj_planner_rbp_flat PRIVATE ${PYTHON_INCLUDE_DIRS}) 120 | target_link_libraries(swarm_traj_planner_rbp_flat 121 | ${catkin_LIBRARIES} 122 | ${OCTOMAP_INCLUDE_DIRS} 123 | ${SIPP_LINK_LIBS} 124 | ${PYTHON_LIBRARIES} 125 | m 126 | ilocplex 127 | cplex 128 | concert 129 | pthread 130 | dl 131 | ) 132 | -------------------------------------------------------------------------------- /swarm_planner/config/catvehicle_control.yaml: -------------------------------------------------------------------------------- 1 | # Author: Jonathan Sprinkle 2 | # Copyright (c) 2015 Arizona Board of Regents 3 | # All rights reserved. 4 | # 5 | # Permission is hereby granted, without written agreement and without 6 | # license or royalty fees, to use, copy, modify, and distribute this 7 | # software and its documentation for any purpose, provided that the 8 | # above copyright notice and the following two paragraphs appear in 9 | # all copies of this software. 10 | # 11 | # IN NO EVENT SHALL THE ARIZONA BOARD OF REGENTS BE LIABLE TO ANY PARTY 12 | # FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES 13 | # ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN 14 | # IF THE ARIZONA BOARD OF REGENTS HAS BEEN ADVISED OF THE POSSIBILITY OF 15 | # SUCH DAMAGE. 16 | # 17 | # THE ARIZONA BOARD OF REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, 18 | # INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY 19 | # AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER 20 | # IS ON AN "AS IS" BASIS, AND THE ARIZONA BOARD OF REGENTS HAS NO OBLIGATION 21 | # TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. 22 | 23 | # Summary: 24 | # This YAML file includes the controller connectivity for ROS-based control 25 | # through Gazebo. For more information and for the tutorials used to create 26 | # this file, see 27 | # http://gazebosim.org/tutorials/?tut=ros_control#Prerequisites 28 | 29 | # controls the rear two tires based on individual motors 30 | # Publish all joint states ----------------------------------- 31 | joint_state_controller: 32 | type: joint_state_controller/JointStateController 33 | publish_rate: 50 34 | 35 | # Velocity Controllers --------------------------------------- 36 | 37 | # The left/right rear wheels are what moves the car. This 38 | # controller set uses PID for velocity set points 39 | joint1_velocity_controller: 40 | type: velocity_controllers/JointVelocityController 41 | joint: back_left_wheel_joint 42 | pid: {p: 100.0, i: 0.01, d: 10.0} 43 | 44 | joint2_velocity_controller: 45 | type: velocity_controllers/JointVelocityController 46 | joint: back_right_wheel_joint 47 | pid: {p: 100.0, i: 0.01, d: 10.0} 48 | 49 | # we are no longer using these controllers for angular rate 50 | # but we may reinstate them later 51 | #front_left_steering_controller: 52 | # type: velocity_controllers/JointVelocityController 53 | # joint: front_left_steering_joint 54 | # pid: {p: 100.0, i: 0.01, d: 10.0} 55 | #front_right_steering_controller: 56 | # type: velocity_controllers/JointVelocityController 57 | # joint: front_right_steering_joint 58 | # pid: {p: 100.0, i: 0.01, d: 10.0} 59 | 60 | # permits a set point for the steering (tire) angle, which 61 | # is enforced by these position controllers. The value is 62 | # set based on the dynamics of the controller, not an 63 | # instantaneous set for the position 64 | front_left_steering_position_controller: 65 | type: position_controllers/JointPositionController 66 | joint: front_left_steering_joint 67 | pid: {p: 100.0, i: 0.01, d: 10.0} 68 | front_right_steering_position_controller: 69 | type: position_controllers/JointPositionController 70 | joint: front_right_steering_joint 71 | pid: {p: 100.0, i: 0.01, d: 10.0} 72 | 73 | #gazebo_ros_control: 74 | # pid_gains: 75 | # back_left_wheel_joint: 76 | # p: 100.0 77 | # i: 0.01 78 | # d: 10.0 79 | # back_right_wheel_joint: 80 | # p: 100.0 81 | # i: 0.01 82 | # d: 10.0 83 | # front_left_steering_joint: 84 | # p: 100.0 85 | # i: 0.01 86 | # d: 10.0 87 | # front_right_steering_joint: 88 | # p: 100.0 89 | # i: 0.01 90 | # d: 10.0 91 | -------------------------------------------------------------------------------- /swarm_planner/config/map.yaml: -------------------------------------------------------------------------------- 1 | image: testmap.png 2 | resolution: 0.1 3 | origin: [0.0, 0.0, 0.0] 4 | occupied_thresh: 0.65 5 | free_thresh: 0.196 6 | negate: 0 7 | -------------------------------------------------------------------------------- /swarm_planner/include/ecbs_planner.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "init_traj_planner.hpp" 4 | #include 5 | 6 | using namespace libMultiRobotPlanning; 7 | 8 | namespace SwarmPlanning { 9 | class ECBSPlanner : public InitTrajPlanner { 10 | public: 11 | ECBSPlanner(std::shared_ptr _distmap_obj, 12 | Mission _mission, 13 | Param _param) 14 | : InitTrajPlanner(std::move(_distmap_obj), 15 | std::move(_mission), 16 | std::move(_param)) { 17 | setObstacles(); 18 | setWaypoints(); 19 | } 20 | 21 | bool update(bool log, SwarmPlanning::PlanResult* planResult_ptr) override { 22 | Environment mapf(dimx, dimy, dimz, ecbs_obstacles, ecbs_goalLocations, mission.quad_size, 23 | param.grid_xy_res); 24 | ECBS ecbs(mapf, param.ecbs_w); 25 | std::vector> solution; 26 | 27 | // Execute ECBS algorithm 28 | bool success = ecbs.search(ecbs_startStates, solution, param.log); 29 | if (!success) { 30 | ROS_ERROR("ECBSPlanner: ECBS Failed!"); 31 | return false; 32 | } 33 | 34 | // Update segment time T 35 | int cost = 0; 36 | int makespan = 0; 37 | for (const auto &s : solution) { 38 | cost += s.cost; 39 | makespan = std::max(makespan, s.cost); 40 | } 41 | for (int i = 0; i <= makespan + 2; i++) { 42 | planResult_ptr->T.emplace_back(i * param.time_step); 43 | } 44 | if (log) { 45 | ROS_INFO_STREAM("ECBSPlanner: M=" << planResult_ptr->T.size() - 1); 46 | ROS_INFO_STREAM("ECBSPlanner: makespan=" << planResult_ptr->T.back()); 47 | } 48 | 49 | planResult_ptr->initTraj.resize(solution.size()); 50 | for (size_t a = 0; a < solution.size(); ++a) { 51 | // Append start, goal points to both ends of initial trajectory respectively 52 | planResult_ptr->initTraj[a].emplace_back(octomap::point3d(mission.startState[a][0], 53 | mission.startState[a][1], 54 | mission.startState[a][2])); 55 | 56 | for (const auto &state : solution[a].states) { 57 | planResult_ptr->initTraj[a].emplace_back( 58 | octomap::point3d(state.first.x * param.grid_xy_res + grid_x_min, 59 | state.first.y * param.grid_xy_res + grid_y_min, 60 | state.first.z * param.grid_z_res + grid_z_min) 61 | ); 62 | } 63 | 64 | // The length of the initial trajectories should be equal 65 | while (planResult_ptr->initTraj[a].size() <= makespan + 2) { 66 | planResult_ptr->initTraj[a].emplace_back(octomap::point3d(mission.goalState[a][0], 67 | mission.goalState[a][1], 68 | mission.goalState[a][2])); 69 | } 70 | } 71 | return true; 72 | } 73 | 74 | private: 75 | std::unordered_set ecbs_obstacles; 76 | std::vector ecbs_startStates; 77 | std::vector ecbs_goalLocations; 78 | 79 | // Find the location of obstacles in grid-space 80 | bool setObstacles() { 81 | double r = 0; 82 | for (int qi = 0; qi < mission.qn; qi++) { 83 | if (r < mission.quad_size[qi]) { 84 | r = mission.quad_size[qi]; 85 | } 86 | } 87 | 88 | int x, y, z; 89 | for (double k = grid_z_min; k < grid_z_max + SP_EPSILON; k += param.grid_z_res) { 90 | for (double i = grid_x_min; i < grid_x_max + SP_EPSILON; i += param.grid_xy_res) { 91 | for (double j = grid_y_min; j < grid_y_max + SP_EPSILON; j += param.grid_xy_res) { 92 | octomap::point3d cur_point(i, j, k); 93 | float dist = distmap_obj.get()->getDistance(cur_point); 94 | if (dist < 0) { 95 | return false; 96 | } 97 | 98 | // To prevent obstacles from putting between grid points, grid_margin is used 99 | if (dist < r + param.grid_margin) { 100 | x = (int) round((i - grid_x_min) / param.grid_xy_res); 101 | y = (int) round((j - grid_y_min) / param.grid_xy_res); 102 | z = (int) round((k - grid_z_min) / param.grid_z_res); 103 | ecbs_obstacles.insert(Location(x, y, z)); 104 | } 105 | } 106 | } 107 | } 108 | return true; 109 | } 110 | 111 | // Set start, goal points of ECBS 112 | bool setWaypoints() { 113 | int xig, yig, zig, xfg, yfg, zfg; 114 | for (int i = 0; i < mission.qn; i++) { 115 | // For start, goal point of ECBS, we use the nearest grid point. 116 | xig = (int) round((mission.startState[i][0] - grid_x_min) / param.grid_xy_res); 117 | yig = (int) round((mission.startState[i][1] - grid_y_min) / param.grid_xy_res); 118 | zig = (int) round((mission.startState[i][2] - grid_z_min) / param.grid_z_res); 119 | xfg = (int) round((mission.goalState[i][0] - grid_x_min) / param.grid_xy_res); 120 | yfg = (int) round((mission.goalState[i][1] - grid_y_min) / param.grid_xy_res); 121 | zfg = (int) round((mission.goalState[i][2] - grid_z_min) / param.grid_z_res); 122 | 123 | if (ecbs_obstacles.find(Location(xig, yig, zig)) != ecbs_obstacles.end()) { 124 | ROS_ERROR_STREAM("ECBSPlanner: start of agent " << i << " is occluded by obstacle"); 125 | return false; 126 | } 127 | if (ecbs_obstacles.find(Location(xfg, yfg, zfg)) != ecbs_obstacles.end()) { 128 | ROS_ERROR_STREAM("ECBSPlanner: goal of agent " << i << " is occluded by obstacle"); 129 | return false; 130 | } 131 | 132 | ecbs_startStates.emplace_back(State(0, xig, yig, zig)); 133 | ecbs_goalLocations.emplace_back(Location(xfg, yfg, zfg)); 134 | } 135 | return true; 136 | } 137 | }; 138 | } -------------------------------------------------------------------------------- /swarm_planner/include/init_traj_planner.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | namespace SwarmPlanning { 9 | class InitTrajPlanner { 10 | public: 11 | virtual bool update(bool log, SwarmPlanning::PlanResult* planResult_ptr) = 0; 12 | 13 | InitTrajPlanner(std::shared_ptr _distmap_obj, 14 | SwarmPlanning::Mission _mission, 15 | SwarmPlanning::Param _param) 16 | : distmap_obj(std::move(_distmap_obj)), 17 | mission(std::move(_mission)), 18 | param(std::move(_param)) { 19 | grid_x_min = ceil((param.world_x_min - SP_EPSILON) / param.grid_xy_res) * param.grid_xy_res; 20 | grid_y_min = ceil((param.world_y_min - SP_EPSILON) / param.grid_xy_res) * param.grid_xy_res; 21 | grid_z_min = ceil((param.world_z_min - SP_EPSILON) / param.grid_z_res) * param.grid_z_res; 22 | 23 | grid_x_max = floor((param.world_x_max + SP_EPSILON) / param.grid_xy_res) * param.grid_xy_res; 24 | grid_y_max = floor((param.world_y_max + SP_EPSILON) / param.grid_xy_res) * param.grid_xy_res; 25 | grid_z_max = floor((param.world_z_max + SP_EPSILON) / param.grid_z_res) * param.grid_z_res; 26 | 27 | dimx = (int) round((grid_x_max - grid_x_min) / param.grid_xy_res) + 1; 28 | dimy = (int) round((grid_y_max - grid_y_min) / param.grid_xy_res) + 1; 29 | dimz = (int) round((grid_z_max - grid_z_min) / param.grid_z_res) + 1; 30 | } 31 | 32 | protected: 33 | std::shared_ptr distmap_obj; 34 | SwarmPlanning::Mission mission; 35 | SwarmPlanning::Param param; 36 | 37 | double grid_x_min, grid_y_min, grid_z_min, grid_x_max, grid_y_max, grid_z_max; 38 | int dimx, dimy, dimz; 39 | }; 40 | } -------------------------------------------------------------------------------- /swarm_planner/include/mission.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | using namespace rapidjson; 9 | 10 | namespace SwarmPlanning { 11 | class Mission { 12 | public: 13 | int qn; // the number of quadrotors 14 | std::vector> startState, goalState, max_vel, max_acc; 15 | std::vector quad_size, quad_speed; 16 | 17 | bool setMission(const ros::NodeHandle &nh); 18 | void applyNoise(double max_noise); 19 | }; 20 | 21 | 22 | bool Mission::setMission(const ros::NodeHandle &nh) { 23 | std::string mission_addr; 24 | nh.param("mission", mission_addr, "demo"); 25 | 26 | std::ifstream ifs(mission_addr); 27 | IStreamWrapper isw(ifs); 28 | Document document; 29 | if(document.ParseStream(isw).HasParseError()){ 30 | ROS_ERROR_STREAM("There is no such mission file " << mission_addr << "\n"); 31 | return false; 32 | } 33 | 34 | const Value& agents = document["agents"]; 35 | qn = agents.Size(); 36 | 37 | startState.resize(qn); 38 | goalState.resize(qn); 39 | quad_size.resize(qn); 40 | quad_speed.resize(qn); 41 | max_vel.resize(qn); 42 | max_acc.resize(qn); 43 | 44 | for(SizeType qi = 0; qi < agents.Size(); qi++){ 45 | // name 46 | std::string name = agents[qi].GetObject()["name"].GetString(); 47 | 48 | // start 49 | std::vector state(9, 0); 50 | const Value& start = agents[qi].GetObject()["start"]; 51 | for(SizeType i = 0; i < start.Size(); i++){ 52 | state[i] = start[i].GetDouble(); 53 | } 54 | startState[qi] = state; 55 | 56 | // goal 57 | state.assign(9, 0); 58 | const Value& goal = agents[qi].GetObject()["goal"]; 59 | for(SizeType i = 0; i < goal.Size(); i++){ 60 | state[i] = goal[i].GetDouble(); 61 | } 62 | goalState[qi] = state; 63 | 64 | // radius 65 | quad_size[qi] = agents[qi].GetObject()["radius"].GetDouble(); 66 | 67 | // speed 68 | quad_speed[qi] = agents[qi].GetObject()["speed"].GetDouble(); 69 | 70 | // maximum velocity, acceleration 71 | std::vector dynamic_limit(3, 0); 72 | Value::MemberIterator quadrotor = document["quadrotors"].FindMember(name.c_str()); 73 | const Value& maxVel = quadrotor->value.GetObject()["max_vel"]; 74 | for(SizeType i = 0; i < maxVel.Size(); i++){ 75 | dynamic_limit[i] = maxVel[i].GetDouble(); 76 | } 77 | max_vel[qi] = dynamic_limit; 78 | 79 | dynamic_limit.assign(3, 0); 80 | const Value& maxAcc = quadrotor->value.GetObject()["max_acc"]; 81 | for(SizeType i = 0; i < maxAcc.Size(); i++){ 82 | dynamic_limit[i] = maxAcc[i].GetDouble(); 83 | } 84 | max_acc[qi] = dynamic_limit; 85 | } 86 | 87 | return true; 88 | } 89 | 90 | void Mission::applyNoise(double max_noise){ 91 | srand((unsigned int)time(nullptr)); 92 | for(int qi = 0; qi < qn; qi++) { 93 | for(int k = 0; k < 3; k++){ 94 | startState[qi][k] += rand()/(double)RAND_MAX * max_noise; 95 | goalState[qi][k] += rand()/(double)RAND_MAX * max_noise; 96 | } 97 | } 98 | } 99 | } 100 | -------------------------------------------------------------------------------- /swarm_planner/include/param.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | namespace SwarmPlanning{ 8 | class Param { 9 | public: 10 | bool log; 11 | std::string package_path; 12 | 13 | double world_x_min; 14 | double world_y_min; 15 | double world_z_min; 16 | double world_x_max; 17 | double world_y_max; 18 | double world_z_max; 19 | 20 | double ecbs_w; 21 | double grid_xy_res; 22 | double grid_z_res; 23 | double grid_margin; 24 | 25 | double box_xy_res; 26 | double box_z_res; 27 | 28 | bool time_scale; 29 | double time_step; 30 | double downwash; // downwash coefficient 31 | int iteration; 32 | bool sequential; 33 | int batch_size; // the number of agents in a batch 34 | int batch_iter; // the number of batches 35 | int n; // degree of polynomial 36 | int phi; // desired derivatives 37 | 38 | std::vector> color; 39 | 40 | bool setROSParam(const ros::NodeHandle &nh); 41 | void setColor(int qn); 42 | }; 43 | 44 | bool Param::setROSParam(const ros::NodeHandle &nh) { 45 | nh.param("log", log, false); 46 | 47 | nh.param("world/x_min", world_x_min, -5); 48 | nh.param("world/y_min", world_y_min, -5); 49 | nh.param("world/z_min", world_z_min, 0); 50 | nh.param("world/x_max", world_x_max, 5); 51 | nh.param("world/y_max", world_y_max, 5); 52 | nh.param("world/z_max", world_z_max, 2.5); 53 | 54 | nh.param("grid/xy_res", grid_xy_res, 0.3); 55 | nh.param("grid/z_res", grid_z_res, 0.6); 56 | nh.param("grid/margin", grid_margin, 0.2); 57 | nh.param("ecbs/w", ecbs_w, 1.3); 58 | 59 | nh.param("box/xy_res", box_xy_res, 0.1); 60 | nh.param("box/z_res", box_z_res, 0.1); 61 | 62 | nh.param("plan/time_scale", time_scale, true); 63 | nh.param("plan/time_step", time_step, 1); 64 | nh.param("plan/downwash", downwash, 2.0); 65 | nh.param("plan/n", n, 5); 66 | nh.param("plan/phi", phi, 3); 67 | nh.param("plan/sequential", sequential, false); 68 | nh.param("plan/batch_size", batch_size, 4); 69 | nh.param("plan/batch_iter", batch_iter, 0); 70 | nh.param("plan/iteration", iteration, 1); 71 | 72 | package_path = ros::package::getPath("swarm_planner"); 73 | 74 | return true; 75 | } 76 | 77 | // HSV Colormap 78 | void Param::setColor(int qn){ 79 | double h,f; 80 | int i; 81 | 82 | color.resize(qn); 83 | for(int qi = 0; qi < qn; qi++){ 84 | color[qi].resize(3); 85 | h = qi * 6 / (double)qn; 86 | i = (int)h; 87 | f = h-i; 88 | 89 | switch(i){ 90 | case 0: 91 | color[qi][0] = 1; 92 | color[qi][1] = f; 93 | color[qi][2] = 0; 94 | break; 95 | case 1: 96 | color[qi][0] = 1-f; 97 | color[qi][1] = 1; 98 | color[qi][2] = 0; 99 | break; 100 | case 2: 101 | color[qi][0] = 0; 102 | color[qi][1] = 1; 103 | color[qi][2] = f; 104 | break; 105 | case 3: 106 | color[qi][0] = 0; 107 | color[qi][1] = 1-f; 108 | color[qi][2] = 1; 109 | break; 110 | case 4: 111 | color[qi][0]= f; 112 | color[qi][1]= 0; 113 | color[qi][2]= 1; 114 | break; 115 | case 5: 116 | color[qi][0]= 1; 117 | color[qi][1]= 0; 118 | color[qi][2]= 1-f; 119 | break; 120 | default: 121 | break; 122 | } 123 | } 124 | } 125 | } 126 | -------------------------------------------------------------------------------- /swarm_planner/include/rapidjson/cursorstreamwrapper.h: -------------------------------------------------------------------------------- 1 | // Tencent is pleased to support the open source community by making RapidJSON available. 2 | // 3 | // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved. 4 | // 5 | // Licensed under the MIT License (the "License"); you may not use this file except 6 | // in compliance with the License. You may obtain a copy of the License at 7 | // 8 | // http://opensource.org/licenses/MIT 9 | // 10 | // Unless required by applicable law or agreed to in writing, software distributed 11 | // under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR 12 | // CONDITIONS OF ANY KIND, either express or implied. See the License for the 13 | // specific language governing permissions and limitations under the License. 14 | 15 | #ifndef RAPIDJSON_CURSORSTREAMWRAPPER_H_ 16 | #define RAPIDJSON_CURSORSTREAMWRAPPER_H_ 17 | 18 | #include "stream.h" 19 | 20 | #if defined(__GNUC__) 21 | RAPIDJSON_DIAG_PUSH 22 | RAPIDJSON_DIAG_OFF(effc++) 23 | #endif 24 | 25 | #if defined(_MSC_VER) && _MSC_VER <= 1800 26 | RAPIDJSON_DIAG_PUSH 27 | RAPIDJSON_DIAG_OFF(4702) // unreachable code 28 | RAPIDJSON_DIAG_OFF(4512) // assignment operator could not be generated 29 | #endif 30 | 31 | RAPIDJSON_NAMESPACE_BEGIN 32 | 33 | 34 | //! Cursor stream wrapper for counting line and column number if error exists. 35 | /*! 36 | \tparam InputStream Any stream that implements Stream Concept 37 | */ 38 | template > 39 | class CursorStreamWrapper : public GenericStreamWrapper { 40 | public: 41 | typedef typename Encoding::Ch Ch; 42 | 43 | CursorStreamWrapper(InputStream& is): 44 | GenericStreamWrapper(is), line_(1), col_(0) {} 45 | 46 | // counting line and column number 47 | Ch Take() { 48 | Ch ch = this->is_.Take(); 49 | if(ch == '\n') { 50 | line_ ++; 51 | col_ = 0; 52 | } else { 53 | col_ ++; 54 | } 55 | return ch; 56 | } 57 | 58 | //! Get the error line number, if error exists. 59 | size_t GetLine() const { return line_; } 60 | //! Get the error column number, if error exists. 61 | size_t GetColumn() const { return col_; } 62 | 63 | private: 64 | size_t line_; //!< Current Line 65 | size_t col_; //!< Current Column 66 | }; 67 | 68 | #if defined(_MSC_VER) && _MSC_VER <= 1800 69 | RAPIDJSON_DIAG_POP 70 | #endif 71 | 72 | #if defined(__GNUC__) 73 | RAPIDJSON_DIAG_POP 74 | #endif 75 | 76 | RAPIDJSON_NAMESPACE_END 77 | 78 | #endif // RAPIDJSON_CURSORSTREAMWRAPPER_H_ 79 | -------------------------------------------------------------------------------- /swarm_planner/include/rapidjson/error/en.h: -------------------------------------------------------------------------------- 1 | // Tencent is pleased to support the open source community by making RapidJSON available. 2 | // 3 | // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved. 4 | // 5 | // Licensed under the MIT License (the "License"); you may not use this file except 6 | // in compliance with the License. You may obtain a copy of the License at 7 | // 8 | // http://opensource.org/licenses/MIT 9 | // 10 | // Unless required by applicable law or agreed to in writing, software distributed 11 | // under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR 12 | // CONDITIONS OF ANY KIND, either express or implied. See the License for the 13 | // specific language governing permissions and limitations under the License. 14 | 15 | #ifndef RAPIDJSON_ERROR_EN_H_ 16 | #define RAPIDJSON_ERROR_EN_H_ 17 | 18 | #include "error.h" 19 | 20 | #ifdef __clang__ 21 | RAPIDJSON_DIAG_PUSH 22 | RAPIDJSON_DIAG_OFF(switch-enum) 23 | RAPIDJSON_DIAG_OFF(covered-switch-default) 24 | #endif 25 | 26 | RAPIDJSON_NAMESPACE_BEGIN 27 | 28 | //! Maps error code of parsing into error message. 29 | /*! 30 | \ingroup RAPIDJSON_ERRORS 31 | \param parseErrorCode Error code obtained in parsing. 32 | \return the error message. 33 | \note User can make a copy of this function for localization. 34 | Using switch-case is safer for future modification of error codes. 35 | */ 36 | inline const RAPIDJSON_ERROR_CHARTYPE* GetParseError_En(ParseErrorCode parseErrorCode) { 37 | switch (parseErrorCode) { 38 | case kParseErrorNone: return RAPIDJSON_ERROR_STRING("No error."); 39 | 40 | case kParseErrorDocumentEmpty: return RAPIDJSON_ERROR_STRING("The document is empty."); 41 | case kParseErrorDocumentRootNotSingular: return RAPIDJSON_ERROR_STRING("The document root must not be followed by other values."); 42 | 43 | case kParseErrorValueInvalid: return RAPIDJSON_ERROR_STRING("Invalid value."); 44 | 45 | case kParseErrorObjectMissName: return RAPIDJSON_ERROR_STRING("Missing a name for object member."); 46 | case kParseErrorObjectMissColon: return RAPIDJSON_ERROR_STRING("Missing a colon after a name of object member."); 47 | case kParseErrorObjectMissCommaOrCurlyBracket: return RAPIDJSON_ERROR_STRING("Missing a comma or '}' after an object member."); 48 | 49 | case kParseErrorArrayMissCommaOrSquareBracket: return RAPIDJSON_ERROR_STRING("Missing a comma or ']' after an array element."); 50 | 51 | case kParseErrorStringUnicodeEscapeInvalidHex: return RAPIDJSON_ERROR_STRING("Incorrect hex digit after \\u escape in string."); 52 | case kParseErrorStringUnicodeSurrogateInvalid: return RAPIDJSON_ERROR_STRING("The surrogate pair in string is invalid."); 53 | case kParseErrorStringEscapeInvalid: return RAPIDJSON_ERROR_STRING("Invalid escape character in string."); 54 | case kParseErrorStringMissQuotationMark: return RAPIDJSON_ERROR_STRING("Missing a closing quotation mark in string."); 55 | case kParseErrorStringInvalidEncoding: return RAPIDJSON_ERROR_STRING("Invalid encoding in string."); 56 | 57 | case kParseErrorNumberTooBig: return RAPIDJSON_ERROR_STRING("Number too big to be stored in double."); 58 | case kParseErrorNumberMissFraction: return RAPIDJSON_ERROR_STRING("Miss fraction part in number."); 59 | case kParseErrorNumberMissExponent: return RAPIDJSON_ERROR_STRING("Miss exponent in number."); 60 | 61 | case kParseErrorTermination: return RAPIDJSON_ERROR_STRING("Terminate parsing due to Handler error."); 62 | case kParseErrorUnspecificSyntaxError: return RAPIDJSON_ERROR_STRING("Unspecific syntax error."); 63 | 64 | default: return RAPIDJSON_ERROR_STRING("Unknown error."); 65 | } 66 | } 67 | 68 | RAPIDJSON_NAMESPACE_END 69 | 70 | #ifdef __clang__ 71 | RAPIDJSON_DIAG_POP 72 | #endif 73 | 74 | #endif // RAPIDJSON_ERROR_EN_H_ 75 | -------------------------------------------------------------------------------- /swarm_planner/include/rapidjson/error/error.h: -------------------------------------------------------------------------------- 1 | // Tencent is pleased to support the open source community by making RapidJSON available. 2 | // 3 | // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved. 4 | // 5 | // Licensed under the MIT License (the "License"); you may not use this file except 6 | // in compliance with the License. You may obtain a copy of the License at 7 | // 8 | // http://opensource.org/licenses/MIT 9 | // 10 | // Unless required by applicable law or agreed to in writing, software distributed 11 | // under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR 12 | // CONDITIONS OF ANY KIND, either express or implied. See the License for the 13 | // specific language governing permissions and limitations under the License. 14 | 15 | #ifndef RAPIDJSON_ERROR_ERROR_H_ 16 | #define RAPIDJSON_ERROR_ERROR_H_ 17 | 18 | #include "../rapidjson.h" 19 | 20 | #ifdef __clang__ 21 | RAPIDJSON_DIAG_PUSH 22 | RAPIDJSON_DIAG_OFF(padded) 23 | #endif 24 | 25 | /*! \file error.h */ 26 | 27 | /*! \defgroup RAPIDJSON_ERRORS RapidJSON error handling */ 28 | 29 | /////////////////////////////////////////////////////////////////////////////// 30 | // RAPIDJSON_ERROR_CHARTYPE 31 | 32 | //! Character type of error messages. 33 | /*! \ingroup RAPIDJSON_ERRORS 34 | The default character type is \c char. 35 | On Windows, user can define this macro as \c TCHAR for supporting both 36 | unicode/non-unicode settings. 37 | */ 38 | #ifndef RAPIDJSON_ERROR_CHARTYPE 39 | #define RAPIDJSON_ERROR_CHARTYPE char 40 | #endif 41 | 42 | /////////////////////////////////////////////////////////////////////////////// 43 | // RAPIDJSON_ERROR_STRING 44 | 45 | //! Macro for converting string literial to \ref RAPIDJSON_ERROR_CHARTYPE[]. 46 | /*! \ingroup RAPIDJSON_ERRORS 47 | By default this conversion macro does nothing. 48 | On Windows, user can define this macro as \c _T(x) for supporting both 49 | unicode/non-unicode settings. 50 | */ 51 | #ifndef RAPIDJSON_ERROR_STRING 52 | #define RAPIDJSON_ERROR_STRING(x) x 53 | #endif 54 | 55 | RAPIDJSON_NAMESPACE_BEGIN 56 | 57 | /////////////////////////////////////////////////////////////////////////////// 58 | // ParseErrorCode 59 | 60 | //! Error code of parsing. 61 | /*! \ingroup RAPIDJSON_ERRORS 62 | \see GenericReader::Parse, GenericReader::GetParseErrorCode 63 | */ 64 | enum ParseErrorCode { 65 | kParseErrorNone = 0, //!< No error. 66 | 67 | kParseErrorDocumentEmpty, //!< The document is empty. 68 | kParseErrorDocumentRootNotSingular, //!< The document root must not follow by other values. 69 | 70 | kParseErrorValueInvalid, //!< Invalid value. 71 | 72 | kParseErrorObjectMissName, //!< Missing a name for object member. 73 | kParseErrorObjectMissColon, //!< Missing a colon after a name of object member. 74 | kParseErrorObjectMissCommaOrCurlyBracket, //!< Missing a comma or '}' after an object member. 75 | 76 | kParseErrorArrayMissCommaOrSquareBracket, //!< Missing a comma or ']' after an array element. 77 | 78 | kParseErrorStringUnicodeEscapeInvalidHex, //!< Incorrect hex digit after \\u escape in string. 79 | kParseErrorStringUnicodeSurrogateInvalid, //!< The surrogate pair in string is invalid. 80 | kParseErrorStringEscapeInvalid, //!< Invalid escape character in string. 81 | kParseErrorStringMissQuotationMark, //!< Missing a closing quotation mark in string. 82 | kParseErrorStringInvalidEncoding, //!< Invalid encoding in string. 83 | 84 | kParseErrorNumberTooBig, //!< Number too big to be stored in double. 85 | kParseErrorNumberMissFraction, //!< Miss fraction part in number. 86 | kParseErrorNumberMissExponent, //!< Miss exponent in number. 87 | 88 | kParseErrorTermination, //!< Parsing was terminated. 89 | kParseErrorUnspecificSyntaxError //!< Unspecific syntax error. 90 | }; 91 | 92 | //! Result of parsing (wraps ParseErrorCode) 93 | /*! 94 | \ingroup RAPIDJSON_ERRORS 95 | \code 96 | Document doc; 97 | ParseResult ok = doc.Parse("[42]"); 98 | if (!ok) { 99 | fprintf(stderr, "JSON parse error: %s (%u)", 100 | GetParseError_En(ok.Code()), ok.Offset()); 101 | exit(EXIT_FAILURE); 102 | } 103 | \endcode 104 | \see GenericReader::Parse, GenericDocument::Parse 105 | */ 106 | struct ParseResult { 107 | //!! Unspecified boolean type 108 | typedef bool (ParseResult::*BooleanType)() const; 109 | public: 110 | //! Default constructor, no error. 111 | ParseResult() : code_(kParseErrorNone), offset_(0) {} 112 | //! Constructor to set an error. 113 | ParseResult(ParseErrorCode code, size_t offset) : code_(code), offset_(offset) {} 114 | 115 | //! Get the error code. 116 | ParseErrorCode Code() const { return code_; } 117 | //! Get the error offset, if \ref IsError(), 0 otherwise. 118 | size_t Offset() const { return offset_; } 119 | 120 | //! Explicit conversion to \c bool, returns \c true, iff !\ref IsError(). 121 | operator BooleanType() const { return !IsError() ? &ParseResult::IsError : NULL; } 122 | //! Whether the result is an error. 123 | bool IsError() const { return code_ != kParseErrorNone; } 124 | 125 | bool operator==(const ParseResult& that) const { return code_ == that.code_; } 126 | bool operator==(ParseErrorCode code) const { return code_ == code; } 127 | friend bool operator==(ParseErrorCode code, const ParseResult & err) { return code == err.code_; } 128 | 129 | bool operator!=(const ParseResult& that) const { return !(*this == that); } 130 | bool operator!=(ParseErrorCode code) const { return !(*this == code); } 131 | friend bool operator!=(ParseErrorCode code, const ParseResult & err) { return err != code; } 132 | 133 | //! Reset error code. 134 | void Clear() { Set(kParseErrorNone); } 135 | //! Update error code and offset. 136 | void Set(ParseErrorCode code, size_t offset = 0) { code_ = code; offset_ = offset; } 137 | 138 | private: 139 | ParseErrorCode code_; 140 | size_t offset_; 141 | }; 142 | 143 | //! Function pointer type of GetParseError(). 144 | /*! \ingroup RAPIDJSON_ERRORS 145 | 146 | This is the prototype for \c GetParseError_X(), where \c X is a locale. 147 | User can dynamically change locale in runtime, e.g.: 148 | \code 149 | GetParseErrorFunc GetParseError = GetParseError_En; // or whatever 150 | const RAPIDJSON_ERROR_CHARTYPE* s = GetParseError(document.GetParseErrorCode()); 151 | \endcode 152 | */ 153 | typedef const RAPIDJSON_ERROR_CHARTYPE* (*GetParseErrorFunc)(ParseErrorCode); 154 | 155 | RAPIDJSON_NAMESPACE_END 156 | 157 | #ifdef __clang__ 158 | RAPIDJSON_DIAG_POP 159 | #endif 160 | 161 | #endif // RAPIDJSON_ERROR_ERROR_H_ 162 | -------------------------------------------------------------------------------- /swarm_planner/include/rapidjson/filereadstream.h: -------------------------------------------------------------------------------- 1 | // Tencent is pleased to support the open source community by making RapidJSON available. 2 | // 3 | // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved. 4 | // 5 | // Licensed under the MIT License (the "License"); you may not use this file except 6 | // in compliance with the License. You may obtain a copy of the License at 7 | // 8 | // http://opensource.org/licenses/MIT 9 | // 10 | // Unless required by applicable law or agreed to in writing, software distributed 11 | // under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR 12 | // CONDITIONS OF ANY KIND, either express or implied. See the License for the 13 | // specific language governing permissions and limitations under the License. 14 | 15 | #ifndef RAPIDJSON_FILEREADSTREAM_H_ 16 | #define RAPIDJSON_FILEREADSTREAM_H_ 17 | 18 | #include "stream.h" 19 | #include 20 | 21 | #ifdef __clang__ 22 | RAPIDJSON_DIAG_PUSH 23 | RAPIDJSON_DIAG_OFF(padded) 24 | RAPIDJSON_DIAG_OFF(unreachable-code) 25 | RAPIDJSON_DIAG_OFF(missing-noreturn) 26 | #endif 27 | 28 | RAPIDJSON_NAMESPACE_BEGIN 29 | 30 | //! File byte stream for input using fread(). 31 | /*! 32 | \note implements Stream concept 33 | */ 34 | class FileReadStream { 35 | public: 36 | typedef char Ch; //!< Character type (byte). 37 | 38 | //! Constructor. 39 | /*! 40 | \param fp File pointer opened for read. 41 | \param buffer user-supplied buffer. 42 | \param bufferSize size of buffer in bytes. Must >=4 bytes. 43 | */ 44 | FileReadStream(std::FILE* fp, char* buffer, size_t bufferSize) : fp_(fp), buffer_(buffer), bufferSize_(bufferSize), bufferLast_(0), current_(buffer_), readCount_(0), count_(0), eof_(false) { 45 | RAPIDJSON_ASSERT(fp_ != 0); 46 | RAPIDJSON_ASSERT(bufferSize >= 4); 47 | Read(); 48 | } 49 | 50 | Ch Peek() const { return *current_; } 51 | Ch Take() { Ch c = *current_; Read(); return c; } 52 | size_t Tell() const { return count_ + static_cast(current_ - buffer_); } 53 | 54 | // Not implemented 55 | void Put(Ch) { RAPIDJSON_ASSERT(false); } 56 | void Flush() { RAPIDJSON_ASSERT(false); } 57 | Ch* PutBegin() { RAPIDJSON_ASSERT(false); return 0; } 58 | size_t PutEnd(Ch*) { RAPIDJSON_ASSERT(false); return 0; } 59 | 60 | // For encoding detection only. 61 | const Ch* Peek4() const { 62 | return (current_ + 4 - !eof_ <= bufferLast_) ? current_ : 0; 63 | } 64 | 65 | private: 66 | void Read() { 67 | if (current_ < bufferLast_) 68 | ++current_; 69 | else if (!eof_) { 70 | count_ += readCount_; 71 | readCount_ = std::fread(buffer_, 1, bufferSize_, fp_); 72 | bufferLast_ = buffer_ + readCount_ - 1; 73 | current_ = buffer_; 74 | 75 | if (readCount_ < bufferSize_) { 76 | buffer_[readCount_] = '\0'; 77 | ++bufferLast_; 78 | eof_ = true; 79 | } 80 | } 81 | } 82 | 83 | std::FILE* fp_; 84 | Ch *buffer_; 85 | size_t bufferSize_; 86 | Ch *bufferLast_; 87 | Ch *current_; 88 | size_t readCount_; 89 | size_t count_; //!< Number of characters read 90 | bool eof_; 91 | }; 92 | 93 | RAPIDJSON_NAMESPACE_END 94 | 95 | #ifdef __clang__ 96 | RAPIDJSON_DIAG_POP 97 | #endif 98 | 99 | #endif // RAPIDJSON_FILESTREAM_H_ 100 | -------------------------------------------------------------------------------- /swarm_planner/include/rapidjson/filewritestream.h: -------------------------------------------------------------------------------- 1 | // Tencent is pleased to support the open source community by making RapidJSON available. 2 | // 3 | // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved. 4 | // 5 | // Licensed under the MIT License (the "License"); you may not use this file except 6 | // in compliance with the License. You may obtain a copy of the License at 7 | // 8 | // http://opensource.org/licenses/MIT 9 | // 10 | // Unless required by applicable law or agreed to in writing, software distributed 11 | // under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR 12 | // CONDITIONS OF ANY KIND, either express or implied. See the License for the 13 | // specific language governing permissions and limitations under the License. 14 | 15 | #ifndef RAPIDJSON_FILEWRITESTREAM_H_ 16 | #define RAPIDJSON_FILEWRITESTREAM_H_ 17 | 18 | #include "stream.h" 19 | #include 20 | 21 | #ifdef __clang__ 22 | RAPIDJSON_DIAG_PUSH 23 | RAPIDJSON_DIAG_OFF(unreachable-code) 24 | #endif 25 | 26 | RAPIDJSON_NAMESPACE_BEGIN 27 | 28 | //! Wrapper of C file stream for output using fwrite(). 29 | /*! 30 | \note implements Stream concept 31 | */ 32 | class FileWriteStream { 33 | public: 34 | typedef char Ch; //!< Character type. Only support char. 35 | 36 | FileWriteStream(std::FILE* fp, char* buffer, size_t bufferSize) : fp_(fp), buffer_(buffer), bufferEnd_(buffer + bufferSize), current_(buffer_) { 37 | RAPIDJSON_ASSERT(fp_ != 0); 38 | } 39 | 40 | void Put(char c) { 41 | if (current_ >= bufferEnd_) 42 | Flush(); 43 | 44 | *current_++ = c; 45 | } 46 | 47 | void PutN(char c, size_t n) { 48 | size_t avail = static_cast(bufferEnd_ - current_); 49 | while (n > avail) { 50 | std::memset(current_, c, avail); 51 | current_ += avail; 52 | Flush(); 53 | n -= avail; 54 | avail = static_cast(bufferEnd_ - current_); 55 | } 56 | 57 | if (n > 0) { 58 | std::memset(current_, c, n); 59 | current_ += n; 60 | } 61 | } 62 | 63 | void Flush() { 64 | if (current_ != buffer_) { 65 | size_t result = std::fwrite(buffer_, 1, static_cast(current_ - buffer_), fp_); 66 | if (result < static_cast(current_ - buffer_)) { 67 | // failure deliberately ignored at this time 68 | // added to avoid warn_unused_result build errors 69 | } 70 | current_ = buffer_; 71 | } 72 | } 73 | 74 | // Not implemented 75 | char Peek() const { RAPIDJSON_ASSERT(false); return 0; } 76 | char Take() { RAPIDJSON_ASSERT(false); return 0; } 77 | size_t Tell() const { RAPIDJSON_ASSERT(false); return 0; } 78 | char* PutBegin() { RAPIDJSON_ASSERT(false); return 0; } 79 | size_t PutEnd(char*) { RAPIDJSON_ASSERT(false); return 0; } 80 | 81 | private: 82 | // Prohibit copy constructor & assignment operator. 83 | FileWriteStream(const FileWriteStream&); 84 | FileWriteStream& operator=(const FileWriteStream&); 85 | 86 | std::FILE* fp_; 87 | char *buffer_; 88 | char *bufferEnd_; 89 | char *current_; 90 | }; 91 | 92 | //! Implement specialized version of PutN() with memset() for better performance. 93 | template<> 94 | inline void PutN(FileWriteStream& stream, char c, size_t n) { 95 | stream.PutN(c, n); 96 | } 97 | 98 | RAPIDJSON_NAMESPACE_END 99 | 100 | #ifdef __clang__ 101 | RAPIDJSON_DIAG_POP 102 | #endif 103 | 104 | #endif // RAPIDJSON_FILESTREAM_H_ 105 | -------------------------------------------------------------------------------- /swarm_planner/include/rapidjson/fwd.h: -------------------------------------------------------------------------------- 1 | // Tencent is pleased to support the open source community by making RapidJSON available. 2 | // 3 | // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved. 4 | // 5 | // Licensed under the MIT License (the "License"); you may not use this file except 6 | // in compliance with the License. You may obtain a copy of the License at 7 | // 8 | // http://opensource.org/licenses/MIT 9 | // 10 | // Unless required by applicable law or agreed to in writing, software distributed 11 | // under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR 12 | // CONDITIONS OF ANY KIND, either express or implied. See the License for the 13 | // specific language governing permissions and limitations under the License. 14 | 15 | #ifndef RAPIDJSON_FWD_H_ 16 | #define RAPIDJSON_FWD_H_ 17 | 18 | #include "rapidjson.h" 19 | 20 | RAPIDJSON_NAMESPACE_BEGIN 21 | 22 | // encodings.h 23 | 24 | template struct UTF8; 25 | template struct UTF16; 26 | template struct UTF16BE; 27 | template struct UTF16LE; 28 | template struct UTF32; 29 | template struct UTF32BE; 30 | template struct UTF32LE; 31 | template struct ASCII; 32 | template struct AutoUTF; 33 | 34 | template 35 | struct Transcoder; 36 | 37 | // allocators.h 38 | 39 | class CrtAllocator; 40 | 41 | template 42 | class MemoryPoolAllocator; 43 | 44 | // stream.h 45 | 46 | template 47 | struct GenericStringStream; 48 | 49 | typedef GenericStringStream > StringStream; 50 | 51 | template 52 | struct GenericInsituStringStream; 53 | 54 | typedef GenericInsituStringStream > InsituStringStream; 55 | 56 | // stringbuffer.h 57 | 58 | template 59 | class GenericStringBuffer; 60 | 61 | typedef GenericStringBuffer, CrtAllocator> StringBuffer; 62 | 63 | // filereadstream.h 64 | 65 | class FileReadStream; 66 | 67 | // filewritestream.h 68 | 69 | class FileWriteStream; 70 | 71 | // memorybuffer.h 72 | 73 | template 74 | struct GenericMemoryBuffer; 75 | 76 | typedef GenericMemoryBuffer MemoryBuffer; 77 | 78 | // memorystream.h 79 | 80 | struct MemoryStream; 81 | 82 | // reader.h 83 | 84 | template 85 | struct BaseReaderHandler; 86 | 87 | template 88 | class GenericReader; 89 | 90 | typedef GenericReader, UTF8, CrtAllocator> Reader; 91 | 92 | // writer.h 93 | 94 | template 95 | class Writer; 96 | 97 | // prettywriter.h 98 | 99 | template 100 | class PrettyWriter; 101 | 102 | // document.h 103 | 104 | template 105 | struct GenericMember; 106 | 107 | template 108 | class GenericMemberIterator; 109 | 110 | template 111 | struct GenericStringRef; 112 | 113 | template 114 | class GenericValue; 115 | 116 | typedef GenericValue, MemoryPoolAllocator > Value; 117 | 118 | template 119 | class GenericDocument; 120 | 121 | typedef GenericDocument, MemoryPoolAllocator, CrtAllocator> Document; 122 | 123 | // pointer.h 124 | 125 | template 126 | class GenericPointer; 127 | 128 | typedef GenericPointer Pointer; 129 | 130 | // schema.h 131 | 132 | template 133 | class IGenericRemoteSchemaDocumentProvider; 134 | 135 | template 136 | class GenericSchemaDocument; 137 | 138 | typedef GenericSchemaDocument SchemaDocument; 139 | typedef IGenericRemoteSchemaDocumentProvider IRemoteSchemaDocumentProvider; 140 | 141 | template < 142 | typename SchemaDocumentType, 143 | typename OutputHandler, 144 | typename StateAllocator> 145 | class GenericSchemaValidator; 146 | 147 | typedef GenericSchemaValidator, void>, CrtAllocator> SchemaValidator; 148 | 149 | RAPIDJSON_NAMESPACE_END 150 | 151 | #endif // RAPIDJSON_RAPIDJSONFWD_H_ 152 | -------------------------------------------------------------------------------- /swarm_planner/include/rapidjson/internal/ieee754.h: -------------------------------------------------------------------------------- 1 | // Tencent is pleased to support the open source community by making RapidJSON available. 2 | // 3 | // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved. 4 | // 5 | // Licensed under the MIT License (the "License"); you may not use this file except 6 | // in compliance with the License. You may obtain a copy of the License at 7 | // 8 | // http://opensource.org/licenses/MIT 9 | // 10 | // Unless required by applicable law or agreed to in writing, software distributed 11 | // under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR 12 | // CONDITIONS OF ANY KIND, either express or implied. See the License for the 13 | // specific language governing permissions and limitations under the License. 14 | 15 | #ifndef RAPIDJSON_IEEE754_ 16 | #define RAPIDJSON_IEEE754_ 17 | 18 | #include "../rapidjson.h" 19 | 20 | RAPIDJSON_NAMESPACE_BEGIN 21 | namespace internal { 22 | 23 | class Double { 24 | public: 25 | Double() {} 26 | Double(double d) : d_(d) {} 27 | Double(uint64_t u) : u_(u) {} 28 | 29 | double Value() const { return d_; } 30 | uint64_t Uint64Value() const { return u_; } 31 | 32 | double NextPositiveDouble() const { 33 | RAPIDJSON_ASSERT(!Sign()); 34 | return Double(u_ + 1).Value(); 35 | } 36 | 37 | bool Sign() const { return (u_ & kSignMask) != 0; } 38 | uint64_t Significand() const { return u_ & kSignificandMask; } 39 | int Exponent() const { return static_cast(((u_ & kExponentMask) >> kSignificandSize) - kExponentBias); } 40 | 41 | bool IsNan() const { return (u_ & kExponentMask) == kExponentMask && Significand() != 0; } 42 | bool IsInf() const { return (u_ & kExponentMask) == kExponentMask && Significand() == 0; } 43 | bool IsNanOrInf() const { return (u_ & kExponentMask) == kExponentMask; } 44 | bool IsNormal() const { return (u_ & kExponentMask) != 0 || Significand() == 0; } 45 | bool IsZero() const { return (u_ & (kExponentMask | kSignificandMask)) == 0; } 46 | 47 | uint64_t IntegerSignificand() const { return IsNormal() ? Significand() | kHiddenBit : Significand(); } 48 | int IntegerExponent() const { return (IsNormal() ? Exponent() : kDenormalExponent) - kSignificandSize; } 49 | uint64_t ToBias() const { return (u_ & kSignMask) ? ~u_ + 1 : u_ | kSignMask; } 50 | 51 | static int EffectiveSignificandSize(int order) { 52 | if (order >= -1021) 53 | return 53; 54 | else if (order <= -1074) 55 | return 0; 56 | else 57 | return order + 1074; 58 | } 59 | 60 | private: 61 | static const int kSignificandSize = 52; 62 | static const int kExponentBias = 0x3FF; 63 | static const int kDenormalExponent = 1 - kExponentBias; 64 | static const uint64_t kSignMask = RAPIDJSON_UINT64_C2(0x80000000, 0x00000000); 65 | static const uint64_t kExponentMask = RAPIDJSON_UINT64_C2(0x7FF00000, 0x00000000); 66 | static const uint64_t kSignificandMask = RAPIDJSON_UINT64_C2(0x000FFFFF, 0xFFFFFFFF); 67 | static const uint64_t kHiddenBit = RAPIDJSON_UINT64_C2(0x00100000, 0x00000000); 68 | 69 | union { 70 | double d_; 71 | uint64_t u_; 72 | }; 73 | }; 74 | 75 | } // namespace internal 76 | RAPIDJSON_NAMESPACE_END 77 | 78 | #endif // RAPIDJSON_IEEE754_ 79 | -------------------------------------------------------------------------------- /swarm_planner/include/rapidjson/internal/meta.h: -------------------------------------------------------------------------------- 1 | // Tencent is pleased to support the open source community by making RapidJSON available. 2 | // 3 | // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved. 4 | // 5 | // Licensed under the MIT License (the "License"); you may not use this file except 6 | // in compliance with the License. You may obtain a copy of the License at 7 | // 8 | // http://opensource.org/licenses/MIT 9 | // 10 | // Unless required by applicable law or agreed to in writing, software distributed 11 | // under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR 12 | // CONDITIONS OF ANY KIND, either express or implied. See the License for the 13 | // specific language governing permissions and limitations under the License. 14 | 15 | #ifndef RAPIDJSON_INTERNAL_META_H_ 16 | #define RAPIDJSON_INTERNAL_META_H_ 17 | 18 | #include "../rapidjson.h" 19 | 20 | #ifdef __GNUC__ 21 | RAPIDJSON_DIAG_PUSH 22 | RAPIDJSON_DIAG_OFF(effc++) 23 | #endif 24 | 25 | #if defined(_MSC_VER) && !defined(__clang__) 26 | RAPIDJSON_DIAG_PUSH 27 | RAPIDJSON_DIAG_OFF(6334) 28 | #endif 29 | 30 | #if RAPIDJSON_HAS_CXX11_TYPETRAITS 31 | #include 32 | #endif 33 | 34 | //@cond RAPIDJSON_INTERNAL 35 | RAPIDJSON_NAMESPACE_BEGIN 36 | namespace internal { 37 | 38 | // Helper to wrap/convert arbitrary types to void, useful for arbitrary type matching 39 | template struct Void { typedef void Type; }; 40 | 41 | /////////////////////////////////////////////////////////////////////////////// 42 | // BoolType, TrueType, FalseType 43 | // 44 | template struct BoolType { 45 | static const bool Value = Cond; 46 | typedef BoolType Type; 47 | }; 48 | typedef BoolType TrueType; 49 | typedef BoolType FalseType; 50 | 51 | 52 | /////////////////////////////////////////////////////////////////////////////// 53 | // SelectIf, BoolExpr, NotExpr, AndExpr, OrExpr 54 | // 55 | 56 | template struct SelectIfImpl { template struct Apply { typedef T1 Type; }; }; 57 | template <> struct SelectIfImpl { template struct Apply { typedef T2 Type; }; }; 58 | template struct SelectIfCond : SelectIfImpl::template Apply {}; 59 | template struct SelectIf : SelectIfCond {}; 60 | 61 | template struct AndExprCond : FalseType {}; 62 | template <> struct AndExprCond : TrueType {}; 63 | template struct OrExprCond : TrueType {}; 64 | template <> struct OrExprCond : FalseType {}; 65 | 66 | template struct BoolExpr : SelectIf::Type {}; 67 | template struct NotExpr : SelectIf::Type {}; 68 | template struct AndExpr : AndExprCond::Type {}; 69 | template struct OrExpr : OrExprCond::Type {}; 70 | 71 | 72 | /////////////////////////////////////////////////////////////////////////////// 73 | // AddConst, MaybeAddConst, RemoveConst 74 | template struct AddConst { typedef const T Type; }; 75 | template struct MaybeAddConst : SelectIfCond {}; 76 | template struct RemoveConst { typedef T Type; }; 77 | template struct RemoveConst { typedef T Type; }; 78 | 79 | 80 | /////////////////////////////////////////////////////////////////////////////// 81 | // IsSame, IsConst, IsMoreConst, IsPointer 82 | // 83 | template struct IsSame : FalseType {}; 84 | template struct IsSame : TrueType {}; 85 | 86 | template struct IsConst : FalseType {}; 87 | template struct IsConst : TrueType {}; 88 | 89 | template 90 | struct IsMoreConst 91 | : AndExpr::Type, typename RemoveConst::Type>, 92 | BoolType::Value >= IsConst::Value> >::Type {}; 93 | 94 | template struct IsPointer : FalseType {}; 95 | template struct IsPointer : TrueType {}; 96 | 97 | /////////////////////////////////////////////////////////////////////////////// 98 | // IsBaseOf 99 | // 100 | #if RAPIDJSON_HAS_CXX11_TYPETRAITS 101 | 102 | template struct IsBaseOf 103 | : BoolType< ::std::is_base_of::value> {}; 104 | 105 | #else // simplified version adopted from Boost 106 | 107 | template struct IsBaseOfImpl { 108 | RAPIDJSON_STATIC_ASSERT(sizeof(B) != 0); 109 | RAPIDJSON_STATIC_ASSERT(sizeof(D) != 0); 110 | 111 | typedef char (&Yes)[1]; 112 | typedef char (&No) [2]; 113 | 114 | template 115 | static Yes Check(const D*, T); 116 | static No Check(const B*, int); 117 | 118 | struct Host { 119 | operator const B*() const; 120 | operator const D*(); 121 | }; 122 | 123 | enum { Value = (sizeof(Check(Host(), 0)) == sizeof(Yes)) }; 124 | }; 125 | 126 | template struct IsBaseOf 127 | : OrExpr, BoolExpr > >::Type {}; 128 | 129 | #endif // RAPIDJSON_HAS_CXX11_TYPETRAITS 130 | 131 | 132 | ////////////////////////////////////////////////////////////////////////// 133 | // EnableIf / DisableIf 134 | // 135 | template struct EnableIfCond { typedef T Type; }; 136 | template struct EnableIfCond { /* empty */ }; 137 | 138 | template struct DisableIfCond { typedef T Type; }; 139 | template struct DisableIfCond { /* empty */ }; 140 | 141 | template 142 | struct EnableIf : EnableIfCond {}; 143 | 144 | template 145 | struct DisableIf : DisableIfCond {}; 146 | 147 | // SFINAE helpers 148 | struct SfinaeTag {}; 149 | template struct RemoveSfinaeTag; 150 | template struct RemoveSfinaeTag { typedef T Type; }; 151 | 152 | #define RAPIDJSON_REMOVEFPTR_(type) \ 153 | typename ::RAPIDJSON_NAMESPACE::internal::RemoveSfinaeTag \ 154 | < ::RAPIDJSON_NAMESPACE::internal::SfinaeTag&(*) type>::Type 155 | 156 | #define RAPIDJSON_ENABLEIF(cond) \ 157 | typename ::RAPIDJSON_NAMESPACE::internal::EnableIf \ 158 | ::Type * = NULL 159 | 160 | #define RAPIDJSON_DISABLEIF(cond) \ 161 | typename ::RAPIDJSON_NAMESPACE::internal::DisableIf \ 162 | ::Type * = NULL 163 | 164 | #define RAPIDJSON_ENABLEIF_RETURN(cond,returntype) \ 165 | typename ::RAPIDJSON_NAMESPACE::internal::EnableIf \ 166 | ::Type 168 | 169 | #define RAPIDJSON_DISABLEIF_RETURN(cond,returntype) \ 170 | typename ::RAPIDJSON_NAMESPACE::internal::DisableIf \ 171 | ::Type 173 | 174 | } // namespace internal 175 | RAPIDJSON_NAMESPACE_END 176 | //@endcond 177 | 178 | #if defined(_MSC_VER) && !defined(__clang__) 179 | RAPIDJSON_DIAG_POP 180 | #endif 181 | 182 | #ifdef __GNUC__ 183 | RAPIDJSON_DIAG_POP 184 | #endif 185 | 186 | #endif // RAPIDJSON_INTERNAL_META_H_ 187 | -------------------------------------------------------------------------------- /swarm_planner/include/rapidjson/internal/pow10.h: -------------------------------------------------------------------------------- 1 | // Tencent is pleased to support the open source community by making RapidJSON available. 2 | // 3 | // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved. 4 | // 5 | // Licensed under the MIT License (the "License"); you may not use this file except 6 | // in compliance with the License. You may obtain a copy of the License at 7 | // 8 | // http://opensource.org/licenses/MIT 9 | // 10 | // Unless required by applicable law or agreed to in writing, software distributed 11 | // under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR 12 | // CONDITIONS OF ANY KIND, either express or implied. See the License for the 13 | // specific language governing permissions and limitations under the License. 14 | 15 | #ifndef RAPIDJSON_POW10_ 16 | #define RAPIDJSON_POW10_ 17 | 18 | #include "../rapidjson.h" 19 | 20 | RAPIDJSON_NAMESPACE_BEGIN 21 | namespace internal { 22 | 23 | //! Computes integer powers of 10 in double (10.0^n). 24 | /*! This function uses lookup table for fast and accurate results. 25 | \param n non-negative exponent. Must <= 308. 26 | \return 10.0^n 27 | */ 28 | inline double Pow10(int n) { 29 | static const double e[] = { // 1e-0...1e308: 309 * 8 bytes = 2472 bytes 30 | 1e+0, 31 | 1e+1, 1e+2, 1e+3, 1e+4, 1e+5, 1e+6, 1e+7, 1e+8, 1e+9, 1e+10, 1e+11, 1e+12, 1e+13, 1e+14, 1e+15, 1e+16, 1e+17, 1e+18, 1e+19, 1e+20, 32 | 1e+21, 1e+22, 1e+23, 1e+24, 1e+25, 1e+26, 1e+27, 1e+28, 1e+29, 1e+30, 1e+31, 1e+32, 1e+33, 1e+34, 1e+35, 1e+36, 1e+37, 1e+38, 1e+39, 1e+40, 33 | 1e+41, 1e+42, 1e+43, 1e+44, 1e+45, 1e+46, 1e+47, 1e+48, 1e+49, 1e+50, 1e+51, 1e+52, 1e+53, 1e+54, 1e+55, 1e+56, 1e+57, 1e+58, 1e+59, 1e+60, 34 | 1e+61, 1e+62, 1e+63, 1e+64, 1e+65, 1e+66, 1e+67, 1e+68, 1e+69, 1e+70, 1e+71, 1e+72, 1e+73, 1e+74, 1e+75, 1e+76, 1e+77, 1e+78, 1e+79, 1e+80, 35 | 1e+81, 1e+82, 1e+83, 1e+84, 1e+85, 1e+86, 1e+87, 1e+88, 1e+89, 1e+90, 1e+91, 1e+92, 1e+93, 1e+94, 1e+95, 1e+96, 1e+97, 1e+98, 1e+99, 1e+100, 36 | 1e+101,1e+102,1e+103,1e+104,1e+105,1e+106,1e+107,1e+108,1e+109,1e+110,1e+111,1e+112,1e+113,1e+114,1e+115,1e+116,1e+117,1e+118,1e+119,1e+120, 37 | 1e+121,1e+122,1e+123,1e+124,1e+125,1e+126,1e+127,1e+128,1e+129,1e+130,1e+131,1e+132,1e+133,1e+134,1e+135,1e+136,1e+137,1e+138,1e+139,1e+140, 38 | 1e+141,1e+142,1e+143,1e+144,1e+145,1e+146,1e+147,1e+148,1e+149,1e+150,1e+151,1e+152,1e+153,1e+154,1e+155,1e+156,1e+157,1e+158,1e+159,1e+160, 39 | 1e+161,1e+162,1e+163,1e+164,1e+165,1e+166,1e+167,1e+168,1e+169,1e+170,1e+171,1e+172,1e+173,1e+174,1e+175,1e+176,1e+177,1e+178,1e+179,1e+180, 40 | 1e+181,1e+182,1e+183,1e+184,1e+185,1e+186,1e+187,1e+188,1e+189,1e+190,1e+191,1e+192,1e+193,1e+194,1e+195,1e+196,1e+197,1e+198,1e+199,1e+200, 41 | 1e+201,1e+202,1e+203,1e+204,1e+205,1e+206,1e+207,1e+208,1e+209,1e+210,1e+211,1e+212,1e+213,1e+214,1e+215,1e+216,1e+217,1e+218,1e+219,1e+220, 42 | 1e+221,1e+222,1e+223,1e+224,1e+225,1e+226,1e+227,1e+228,1e+229,1e+230,1e+231,1e+232,1e+233,1e+234,1e+235,1e+236,1e+237,1e+238,1e+239,1e+240, 43 | 1e+241,1e+242,1e+243,1e+244,1e+245,1e+246,1e+247,1e+248,1e+249,1e+250,1e+251,1e+252,1e+253,1e+254,1e+255,1e+256,1e+257,1e+258,1e+259,1e+260, 44 | 1e+261,1e+262,1e+263,1e+264,1e+265,1e+266,1e+267,1e+268,1e+269,1e+270,1e+271,1e+272,1e+273,1e+274,1e+275,1e+276,1e+277,1e+278,1e+279,1e+280, 45 | 1e+281,1e+282,1e+283,1e+284,1e+285,1e+286,1e+287,1e+288,1e+289,1e+290,1e+291,1e+292,1e+293,1e+294,1e+295,1e+296,1e+297,1e+298,1e+299,1e+300, 46 | 1e+301,1e+302,1e+303,1e+304,1e+305,1e+306,1e+307,1e+308 47 | }; 48 | RAPIDJSON_ASSERT(n >= 0 && n <= 308); 49 | return e[n]; 50 | } 51 | 52 | } // namespace internal 53 | RAPIDJSON_NAMESPACE_END 54 | 55 | #endif // RAPIDJSON_POW10_ 56 | -------------------------------------------------------------------------------- /swarm_planner/include/rapidjson/internal/strfunc.h: -------------------------------------------------------------------------------- 1 | // Tencent is pleased to support the open source community by making RapidJSON available. 2 | // 3 | // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved. 4 | // 5 | // Licensed under the MIT License (the "License"); you may not use this file except 6 | // in compliance with the License. You may obtain a copy of the License at 7 | // 8 | // http://opensource.org/licenses/MIT 9 | // 10 | // Unless required by applicable law or agreed to in writing, software distributed 11 | // under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR 12 | // CONDITIONS OF ANY KIND, either express or implied. See the License for the 13 | // specific language governing permissions and limitations under the License. 14 | 15 | #ifndef RAPIDJSON_INTERNAL_STRFUNC_H_ 16 | #define RAPIDJSON_INTERNAL_STRFUNC_H_ 17 | 18 | #include "../stream.h" 19 | #include 20 | 21 | RAPIDJSON_NAMESPACE_BEGIN 22 | namespace internal { 23 | 24 | //! Custom strlen() which works on different character types. 25 | /*! \tparam Ch Character type (e.g. char, wchar_t, short) 26 | \param s Null-terminated input string. 27 | \return Number of characters in the string. 28 | \note This has the same semantics as strlen(), the return value is not number of Unicode codepoints. 29 | */ 30 | template 31 | inline SizeType StrLen(const Ch* s) { 32 | RAPIDJSON_ASSERT(s != 0); 33 | const Ch* p = s; 34 | while (*p) ++p; 35 | return SizeType(p - s); 36 | } 37 | 38 | template <> 39 | inline SizeType StrLen(const char* s) { 40 | return SizeType(std::strlen(s)); 41 | } 42 | 43 | template <> 44 | inline SizeType StrLen(const wchar_t* s) { 45 | return SizeType(std::wcslen(s)); 46 | } 47 | 48 | //! Returns number of code points in a encoded string. 49 | template 50 | bool CountStringCodePoint(const typename Encoding::Ch* s, SizeType length, SizeType* outCount) { 51 | RAPIDJSON_ASSERT(s != 0); 52 | RAPIDJSON_ASSERT(outCount != 0); 53 | GenericStringStream is(s); 54 | const typename Encoding::Ch* end = s + length; 55 | SizeType count = 0; 56 | while (is.src_ < end) { 57 | unsigned codepoint; 58 | if (!Encoding::Decode(is, &codepoint)) 59 | return false; 60 | count++; 61 | } 62 | *outCount = count; 63 | return true; 64 | } 65 | 66 | } // namespace internal 67 | RAPIDJSON_NAMESPACE_END 68 | 69 | #endif // RAPIDJSON_INTERNAL_STRFUNC_H_ 70 | -------------------------------------------------------------------------------- /swarm_planner/include/rapidjson/internal/swap.h: -------------------------------------------------------------------------------- 1 | // Tencent is pleased to support the open source community by making RapidJSON available. 2 | // 3 | // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved. 4 | // 5 | // Licensed under the MIT License (the "License"); you may not use this file except 6 | // in compliance with the License. You may obtain a copy of the License at 7 | // 8 | // http://opensource.org/licenses/MIT 9 | // 10 | // Unless required by applicable law or agreed to in writing, software distributed 11 | // under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR 12 | // CONDITIONS OF ANY KIND, either express or implied. See the License for the 13 | // specific language governing permissions and limitations under the License. 14 | 15 | #ifndef RAPIDJSON_INTERNAL_SWAP_H_ 16 | #define RAPIDJSON_INTERNAL_SWAP_H_ 17 | 18 | #include "../rapidjson.h" 19 | 20 | #if defined(__clang__) 21 | RAPIDJSON_DIAG_PUSH 22 | RAPIDJSON_DIAG_OFF(c++98-compat) 23 | #endif 24 | 25 | RAPIDJSON_NAMESPACE_BEGIN 26 | namespace internal { 27 | 28 | //! Custom swap() to avoid dependency on C++ header 29 | /*! \tparam T Type of the arguments to swap, should be instantiated with primitive C++ types only. 30 | \note This has the same semantics as std::swap(). 31 | */ 32 | template 33 | inline void Swap(T& a, T& b) RAPIDJSON_NOEXCEPT { 34 | T tmp = a; 35 | a = b; 36 | b = tmp; 37 | } 38 | 39 | } // namespace internal 40 | RAPIDJSON_NAMESPACE_END 41 | 42 | #if defined(__clang__) 43 | RAPIDJSON_DIAG_POP 44 | #endif 45 | 46 | #endif // RAPIDJSON_INTERNAL_SWAP_H_ 47 | -------------------------------------------------------------------------------- /swarm_planner/include/rapidjson/istreamwrapper.h: -------------------------------------------------------------------------------- 1 | // Tencent is pleased to support the open source community by making RapidJSON available. 2 | // 3 | // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved. 4 | // 5 | // Licensed under the MIT License (the "License"); you may not use this file except 6 | // in compliance with the License. You may obtain a copy of the License at 7 | // 8 | // http://opensource.org/licenses/MIT 9 | // 10 | // Unless required by applicable law or agreed to in writing, software distributed 11 | // under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR 12 | // CONDITIONS OF ANY KIND, either express or implied. See the License for the 13 | // specific language governing permissions and limitations under the License. 14 | 15 | #ifndef RAPIDJSON_ISTREAMWRAPPER_H_ 16 | #define RAPIDJSON_ISTREAMWRAPPER_H_ 17 | 18 | #include "stream.h" 19 | #include 20 | #include 21 | 22 | #ifdef __clang__ 23 | RAPIDJSON_DIAG_PUSH 24 | RAPIDJSON_DIAG_OFF(padded) 25 | #elif defined(_MSC_VER) 26 | RAPIDJSON_DIAG_PUSH 27 | RAPIDJSON_DIAG_OFF(4351) // new behavior: elements of array 'array' will be default initialized 28 | #endif 29 | 30 | RAPIDJSON_NAMESPACE_BEGIN 31 | 32 | //! Wrapper of \c std::basic_istream into RapidJSON's Stream concept. 33 | /*! 34 | The classes can be wrapped including but not limited to: 35 | 36 | - \c std::istringstream 37 | - \c std::stringstream 38 | - \c std::wistringstream 39 | - \c std::wstringstream 40 | - \c std::ifstream 41 | - \c std::fstream 42 | - \c std::wifstream 43 | - \c std::wfstream 44 | 45 | \tparam StreamType Class derived from \c std::basic_istream. 46 | */ 47 | 48 | template 49 | class BasicIStreamWrapper { 50 | public: 51 | typedef typename StreamType::char_type Ch; 52 | 53 | //! Constructor. 54 | /*! 55 | \param stream stream opened for read. 56 | */ 57 | BasicIStreamWrapper(StreamType &stream) : stream_(stream), buffer_(peekBuffer_), bufferSize_(4), bufferLast_(0), current_(buffer_), readCount_(0), count_(0), eof_(false) { 58 | Read(); 59 | } 60 | 61 | //! Constructor. 62 | /*! 63 | \param stream stream opened for read. 64 | \param buffer user-supplied buffer. 65 | \param bufferSize size of buffer in bytes. Must >=4 bytes. 66 | */ 67 | BasicIStreamWrapper(StreamType &stream, char* buffer, size_t bufferSize) : stream_(stream), buffer_(buffer), bufferSize_(bufferSize), bufferLast_(0), current_(buffer_), readCount_(0), count_(0), eof_(false) { 68 | RAPIDJSON_ASSERT(bufferSize >= 4); 69 | Read(); 70 | } 71 | 72 | Ch Peek() const { return *current_; } 73 | Ch Take() { Ch c = *current_; Read(); return c; } 74 | size_t Tell() const { return count_ + static_cast(current_ - buffer_); } 75 | 76 | // Not implemented 77 | void Put(Ch) { RAPIDJSON_ASSERT(false); } 78 | void Flush() { RAPIDJSON_ASSERT(false); } 79 | Ch* PutBegin() { RAPIDJSON_ASSERT(false); return 0; } 80 | size_t PutEnd(Ch*) { RAPIDJSON_ASSERT(false); return 0; } 81 | 82 | // For encoding detection only. 83 | const Ch* Peek4() const { 84 | return (current_ + 4 - !eof_ <= bufferLast_) ? current_ : 0; 85 | } 86 | 87 | private: 88 | BasicIStreamWrapper(); 89 | BasicIStreamWrapper(const BasicIStreamWrapper&); 90 | BasicIStreamWrapper& operator=(const BasicIStreamWrapper&); 91 | 92 | void Read() { 93 | if (current_ < bufferLast_) 94 | ++current_; 95 | else if (!eof_) { 96 | count_ += readCount_; 97 | readCount_ = bufferSize_; 98 | bufferLast_ = buffer_ + readCount_ - 1; 99 | current_ = buffer_; 100 | 101 | if (!stream_.read(buffer_, static_cast(bufferSize_))) { 102 | readCount_ = static_cast(stream_.gcount()); 103 | *(bufferLast_ = buffer_ + readCount_) = '\0'; 104 | eof_ = true; 105 | } 106 | } 107 | } 108 | 109 | StreamType &stream_; 110 | Ch peekBuffer_[4], *buffer_; 111 | size_t bufferSize_; 112 | Ch *bufferLast_; 113 | Ch *current_; 114 | size_t readCount_; 115 | size_t count_; //!< Number of characters read 116 | bool eof_; 117 | }; 118 | 119 | typedef BasicIStreamWrapper IStreamWrapper; 120 | typedef BasicIStreamWrapper WIStreamWrapper; 121 | 122 | #if defined(__clang__) || defined(_MSC_VER) 123 | RAPIDJSON_DIAG_POP 124 | #endif 125 | 126 | RAPIDJSON_NAMESPACE_END 127 | 128 | #endif // RAPIDJSON_ISTREAMWRAPPER_H_ 129 | -------------------------------------------------------------------------------- /swarm_planner/include/rapidjson/memorybuffer.h: -------------------------------------------------------------------------------- 1 | // Tencent is pleased to support the open source community by making RapidJSON available. 2 | // 3 | // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved. 4 | // 5 | // Licensed under the MIT License (the "License"); you may not use this file except 6 | // in compliance with the License. You may obtain a copy of the License at 7 | // 8 | // http://opensource.org/licenses/MIT 9 | // 10 | // Unless required by applicable law or agreed to in writing, software distributed 11 | // under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR 12 | // CONDITIONS OF ANY KIND, either express or implied. See the License for the 13 | // specific language governing permissions and limitations under the License. 14 | 15 | #ifndef RAPIDJSON_MEMORYBUFFER_H_ 16 | #define RAPIDJSON_MEMORYBUFFER_H_ 17 | 18 | #include "stream.h" 19 | #include "internal/stack.h" 20 | 21 | RAPIDJSON_NAMESPACE_BEGIN 22 | 23 | //! Represents an in-memory output byte stream. 24 | /*! 25 | This class is mainly for being wrapped by EncodedOutputStream or AutoUTFOutputStream. 26 | 27 | It is similar to FileWriteBuffer but the destination is an in-memory buffer instead of a file. 28 | 29 | Differences between MemoryBuffer and StringBuffer: 30 | 1. StringBuffer has Encoding but MemoryBuffer is only a byte buffer. 31 | 2. StringBuffer::GetString() returns a null-terminated string. MemoryBuffer::GetBuffer() returns a buffer without terminator. 32 | 33 | \tparam Allocator type for allocating memory buffer. 34 | \note implements Stream concept 35 | */ 36 | template 37 | struct GenericMemoryBuffer { 38 | typedef char Ch; // byte 39 | 40 | GenericMemoryBuffer(Allocator* allocator = 0, size_t capacity = kDefaultCapacity) : stack_(allocator, capacity) {} 41 | 42 | void Put(Ch c) { *stack_.template Push() = c; } 43 | void Flush() {} 44 | 45 | void Clear() { stack_.Clear(); } 46 | void ShrinkToFit() { stack_.ShrinkToFit(); } 47 | Ch* Push(size_t count) { return stack_.template Push(count); } 48 | void Pop(size_t count) { stack_.template Pop(count); } 49 | 50 | const Ch* GetBuffer() const { 51 | return stack_.template Bottom(); 52 | } 53 | 54 | size_t GetSize() const { return stack_.GetSize(); } 55 | 56 | static const size_t kDefaultCapacity = 256; 57 | mutable internal::Stack stack_; 58 | }; 59 | 60 | typedef GenericMemoryBuffer<> MemoryBuffer; 61 | 62 | //! Implement specialized version of PutN() with memset() for better performance. 63 | template<> 64 | inline void PutN(MemoryBuffer& memoryBuffer, char c, size_t n) { 65 | std::memset(memoryBuffer.stack_.Push(n), c, n * sizeof(c)); 66 | } 67 | 68 | RAPIDJSON_NAMESPACE_END 69 | 70 | #endif // RAPIDJSON_MEMORYBUFFER_H_ 71 | -------------------------------------------------------------------------------- /swarm_planner/include/rapidjson/memorystream.h: -------------------------------------------------------------------------------- 1 | // Tencent is pleased to support the open source community by making RapidJSON available. 2 | // 3 | // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved. 4 | // 5 | // Licensed under the MIT License (the "License"); you may not use this file except 6 | // in compliance with the License. You may obtain a copy of the License at 7 | // 8 | // http://opensource.org/licenses/MIT 9 | // 10 | // Unless required by applicable law or agreed to in writing, software distributed 11 | // under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR 12 | // CONDITIONS OF ANY KIND, either express or implied. See the License for the 13 | // specific language governing permissions and limitations under the License. 14 | 15 | #ifndef RAPIDJSON_MEMORYSTREAM_H_ 16 | #define RAPIDJSON_MEMORYSTREAM_H_ 17 | 18 | #include "stream.h" 19 | 20 | #ifdef __clang__ 21 | RAPIDJSON_DIAG_PUSH 22 | RAPIDJSON_DIAG_OFF(unreachable-code) 23 | RAPIDJSON_DIAG_OFF(missing-noreturn) 24 | #endif 25 | 26 | RAPIDJSON_NAMESPACE_BEGIN 27 | 28 | //! Represents an in-memory input byte stream. 29 | /*! 30 | This class is mainly for being wrapped by EncodedInputStream or AutoUTFInputStream. 31 | 32 | It is similar to FileReadBuffer but the source is an in-memory buffer instead of a file. 33 | 34 | Differences between MemoryStream and StringStream: 35 | 1. StringStream has encoding but MemoryStream is a byte stream. 36 | 2. MemoryStream needs size of the source buffer and the buffer don't need to be null terminated. StringStream assume null-terminated string as source. 37 | 3. MemoryStream supports Peek4() for encoding detection. StringStream is specified with an encoding so it should not have Peek4(). 38 | \note implements Stream concept 39 | */ 40 | struct MemoryStream { 41 | typedef char Ch; // byte 42 | 43 | MemoryStream(const Ch *src, size_t size) : src_(src), begin_(src), end_(src + size), size_(size) {} 44 | 45 | Ch Peek() const { return RAPIDJSON_UNLIKELY(src_ == end_) ? '\0' : *src_; } 46 | Ch Take() { return RAPIDJSON_UNLIKELY(src_ == end_) ? '\0' : *src_++; } 47 | size_t Tell() const { return static_cast(src_ - begin_); } 48 | 49 | Ch* PutBegin() { RAPIDJSON_ASSERT(false); return 0; } 50 | void Put(Ch) { RAPIDJSON_ASSERT(false); } 51 | void Flush() { RAPIDJSON_ASSERT(false); } 52 | size_t PutEnd(Ch*) { RAPIDJSON_ASSERT(false); return 0; } 53 | 54 | // For encoding detection only. 55 | const Ch* Peek4() const { 56 | return Tell() + 4 <= size_ ? src_ : 0; 57 | } 58 | 59 | const Ch* src_; //!< Current read position. 60 | const Ch* begin_; //!< Original head of the string. 61 | const Ch* end_; //!< End of stream. 62 | size_t size_; //!< Size of the stream. 63 | }; 64 | 65 | RAPIDJSON_NAMESPACE_END 66 | 67 | #ifdef __clang__ 68 | RAPIDJSON_DIAG_POP 69 | #endif 70 | 71 | #endif // RAPIDJSON_MEMORYBUFFER_H_ 72 | -------------------------------------------------------------------------------- /swarm_planner/include/rapidjson/ostreamwrapper.h: -------------------------------------------------------------------------------- 1 | // Tencent is pleased to support the open source community by making RapidJSON available. 2 | // 3 | // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved. 4 | // 5 | // Licensed under the MIT License (the "License"); you may not use this file except 6 | // in compliance with the License. You may obtain a copy of the License at 7 | // 8 | // http://opensource.org/licenses/MIT 9 | // 10 | // Unless required by applicable law or agreed to in writing, software distributed 11 | // under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR 12 | // CONDITIONS OF ANY KIND, either express or implied. See the License for the 13 | // specific language governing permissions and limitations under the License. 14 | 15 | #ifndef RAPIDJSON_OSTREAMWRAPPER_H_ 16 | #define RAPIDJSON_OSTREAMWRAPPER_H_ 17 | 18 | #include "stream.h" 19 | #include 20 | 21 | #ifdef __clang__ 22 | RAPIDJSON_DIAG_PUSH 23 | RAPIDJSON_DIAG_OFF(padded) 24 | #endif 25 | 26 | RAPIDJSON_NAMESPACE_BEGIN 27 | 28 | //! Wrapper of \c std::basic_ostream into RapidJSON's Stream concept. 29 | /*! 30 | The classes can be wrapped including but not limited to: 31 | 32 | - \c std::ostringstream 33 | - \c std::stringstream 34 | - \c std::wpstringstream 35 | - \c std::wstringstream 36 | - \c std::ifstream 37 | - \c std::fstream 38 | - \c std::wofstream 39 | - \c std::wfstream 40 | 41 | \tparam StreamType Class derived from \c std::basic_ostream. 42 | */ 43 | 44 | template 45 | class BasicOStreamWrapper { 46 | public: 47 | typedef typename StreamType::char_type Ch; 48 | BasicOStreamWrapper(StreamType& stream) : stream_(stream) {} 49 | 50 | void Put(Ch c) { 51 | stream_.put(c); 52 | } 53 | 54 | void Flush() { 55 | stream_.flush(); 56 | } 57 | 58 | // Not implemented 59 | char Peek() const { RAPIDJSON_ASSERT(false); return 0; } 60 | char Take() { RAPIDJSON_ASSERT(false); return 0; } 61 | size_t Tell() const { RAPIDJSON_ASSERT(false); return 0; } 62 | char* PutBegin() { RAPIDJSON_ASSERT(false); return 0; } 63 | size_t PutEnd(char*) { RAPIDJSON_ASSERT(false); return 0; } 64 | 65 | private: 66 | BasicOStreamWrapper(const BasicOStreamWrapper&); 67 | BasicOStreamWrapper& operator=(const BasicOStreamWrapper&); 68 | 69 | StreamType& stream_; 70 | }; 71 | 72 | typedef BasicOStreamWrapper OStreamWrapper; 73 | typedef BasicOStreamWrapper WOStreamWrapper; 74 | 75 | #ifdef __clang__ 76 | RAPIDJSON_DIAG_POP 77 | #endif 78 | 79 | RAPIDJSON_NAMESPACE_END 80 | 81 | #endif // RAPIDJSON_OSTREAMWRAPPER_H_ 82 | -------------------------------------------------------------------------------- /swarm_planner/include/rapidjson/stringbuffer.h: -------------------------------------------------------------------------------- 1 | // Tencent is pleased to support the open source community by making RapidJSON available. 2 | // 3 | // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved. 4 | // 5 | // Licensed under the MIT License (the "License"); you may not use this file except 6 | // in compliance with the License. You may obtain a copy of the License at 7 | // 8 | // http://opensource.org/licenses/MIT 9 | // 10 | // Unless required by applicable law or agreed to in writing, software distributed 11 | // under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR 12 | // CONDITIONS OF ANY KIND, either express or implied. See the License for the 13 | // specific language governing permissions and limitations under the License. 14 | 15 | #ifndef RAPIDJSON_STRINGBUFFER_H_ 16 | #define RAPIDJSON_STRINGBUFFER_H_ 17 | 18 | #include "stream.h" 19 | #include "internal/stack.h" 20 | 21 | #if RAPIDJSON_HAS_CXX11_RVALUE_REFS 22 | #include // std::move 23 | #endif 24 | 25 | #include "internal/stack.h" 26 | 27 | #if defined(__clang__) 28 | RAPIDJSON_DIAG_PUSH 29 | RAPIDJSON_DIAG_OFF(c++98-compat) 30 | #endif 31 | 32 | RAPIDJSON_NAMESPACE_BEGIN 33 | 34 | //! Represents an in-memory output stream. 35 | /*! 36 | \tparam Encoding Encoding of the stream. 37 | \tparam Allocator type for allocating memory buffer. 38 | \note implements Stream concept 39 | */ 40 | template 41 | class GenericStringBuffer { 42 | public: 43 | typedef typename Encoding::Ch Ch; 44 | 45 | GenericStringBuffer(Allocator* allocator = 0, size_t capacity = kDefaultCapacity) : stack_(allocator, capacity) {} 46 | 47 | #if RAPIDJSON_HAS_CXX11_RVALUE_REFS 48 | GenericStringBuffer(GenericStringBuffer&& rhs) : stack_(std::move(rhs.stack_)) {} 49 | GenericStringBuffer& operator=(GenericStringBuffer&& rhs) { 50 | if (&rhs != this) 51 | stack_ = std::move(rhs.stack_); 52 | return *this; 53 | } 54 | #endif 55 | 56 | void Put(Ch c) { *stack_.template Push() = c; } 57 | void PutUnsafe(Ch c) { *stack_.template PushUnsafe() = c; } 58 | void Flush() {} 59 | 60 | void Clear() { stack_.Clear(); } 61 | void ShrinkToFit() { 62 | // Push and pop a null terminator. This is safe. 63 | *stack_.template Push() = '\0'; 64 | stack_.ShrinkToFit(); 65 | stack_.template Pop(1); 66 | } 67 | 68 | void Reserve(size_t count) { stack_.template Reserve(count); } 69 | Ch* Push(size_t count) { return stack_.template Push(count); } 70 | Ch* PushUnsafe(size_t count) { return stack_.template PushUnsafe(count); } 71 | void Pop(size_t count) { stack_.template Pop(count); } 72 | 73 | const Ch* GetString() const { 74 | // Push and pop a null terminator. This is safe. 75 | *stack_.template Push() = '\0'; 76 | stack_.template Pop(1); 77 | 78 | return stack_.template Bottom(); 79 | } 80 | 81 | //! Get the size of string in bytes in the string buffer. 82 | size_t GetSize() const { return stack_.GetSize(); } 83 | 84 | //! Get the length of string in Ch in the string buffer. 85 | size_t GetLength() const { return stack_.GetSize() / sizeof(Ch); } 86 | 87 | static const size_t kDefaultCapacity = 256; 88 | mutable internal::Stack stack_; 89 | 90 | private: 91 | // Prohibit copy constructor & assignment operator. 92 | GenericStringBuffer(const GenericStringBuffer&); 93 | GenericStringBuffer& operator=(const GenericStringBuffer&); 94 | }; 95 | 96 | //! String buffer with UTF8 encoding 97 | typedef GenericStringBuffer > StringBuffer; 98 | 99 | template 100 | inline void PutReserve(GenericStringBuffer& stream, size_t count) { 101 | stream.Reserve(count); 102 | } 103 | 104 | template 105 | inline void PutUnsafe(GenericStringBuffer& stream, typename Encoding::Ch c) { 106 | stream.PutUnsafe(c); 107 | } 108 | 109 | //! Implement specialized version of PutN() with memset() for better performance. 110 | template<> 111 | inline void PutN(GenericStringBuffer >& stream, char c, size_t n) { 112 | std::memset(stream.stack_.Push(n), c, n * sizeof(c)); 113 | } 114 | 115 | RAPIDJSON_NAMESPACE_END 116 | 117 | #if defined(__clang__) 118 | RAPIDJSON_DIAG_POP 119 | #endif 120 | 121 | #endif // RAPIDJSON_STRINGBUFFER_H_ 122 | -------------------------------------------------------------------------------- /swarm_planner/include/sp_const.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #define SP_EPSILON 1e-9 4 | #define SP_EPSILON_FLOAT 1e-6 5 | #define SP_INFINITY 1e+9 6 | 7 | #define SP_PT_RBP 0 8 | #define SP_PT_SCP 1 9 | 10 | #define SP_IPT_ECBS 0 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | typedef std::vector> initTraj_t; 17 | typedef std::vector, double>>> SFC_t; 18 | typedef std::vector >>> RSFC_t; 19 | 20 | namespace SwarmPlanning{ 21 | struct PlanResult{ 22 | initTraj_t initTraj; // discrete initial trajectory: pi_0,...,pi_M 23 | std::vector T; // segment time: T_0,...,T_M 24 | SFC_t SFC; // safe flight corridors to avoid obstacles 25 | RSFC_t RSFC; // relative safe flight corridors to avoid inter-collision 26 | std_msgs::Float64MultiArray msgs_traj_info; // [N, n, T_0, ... , T_M] 27 | std::vector msgs_traj_coef; // 28 | }; 29 | } -------------------------------------------------------------------------------- /swarm_planner/include/timer.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | class Timer { 7 | public: 8 | Timer() 9 | : start_(std::chrono::high_resolution_clock::now()), 10 | end_(std::chrono::high_resolution_clock::now()) {} 11 | 12 | void reset() { start_ = std::chrono::high_resolution_clock::now(); } 13 | 14 | void stop() { end_ = std::chrono::high_resolution_clock::now(); } 15 | 16 | double elapsedSeconds() const { 17 | auto timeSpan = std::chrono::duration_cast>( 18 | end_ - start_); 19 | return timeSpan.count(); 20 | } 21 | 22 | private: 23 | std::chrono::high_resolution_clock::time_point start_; 24 | std::chrono::high_resolution_clock::time_point end_; 25 | }; 26 | 27 | class ScopedTimer : public Timer { 28 | public: 29 | ScopedTimer() {} 30 | 31 | ~ScopedTimer() { 32 | stop(); 33 | std::cout << "Elapsed: " << elapsedSeconds() << " s" << std::endl; 34 | } 35 | }; 36 | -------------------------------------------------------------------------------- /swarm_planner/launch/create_octomap_world.launch: -------------------------------------------------------------------------------- 1 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /swarm_planner/launch/map_saver.launch: -------------------------------------------------------------------------------- 1 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /swarm_planner/launch/plan_rbp_empty_space.launch: -------------------------------------------------------------------------------- 1 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | -------------------------------------------------------------------------------- /swarm_planner/launch/plan_rbp_test.launch: -------------------------------------------------------------------------------- 1 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | -------------------------------------------------------------------------------- /swarm_planner/launch/plan_scp_empty_space.launch: -------------------------------------------------------------------------------- 1 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | -------------------------------------------------------------------------------- /swarm_planner/launch/rqtplot_config/rqtplot_config.perspective: -------------------------------------------------------------------------------- 1 | { 2 | "keys": {}, 3 | "groups": { 4 | "pluginmanager": { 5 | "keys": { 6 | "running-plugins": { 7 | "type": "repr", 8 | "repr": "{u'rqt_plot/Plot': [1]}" 9 | } 10 | }, 11 | "groups": { 12 | "plugin__rqt_plot__Plot__1": { 13 | "keys": {}, 14 | "groups": { 15 | "dock_widget__DataPlotWidget": { 16 | "keys": { 17 | "dockable": { 18 | "type": "repr", 19 | "repr": "True" 20 | }, 21 | "parent": { 22 | "type": "repr", 23 | "repr": "None" 24 | }, 25 | "dock_widget_title": { 26 | "type": "repr", 27 | "repr": "u'MatPlot'" 28 | } 29 | }, 30 | "groups": {} 31 | }, 32 | "plugin": { 33 | "keys": { 34 | "autoscroll": { 35 | "type": "repr", 36 | "repr": "False" 37 | }, 38 | "plot_type": { 39 | "type": "repr", 40 | "repr": "1" 41 | }, 42 | "topics": { 43 | "type": "repr", 44 | "repr": "[u'/min_dist/position/y', u'/min_dist/position/x']" 45 | }, 46 | "y_limits": { 47 | "type": "repr", 48 | "repr": "[0.0, 5.0]" 49 | }, 50 | "x_limits": { 51 | "type": "repr", 52 | "repr": "[0.0, 40.0]" 53 | } 54 | }, 55 | "groups": {} 56 | } 57 | } 58 | } 59 | } 60 | }, 61 | "mainwindow": { 62 | "keys": { 63 | "geometry": { 64 | "type": "repr(QByteArray.hex)", 65 | "repr(QByteArray.hex)": "QtCore.QByteArray('01d9d0cb00020000000009820000008700000d79000003b400000982000000a500000d79000003b400000001000000000780')", 66 | "pretty-print": " y y " 67 | }, 68 | "state": { 69 | "type": "repr(QByteArray.hex)", 70 | "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff00000000fd0000000100000003000003f8000002e4fc0100000001fb00000042007200710074005f0070006c006f0074005f005f0050006c006f0074005f005f0031005f005f00440061007400610050006c006f00740057006900640067006500740100000000000003f80000013f00ffffff000003f80000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')", 71 | "pretty-print": " Brqt_plot__Plot__1__DataPlotWidget 6MinimizedDockWidgetsToolbar " 72 | } 73 | }, 74 | "groups": { 75 | "toolbar_areas": { 76 | "keys": { 77 | "MinimizedDockWidgetsToolbar": { 78 | "type": "repr", 79 | "repr": "8" 80 | } 81 | }, 82 | "groups": {} 83 | } 84 | } 85 | } 86 | } 87 | } -------------------------------------------------------------------------------- /swarm_planner/missions/mission_16agents_15.json: -------------------------------------------------------------------------------- 1 | { 2 | "quadrotors": { 3 | "crazyflie": {"max_vel": [1.7, 1.7, 1.7], "max_acc": [6.2, 6.2, 6.2], "radius": 0.15, "speed": 1.0 }, 4 | "default": {"max_vel": [1.7, 1.7, 1.7], "max_acc": [6.2, 6.2, 6.2], "radius": 0.15, "speed": 1.0 } 5 | }, 6 | 7 | "agents": [ 8 | {"name": "crazyflie", "start": [4.0, 0.0, 1.0], "goal": [-4.0, 0.0, 1.0], "radius": 0.15, "speed": 1.0 }, 9 | {"name": "crazyflie", "start": [4.0, 2.0, 1.0], "goal": [-4.0, -2.0, 1.0], "radius": 0.15, "speed": 1.0 }, 10 | {"name": "crazyflie", "start": [4.0, 4.0, 1.0], "goal": [-4.0, -4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 11 | {"name": "crazyflie", "start": [2.0, 4.0, 1.0], "goal": [-2.0, -4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 12 | {"name": "crazyflie", "start": [0.0, 4.0, 1.0], "goal": [0.0, -4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 13 | {"name": "crazyflie", "start": [-2.0, 4.0, 1.0], "goal": [2.0, -4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 14 | {"name": "crazyflie", "start": [-4.0, 4.0, 1.0], "goal": [4.0, -4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 15 | {"name": "crazyflie", "start": [-4.0, 2.0, 1.0], "goal": [4.0, -2.0, 1.0], "radius": 0.15, "speed": 1.0 }, 16 | {"name": "crazyflie", "start": [-4.0, 0.0, 1.0], "goal": [4.0, 0.0, 1.0], "radius": 0.15, "speed": 1.0 }, 17 | {"name": "crazyflie", "start": [-4.0, -2.0, 1.0], "goal": [4.0, 2.0, 1.0], "radius": 0.15, "speed": 1.0 }, 18 | {"name": "crazyflie", "start": [-4.0, -4.0, 1.0], "goal": [4.0, 4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 19 | {"name": "crazyflie", "start": [-2.0, -4.0, 1.0], "goal": [2.0, 4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 20 | {"name": "crazyflie", "start": [0.0, -4.0, 1.0], "goal": [0.0, 4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 21 | {"name": "crazyflie", "start": [2.0, -4.0, 1.0], "goal": [-2.0, 4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 22 | {"name": "crazyflie", "start": [4.0, -4.0, 1.0], "goal": [-4.0, 4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 23 | {"name": "crazyflie", "start": [4.0, -2.0, 1.0], "goal": [-4.0, 2.0, 1.0], "radius": 0.15, "speed": 1.0 } 24 | ] 25 | } 26 | 27 | -------------------------------------------------------------------------------- /swarm_planner/missions/mission_16agents_20.json: -------------------------------------------------------------------------------- 1 | { 2 | "quadrotors": { 3 | "crazyflie": {"max_vel": [1.7, 1.7, 1.7], "max_acc": [6.2, 6.2, 6.2], "radius": 0.15, "speed": 1.0 }, 4 | "default": {"max_vel": [1.7, 1.7, 1.7], "max_acc": [6.2, 6.2, 6.2], "radius": 0.15, "speed": 1.0 } 5 | }, 6 | 7 | "agents": [ 8 | {"name": "crazyflie", "start": [4.0, 0.0, 1.0], "goal": [-4.0, 0.0, 1.0], "radius": 0.20, "speed": 1.0 }, 9 | {"name": "crazyflie", "start": [4.0, 2.0, 1.0], "goal": [-4.0, -2.0, 1.0], "radius": 0.20, "speed": 1.0 }, 10 | {"name": "crazyflie", "start": [4.0, 4.0, 1.0], "goal": [-4.0, -4.0, 1.0], "radius": 0.20, "speed": 1.0 }, 11 | {"name": "crazyflie", "start": [2.0, 4.0, 1.0], "goal": [-2.0, -4.0, 1.0], "radius": 0.20, "speed": 1.0 }, 12 | {"name": "crazyflie", "start": [0.0, 4.0, 1.0], "goal": [0.0, -4.0, 1.0], "radius": 0.20, "speed": 1.0 }, 13 | {"name": "crazyflie", "start": [-2.0, 4.0, 1.0], "goal": [2.0, -4.0, 1.0], "radius": 0.20, "speed": 1.0 }, 14 | {"name": "crazyflie", "start": [-4.0, 4.0, 1.0], "goal": [4.0, -4.0, 1.0], "radius": 0.20, "speed": 1.0 }, 15 | {"name": "crazyflie", "start": [-4.0, 2.0, 1.0], "goal": [4.0, -2.0, 1.0], "radius": 0.20, "speed": 1.0 }, 16 | {"name": "crazyflie", "start": [-4.0, 0.0, 1.0], "goal": [4.0, 0.0, 1.0], "radius": 0.20, "speed": 1.0 }, 17 | {"name": "crazyflie", "start": [-4.0, -2.0, 1.0], "goal": [4.0, 2.0, 1.0], "radius": 0.20, "speed": 1.0 }, 18 | {"name": "crazyflie", "start": [-4.0, -4.0, 1.0], "goal": [4.0, 4.0, 1.0], "radius": 0.20, "speed": 1.0 }, 19 | {"name": "crazyflie", "start": [-2.0, -4.0, 1.0], "goal": [2.0, 4.0, 1.0], "radius": 0.20, "speed": 1.0 }, 20 | {"name": "crazyflie", "start": [0.0, -4.0, 1.0], "goal": [0.0, 4.0, 1.0], "radius": 0.20, "speed": 1.0 }, 21 | {"name": "crazyflie", "start": [2.0, -4.0, 1.0], "goal": [-2.0, 4.0, 1.0], "radius": 0.20, "speed": 1.0 }, 22 | {"name": "crazyflie", "start": [4.0, -4.0, 1.0], "goal": [-4.0, 4.0, 1.0], "radius": 0.20, "speed": 1.0 }, 23 | {"name": "crazyflie", "start": [4.0, -2.0, 1.0], "goal": [-4.0, 2.0, 1.0], "radius": 0.20, "speed": 1.0 } 24 | ] 25 | } 26 | 27 | -------------------------------------------------------------------------------- /swarm_planner/missions/mission_16agents_25.json: -------------------------------------------------------------------------------- 1 | { 2 | "quadrotors": { 3 | "crazyflie": {"max_vel": [1.7, 1.7, 1.7], "max_acc": [6.2, 6.2, 6.2], "radius": 0.15, "speed": 1.0 }, 4 | "default": {"max_vel": [1.7, 1.7, 1.7], "max_acc": [6.2, 6.2, 6.2], "radius": 0.15, "speed": 1.0 } 5 | }, 6 | 7 | "agents": [ 8 | {"name": "crazyflie", "start": [4.0, 0.0, 1.0], "goal": [-4.0, 0.0, 1.0], "radius": 0.25, "speed": 1.0 }, 9 | {"name": "crazyflie", "start": [4.0, 2.0, 1.0], "goal": [-4.0, -2.0, 1.0], "radius": 0.25, "speed": 1.0 }, 10 | {"name": "crazyflie", "start": [4.0, 4.0, 1.0], "goal": [-4.0, -4.0, 1.0], "radius": 0.25, "speed": 1.0 }, 11 | {"name": "crazyflie", "start": [2.0, 4.0, 1.0], "goal": [-2.0, -4.0, 1.0], "radius": 0.25, "speed": 1.0 }, 12 | {"name": "crazyflie", "start": [0.0, 4.0, 1.0], "goal": [0.0, -4.0, 1.0], "radius": 0.25, "speed": 1.0 }, 13 | {"name": "crazyflie", "start": [-2.0, 4.0, 1.0], "goal": [2.0, -4.0, 1.0], "radius": 0.25, "speed": 1.0 }, 14 | {"name": "crazyflie", "start": [-4.0, 4.0, 1.0], "goal": [4.0, -4.0, 1.0], "radius": 0.25, "speed": 1.0 }, 15 | {"name": "crazyflie", "start": [-4.0, 2.0, 1.0], "goal": [4.0, -2.0, 1.0], "radius": 0.25, "speed": 1.0 }, 16 | {"name": "crazyflie", "start": [-4.0, 0.0, 1.0], "goal": [4.0, 0.0, 1.0], "radius": 0.25, "speed": 1.0 }, 17 | {"name": "crazyflie", "start": [-4.0, -2.0, 1.0], "goal": [4.0, 2.0, 1.0], "radius": 0.25, "speed": 1.0 }, 18 | {"name": "crazyflie", "start": [-4.0, -4.0, 1.0], "goal": [4.0, 4.0, 1.0], "radius": 0.25, "speed": 1.0 }, 19 | {"name": "crazyflie", "start": [-2.0, -4.0, 1.0], "goal": [2.0, 4.0, 1.0], "radius": 0.25, "speed": 1.0 }, 20 | {"name": "crazyflie", "start": [0.0, -4.0, 1.0], "goal": [0.0, 4.0, 1.0], "radius": 0.25, "speed": 1.0 }, 21 | {"name": "crazyflie", "start": [2.0, -4.0, 1.0], "goal": [-2.0, 4.0, 1.0], "radius": 0.25, "speed": 1.0 }, 22 | {"name": "crazyflie", "start": [4.0, -4.0, 1.0], "goal": [-4.0, 4.0, 1.0], "radius": 0.25, "speed": 1.0 }, 23 | {"name": "crazyflie", "start": [4.0, -2.0, 1.0], "goal": [-4.0, 2.0, 1.0], "radius": 0.25, "speed": 1.0 } 24 | ] 25 | } 26 | 27 | -------------------------------------------------------------------------------- /swarm_planner/missions/mission_16agents_30.json: -------------------------------------------------------------------------------- 1 | { 2 | "quadrotors": { 3 | "crazyflie": {"max_vel": [1.7, 1.7, 1.7], "max_acc": [6.2, 6.2, 6.2], "radius": 0.15, "speed": 1.0 }, 4 | "default": {"max_vel": [1.7, 1.7, 1.7], "max_acc": [6.2, 6.2, 6.2], "radius": 0.15, "speed": 1.0 } 5 | }, 6 | 7 | "agents": [ 8 | {"name": "crazyflie", "start": [4.0, 0.0, 1.0], "goal": [-4.0, 0.0, 1.0], "radius": 0.30, "speed": 1.0 }, 9 | {"name": "crazyflie", "start": [4.0, 2.0, 1.0], "goal": [-4.0, -2.0, 1.0], "radius": 0.30, "speed": 1.0 }, 10 | {"name": "crazyflie", "start": [4.0, 4.0, 1.0], "goal": [-4.0, -4.0, 1.0], "radius": 0.30, "speed": 1.0 }, 11 | {"name": "crazyflie", "start": [2.0, 4.0, 1.0], "goal": [-2.0, -4.0, 1.0], "radius": 0.30, "speed": 1.0 }, 12 | {"name": "crazyflie", "start": [0.0, 4.0, 1.0], "goal": [0.0, -4.0, 1.0], "radius": 0.30, "speed": 1.0 }, 13 | {"name": "crazyflie", "start": [-2.0, 4.0, 1.0], "goal": [2.0, -4.0, 1.0], "radius": 0.30, "speed": 1.0 }, 14 | {"name": "crazyflie", "start": [-4.0, 4.0, 1.0], "goal": [4.0, -4.0, 1.0], "radius": 0.30, "speed": 1.0 }, 15 | {"name": "crazyflie", "start": [-4.0, 2.0, 1.0], "goal": [4.0, -2.0, 1.0], "radius": 0.30, "speed": 1.0 }, 16 | {"name": "crazyflie", "start": [-4.0, 0.0, 1.0], "goal": [4.0, 0.0, 1.0], "radius": 0.30, "speed": 1.0 }, 17 | {"name": "crazyflie", "start": [-4.0, -2.0, 1.0], "goal": [4.0, 2.0, 1.0], "radius": 0.30, "speed": 1.0 }, 18 | {"name": "crazyflie", "start": [-4.0, -4.0, 1.0], "goal": [4.0, 4.0, 1.0], "radius": 0.30, "speed": 1.0 }, 19 | {"name": "crazyflie", "start": [-2.0, -4.0, 1.0], "goal": [2.0, 4.0, 1.0], "radius": 0.30, "speed": 1.0 }, 20 | {"name": "crazyflie", "start": [0.0, -4.0, 1.0], "goal": [0.0, 4.0, 1.0], "radius": 0.30, "speed": 1.0 }, 21 | {"name": "crazyflie", "start": [2.0, -4.0, 1.0], "goal": [-2.0, 4.0, 1.0], "radius": 0.30, "speed": 1.0 }, 22 | {"name": "crazyflie", "start": [4.0, -4.0, 1.0], "goal": [-4.0, 4.0, 1.0], "radius": 0.30, "speed": 1.0 }, 23 | {"name": "crazyflie", "start": [4.0, -2.0, 1.0], "goal": [-4.0, 2.0, 1.0], "radius": 0.30, "speed": 1.0 } 24 | ] 25 | } 26 | 27 | -------------------------------------------------------------------------------- /swarm_planner/missions/mission_16agents_35.json: -------------------------------------------------------------------------------- 1 | { 2 | "quadrotors": { 3 | "crazyflie": {"max_vel": [1.7, 1.7, 1.7], "max_acc": [6.2, 6.2, 6.2], "radius": 0.15, "speed": 1.0 }, 4 | "default": {"max_vel": [1.7, 1.7, 1.7], "max_acc": [6.2, 6.2, 6.2], "radius": 0.15, "speed": 1.0 } 5 | }, 6 | 7 | "agents": [ 8 | {"name": "crazyflie", "start": [4.0, 0.0, 1.0], "goal": [-4.0, 0.0, 1.0], "radius": 0.35, "speed": 1.0 }, 9 | {"name": "crazyflie", "start": [4.0, 2.0, 1.0], "goal": [-4.0, -2.0, 1.0], "radius": 0.35, "speed": 1.0 }, 10 | {"name": "crazyflie", "start": [4.0, 4.0, 1.0], "goal": [-4.0, -4.0, 1.0], "radius": 0.35, "speed": 1.0 }, 11 | {"name": "crazyflie", "start": [2.0, 4.0, 1.0], "goal": [-2.0, -4.0, 1.0], "radius": 0.35, "speed": 1.0 }, 12 | {"name": "crazyflie", "start": [0.0, 4.0, 1.0], "goal": [0.0, -4.0, 1.0], "radius": 0.35, "speed": 1.0 }, 13 | {"name": "crazyflie", "start": [-2.0, 4.0, 1.0], "goal": [2.0, -4.0, 1.0], "radius": 0.35, "speed": 1.0 }, 14 | {"name": "crazyflie", "start": [-4.0, 4.0, 1.0], "goal": [4.0, -4.0, 1.0], "radius": 0.35, "speed": 1.0 }, 15 | {"name": "crazyflie", "start": [-4.0, 2.0, 1.0], "goal": [4.0, -2.0, 1.0], "radius": 0.35, "speed": 1.0 }, 16 | {"name": "crazyflie", "start": [-4.0, 0.0, 1.0], "goal": [4.0, 0.0, 1.0], "radius": 0.35, "speed": 1.0 }, 17 | {"name": "crazyflie", "start": [-4.0, -2.0, 1.0], "goal": [4.0, 2.0, 1.0], "radius": 0.35, "speed": 1.0 }, 18 | {"name": "crazyflie", "start": [-4.0, -4.0, 1.0], "goal": [4.0, 4.0, 1.0], "radius": 0.35, "speed": 1.0 }, 19 | {"name": "crazyflie", "start": [-2.0, -4.0, 1.0], "goal": [2.0, 4.0, 1.0], "radius": 0.35, "speed": 1.0 }, 20 | {"name": "crazyflie", "start": [0.0, -4.0, 1.0], "goal": [0.0, 4.0, 1.0], "radius": 0.35, "speed": 1.0 }, 21 | {"name": "crazyflie", "start": [2.0, -4.0, 1.0], "goal": [-2.0, 4.0, 1.0], "radius": 0.35, "speed": 1.0 }, 22 | {"name": "crazyflie", "start": [4.0, -4.0, 1.0], "goal": [-4.0, 4.0, 1.0], "radius": 0.35, "speed": 1.0 }, 23 | {"name": "crazyflie", "start": [4.0, -2.0, 1.0], "goal": [-4.0, 2.0, 1.0], "radius": 0.35, "speed": 1.0 } 24 | ] 25 | } 26 | 27 | -------------------------------------------------------------------------------- /swarm_planner/missions/mission_2agents_25.json: -------------------------------------------------------------------------------- 1 | { 2 | "quadrotors": { 3 | "crazyflie": {"max_vel": [1.7, 1.7, 1.7], "max_acc": [6.2, 6.2, 6.2], "radius": 0.15, "speed": 1.0 }, 4 | "default": {"max_vel": [1.7, 1.7, 1.7], "max_acc": [6.2, 6.2, 6.2], "radius": 0.15, "speed": 1.0 } 5 | }, 6 | 7 | "agents": [ 8 | {"name": "crazyflie", "start": [-1.0, 0.0, 0.5], "goal": [1.0, 0.0, 0.5], "radius": 0.25, "speed": 1.0 }, 9 | {"name": "crazyflie", "start": [1.0, 0.0, 0.5], "goal": [-1.0, 0.0, 0.5], "radius": 0.25, "speed": 1.0 } 10 | ] 11 | } 12 | 13 | -------------------------------------------------------------------------------- /swarm_planner/missions/mission_2agents_50.json: -------------------------------------------------------------------------------- 1 | { 2 | "quadrotors": { 3 | "crazyflie": {"max_vel": [1.7, 1.7, 1.7], "max_acc": [6.2, 6.2, 6.2], "radius": 0.15, "speed": 1.0 }, 4 | "default": {"max_vel": [1.7, 1.7, 1.7], "max_acc": [6.2, 6.2, 6.2], "radius": 0.15, "speed": 1.0 } 5 | }, 6 | 7 | "agents": [ 8 | {"name": "crazyflie", "start": [-1.0, 0.0, 0.5], "goal": [1.0, 0.0, 0.5], "radius": 0.4, "speed": 1.0 }, 9 | {"name": "crazyflie", "start": [1.0, 0.0, 0.5], "goal": [-1.0, 0.0, 0.5], "radius": 0.4, "speed": 1.0 } 10 | ] 11 | } 12 | 13 | -------------------------------------------------------------------------------- /swarm_planner/missions/mission_32agents_12.json: -------------------------------------------------------------------------------- 1 | { 2 | "quadrotors": { 3 | "crazyflie": {"max_vel": [1.7, 1.7, 1.7], "max_acc": [6.2, 6.2, 6.2], "radius": 0.15, "speed": 1.0 }, 4 | "default": {"max_vel": [1.7, 1.7, 1.7], "max_acc": [6.2, 6.2, 6.2], "radius": 0.15, "speed": 1.0 } 5 | }, 6 | 7 | "agents": [ 8 | {"name": "crazyflie", "start": [4.0, 0.0, 1.0], "goal": [-4.0, 0.0, 1.0], "radius": 0.12, "speed": 1.0 }, 9 | {"name": "crazyflie", "start": [4.0, 1.0, 1.0], "goal": [-4.0, -1.0, 1.0], "radius": 0.12, "speed": 1.0 }, 10 | {"name": "crazyflie", "start": [4.0, 2.0, 1.0], "goal": [-4.0, -2.0, 1.0], "radius": 0.12, "speed": 1.0 }, 11 | {"name": "crazyflie", "start": [4.0, 3.0, 1.0], "goal": [-4.0, -3.0, 1.0], "radius": 0.12, "speed": 1.0 }, 12 | {"name": "crazyflie", "start": [4.0, 4.0, 1.0], "goal": [-4.0, -4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 13 | {"name": "crazyflie", "start": [3.0, 4.0, 1.0], "goal": [-3.0, -4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 14 | {"name": "crazyflie", "start": [2.0, 4.0, 1.0], "goal": [-2.0, -4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 15 | {"name": "crazyflie", "start": [1.0, 4.0, 1.0], "goal": [-1.0, -4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 16 | {"name": "crazyflie", "start": [0.0, 4.0, 1.0], "goal": [0.0, -4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 17 | {"name": "crazyflie", "start": [-1.0, 4.0, 1.0], "goal": [1.0, -4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 18 | {"name": "crazyflie", "start": [-2.0, 4.0, 1.0], "goal": [2.0, -4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 19 | {"name": "crazyflie", "start": [-3.0, 4.0, 1.0], "goal": [3.0, -4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 20 | {"name": "crazyflie", "start": [-4.0, 4.0, 1.0], "goal": [4.0, -4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 21 | {"name": "crazyflie", "start": [-4.0, 3.0, 1.0], "goal": [4.0, -3.0, 1.0], "radius": 0.12, "speed": 1.0 }, 22 | {"name": "crazyflie", "start": [-4.0, 2.0, 1.0], "goal": [4.0, -2.0, 1.0], "radius": 0.12, "speed": 1.0 }, 23 | {"name": "crazyflie", "start": [-4.0, 1.0, 1.0], "goal": [4.0, -1.0, 1.0], "radius": 0.12, "speed": 1.0 }, 24 | {"name": "crazyflie", "start": [-4.0, 0.0, 1.0], "goal": [4.0, 0.0, 1.0], "radius": 0.12, "speed": 1.0 }, 25 | {"name": "crazyflie", "start": [-4.0, -1.0, 1.0], "goal": [4.0, 1.0, 1.0], "radius": 0.12, "speed": 1.0 }, 26 | {"name": "crazyflie", "start": [-4.0, -2.0, 1.0], "goal": [4.0, 2.0, 1.0], "radius": 0.12, "speed": 1.0 }, 27 | {"name": "crazyflie", "start": [-4.0, -3.0, 1.0], "goal": [4.0, 3.0, 1.0], "radius": 0.12, "speed": 1.0 }, 28 | {"name": "crazyflie", "start": [-4.0, -4.0, 1.0], "goal": [4.0, 4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 29 | {"name": "crazyflie", "start": [-3.0, -4.0, 1.0], "goal": [3.0, 4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 30 | {"name": "crazyflie", "start": [-2.0, -4.0, 1.0], "goal": [2.0, 4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 31 | {"name": "crazyflie", "start": [-1.0, -4.0, 1.0], "goal": [1.0, 4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 32 | {"name": "crazyflie", "start": [0.0, -4.0, 1.0], "goal": [0.0, 4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 33 | {"name": "crazyflie", "start": [1.0, -4.0, 1.0], "goal": [-1.0, 4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 34 | {"name": "crazyflie", "start": [2.0, -4.0, 1.0], "goal": [-2.0, 4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 35 | {"name": "crazyflie", "start": [3.0, -4.0, 1.0], "goal": [-3.0, 4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 36 | {"name": "crazyflie", "start": [4.0, -4.0, 1.0], "goal": [-4.0, 4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 37 | {"name": "crazyflie", "start": [4.0, -3.0, 1.0], "goal": [-4.0, 3.0, 1.0], "radius": 0.12, "speed": 1.0 }, 38 | {"name": "crazyflie", "start": [4.0, -2.0, 1.0], "goal": [-4.0, 2.0, 1.0], "radius": 0.12, "speed": 1.0 }, 39 | {"name": "crazyflie", "start": [4.0, -1.0, 1.0], "goal": [-4.0, 1.0, 1.0], "radius": 0.12, "speed": 1.0 } 40 | ] 41 | } 42 | 43 | -------------------------------------------------------------------------------- /swarm_planner/missions/mission_32agents_15.json: -------------------------------------------------------------------------------- 1 | { 2 | "quadrotors": { 3 | "crazyflie": {"max_vel": [1.7, 1.7, 1.7], "max_acc": [6.2, 6.2, 6.2], "radius": 0.15, "speed": 1.0 }, 4 | "default": {"max_vel": [1.7, 1.7, 1.7], "max_acc": [6.2, 6.2, 6.2], "radius": 0.15, "speed": 1.0 } 5 | }, 6 | 7 | "agents": [ 8 | {"name": "crazyflie", "start": [4.0, 0.0, 1.0], "goal": [-4.0, 0.0, 1.0], "radius": 0.15, "speed": 1.0 }, 9 | {"name": "crazyflie", "start": [4.0, 1.0, 1.0], "goal": [-4.0, -1.0, 1.0], "radius": 0.15, "speed": 1.0 }, 10 | {"name": "crazyflie", "start": [4.0, 2.0, 1.0], "goal": [-4.0, -2.0, 1.0], "radius": 0.15, "speed": 1.0 }, 11 | {"name": "crazyflie", "start": [4.0, 3.0, 1.0], "goal": [-4.0, -3.0, 1.0], "radius": 0.15, "speed": 1.0 }, 12 | {"name": "crazyflie", "start": [4.0, 4.0, 1.0], "goal": [-4.0, -4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 13 | {"name": "crazyflie", "start": [3.0, 4.0, 1.0], "goal": [-3.0, -4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 14 | {"name": "crazyflie", "start": [2.0, 4.0, 1.0], "goal": [-2.0, -4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 15 | {"name": "crazyflie", "start": [1.0, 4.0, 1.0], "goal": [-1.0, -4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 16 | {"name": "crazyflie", "start": [0.0, 4.0, 1.0], "goal": [0.0, -4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 17 | {"name": "crazyflie", "start": [-1.0, 4.0, 1.0], "goal": [1.0, -4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 18 | {"name": "crazyflie", "start": [-2.0, 4.0, 1.0], "goal": [2.0, -4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 19 | {"name": "crazyflie", "start": [-3.0, 4.0, 1.0], "goal": [3.0, -4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 20 | {"name": "crazyflie", "start": [-4.0, 4.0, 1.0], "goal": [4.0, -4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 21 | {"name": "crazyflie", "start": [-4.0, 3.0, 1.0], "goal": [4.0, -3.0, 1.0], "radius": 0.15, "speed": 1.0 }, 22 | {"name": "crazyflie", "start": [-4.0, 2.0, 1.0], "goal": [4.0, -2.0, 1.0], "radius": 0.15, "speed": 1.0 }, 23 | {"name": "crazyflie", "start": [-4.0, 1.0, 1.0], "goal": [4.0, -1.0, 1.0], "radius": 0.15, "speed": 1.0 }, 24 | {"name": "crazyflie", "start": [-4.0, 0.0, 1.0], "goal": [4.0, 0.0, 1.0], "radius": 0.15, "speed": 1.0 }, 25 | {"name": "crazyflie", "start": [-4.0, -1.0, 1.0], "goal": [4.0, 1.0, 1.0], "radius": 0.15, "speed": 1.0 }, 26 | {"name": "crazyflie", "start": [-4.0, -2.0, 1.0], "goal": [4.0, 2.0, 1.0], "radius": 0.15, "speed": 1.0 }, 27 | {"name": "crazyflie", "start": [-4.0, -3.0, 1.0], "goal": [4.0, 3.0, 1.0], "radius": 0.15, "speed": 1.0 }, 28 | {"name": "crazyflie", "start": [-4.0, -4.0, 1.0], "goal": [4.0, 4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 29 | {"name": "crazyflie", "start": [-3.0, -4.0, 1.0], "goal": [3.0, 4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 30 | {"name": "crazyflie", "start": [-2.0, -4.0, 1.0], "goal": [2.0, 4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 31 | {"name": "crazyflie", "start": [-1.0, -4.0, 1.0], "goal": [1.0, 4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 32 | {"name": "crazyflie", "start": [0.0, -4.0, 1.0], "goal": [0.0, 4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 33 | {"name": "crazyflie", "start": [1.0, -4.0, 1.0], "goal": [-1.0, 4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 34 | {"name": "crazyflie", "start": [2.0, -4.0, 1.0], "goal": [-2.0, 4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 35 | {"name": "crazyflie", "start": [3.0, -4.0, 1.0], "goal": [-3.0, 4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 36 | {"name": "crazyflie", "start": [4.0, -4.0, 1.0], "goal": [-4.0, 4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 37 | {"name": "crazyflie", "start": [4.0, -3.0, 1.0], "goal": [-4.0, 3.0, 1.0], "radius": 0.15, "speed": 1.0 }, 38 | {"name": "crazyflie", "start": [4.0, -2.0, 1.0], "goal": [-4.0, 2.0, 1.0], "radius": 0.15, "speed": 1.0 }, 39 | {"name": "crazyflie", "start": [4.0, -1.0, 1.0], "goal": [-4.0, 1.0, 1.0], "radius": 0.15, "speed": 1.0 } 40 | ] 41 | } 42 | 43 | -------------------------------------------------------------------------------- /swarm_planner/missions/mission_4agents_15.json: -------------------------------------------------------------------------------- 1 | { 2 | "quadrotors": { 3 | "crazyflie": {"max_vel": [1.7, 1.7, 1.7], "max_acc": [6.2, 6.2, 6.2], "radius": 0.15, "speed": 1.0 }, 4 | "default": {"max_vel": [1.7, 1.7, 1.7], "max_acc": [6.2, 6.2, 6.2], "radius": 0.15, "speed": 1.0 } 5 | }, 6 | 7 | "agents": [ 8 | {"name": "crazyflie", "start": [4.0, 0.0, 1.0], "goal": [-4.0, 0.0, 1.0], "radius": 0.15, "speed": 1.0 }, 9 | {"name": "crazyflie", "start": [0.0, 4.0, 1.0], "goal": [0.0, -4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 10 | {"name": "crazyflie", "start": [-4.0, 0.0, 1.0], "goal": [4.0, 0.0, 1.0], "radius": 0.15, "speed": 1.0 }, 11 | {"name": "crazyflie", "start": [0.0, -4.0, 1.0], "goal": [0.0, 4.0, 1.0], "radius": 0.15, "speed": 1.0 } 12 | ] 13 | } 14 | 15 | -------------------------------------------------------------------------------- /swarm_planner/missions/mission_4agents_15_2.json: -------------------------------------------------------------------------------- 1 | { 2 | "quadrotors": { 3 | "crazyflie": {"max_vel": [1.7, 1.7, 1.7], "max_acc": [6.2, 6.2, 6.2], "radius": 0.15, "speed": 1.0 }, 4 | "default": {"max_vel": [1.7, 1.7, 1.7], "max_acc": [6.2, 6.2, 6.2], "radius": 0.15, "speed": 1.0 } 5 | }, 6 | 7 | "agents": [ 8 | {"name": "crazyflie", "start": [4.0, 4.0, 1.0], "goal": [-4.0, -4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 9 | {"name": "crazyflie", "start": [-4.0, 4.0, 1.0], "goal": [4.0, -4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 10 | {"name": "crazyflie", "start": [-4.0, -4.0, 1.0], "goal": [4.0, 4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 11 | {"name": "crazyflie", "start": [4.0, -4.0, 1.0], "goal": [-4.0, 4.0, 1.0], "radius": 0.15, "speed": 1.0 } 12 | ] 13 | } 14 | 15 | -------------------------------------------------------------------------------- /swarm_planner/missions/mission_4agents_25_aty.json: -------------------------------------------------------------------------------- 1 | { 2 | "quadrotors": { 3 | "crazyflie": {"max_vel": [1.7, 1.7, 1.7], "max_acc": [6.2, 6.2, 6.2], "radius": 0.15, "speed": 1.0 }, 4 | "default": {"max_vel": [1.7, 1.7, 1.7], "max_acc": [6.2, 6.2, 6.2], "radius": 0.15, "speed": 1.0 } 5 | }, 6 | 7 | "agents": [ 8 | {"name": "crazyflie", "start": [-7.0, 1.5, 0.5], "goal": [8.0, 1.5, 0.5], "radius": 0.25, "speed": 1.0 }, 9 | {"name": "crazyflie", "start": [7.0, 1.5, 0.5], "goal": [-8.0, 1.5, 0.5], "radius": 0.25, "speed": 1.0 }, 10 | {"name": "crazyflie", "start": [-8.0, 1.5, 0.5], "goal": [7.0, 1.5, 0.5], "radius": 0.25, "speed": 1.0 }, 11 | {"name": "crazyflie", "start": [8.0, 1.5, 0.5], "goal": [-7.0, 1.5, 0.5], "radius": 0.25, "speed": 1.0 } 12 | ] 13 | } 14 | 15 | -------------------------------------------------------------------------------- /swarm_planner/missions/mission_4agents_35_aty.json: -------------------------------------------------------------------------------- 1 | { 2 | "quadrotors": { 3 | "crazyflie": {"max_vel": [1.7, 1.7, 1.7], "max_acc": [6.2, 6.2, 6.2], "radius": 0.15, "speed": 1.0 }, 4 | "default": {"max_vel": [1.7, 1.7, 1.7], "max_acc": [6.2, 6.2, 6.2], "radius": 0.15, "speed": 1.0 } 5 | }, 6 | 7 | "agents": [ 8 | {"name": "crazyflie", "start": [-7.0, 1.5, 0.5], "goal": [8.0, 1.5, 0.5], "radius": 0.35, "speed": 1.0 }, 9 | {"name": "crazyflie", "start": [7.0, 1.5, 0.5], "goal": [-8.0, 1.5, 0.5], "radius": 0.35, "speed": 1.0 }, 10 | {"name": "crazyflie", "start": [-8.0, 1.5, 0.5], "goal": [7.0, 1.5, 0.5], "radius": 0.35, "speed": 1.0 }, 11 | {"name": "crazyflie", "start": [8.0, 1.5, 0.5], "goal": [-7.0, 1.5, 0.5], "radius": 0.35, "speed": 1.0 } 12 | ] 13 | } 14 | 15 | -------------------------------------------------------------------------------- /swarm_planner/missions/mission_64agents_12.json: -------------------------------------------------------------------------------- 1 | { 2 | "quadrotors": { 3 | "crazyflie": {"max_vel": [1.7, 1.7, 1.7], "max_acc": [6.2, 6.2, 6.2], "radius": 0.15, "speed": 1.0 }, 4 | "default": {"max_vel": [1.7, 1.7, 1.7], "max_acc": [6.2, 6.2, 6.2], "radius": 0.15, "speed": 1.0 } 5 | }, 6 | 7 | "agents": [ 8 | {"name": "crazyflie", "start": [4.0, 0.0, 1.0], "goal": [-4.0, 0.0, 1.0], "radius": 0.12, "speed": 1.0 }, 9 | {"name": "crazyflie", "start": [4.0, 0.5, 1.0], "goal": [-4.0, -0.5, 1.0], "radius": 0.12, "speed": 1.0 }, 10 | {"name": "crazyflie", "start": [4.0, 1.0, 1.0], "goal": [-4.0, -1.0, 1.0], "radius": 0.12, "speed": 1.0 }, 11 | {"name": "crazyflie", "start": [4.0, 1.5, 1.0], "goal": [-4.0, -1.5, 1.0], "radius": 0.12, "speed": 1.0 }, 12 | {"name": "crazyflie", "start": [4.0, 2.0, 1.0], "goal": [-4.0, -2.0, 1.0], "radius": 0.12, "speed": 1.0 }, 13 | {"name": "crazyflie", "start": [4.0, 2.5, 1.0], "goal": [-4.0, -2.5, 1.0], "radius": 0.12, "speed": 1.0 }, 14 | {"name": "crazyflie", "start": [4.0, 3.0, 1.0], "goal": [-4.0, -3.0, 1.0], "radius": 0.12, "speed": 1.0 }, 15 | {"name": "crazyflie", "start": [4.0, 3.5, 1.0], "goal": [-4.0, -3.5, 1.0], "radius": 0.12, "speed": 1.0 }, 16 | {"name": "crazyflie", "start": [4.0, 4.0, 1.0], "goal": [-4.0, -4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 17 | {"name": "crazyflie", "start": [3.5, 4.0, 1.0], "goal": [-3.5, -4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 18 | {"name": "crazyflie", "start": [3.0, 4.0, 1.0], "goal": [-3.0, -4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 19 | {"name": "crazyflie", "start": [2.5, 4.0, 1.0], "goal": [-2.5, -4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 20 | {"name": "crazyflie", "start": [2.0, 4.0, 1.0], "goal": [-2.0, -4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 21 | {"name": "crazyflie", "start": [1.5, 4.0, 1.0], "goal": [-1.5, -4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 22 | {"name": "crazyflie", "start": [1.0, 4.0, 1.0], "goal": [-1.0, -4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 23 | {"name": "crazyflie", "start": [0.5, 4.0, 1.0], "goal": [-0.5, -4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 24 | {"name": "crazyflie", "start": [0.0, 4.0, 1.0], "goal": [0.0, -4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 25 | {"name": "crazyflie", "start": [-0.5, 4.0, 1.0], "goal": [0.5, -4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 26 | {"name": "crazyflie", "start": [-1.0, 4.0, 1.0], "goal": [1.0, -4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 27 | {"name": "crazyflie", "start": [-1.5, 4.0, 1.0], "goal": [1.5, -4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 28 | {"name": "crazyflie", "start": [-2.0, 4.0, 1.0], "goal": [2.0, -4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 29 | {"name": "crazyflie", "start": [-2.5, 4.0, 1.0], "goal": [2.5, -4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 30 | {"name": "crazyflie", "start": [-3.0, 4.0, 1.0], "goal": [3.0, -4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 31 | {"name": "crazyflie", "start": [-3.5, 4.0, 1.0], "goal": [3.5, -4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 32 | {"name": "crazyflie", "start": [-4.0, 4.0, 1.0], "goal": [4.0, -4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 33 | {"name": "crazyflie", "start": [-4.0, 3.5, 1.0], "goal": [4.0, -3.5, 1.0], "radius": 0.12, "speed": 1.0 }, 34 | {"name": "crazyflie", "start": [-4.0, 3.0, 1.0], "goal": [4.0, -3.0, 1.0], "radius": 0.12, "speed": 1.0 }, 35 | {"name": "crazyflie", "start": [-4.0, 2.5, 1.0], "goal": [4.0, -2.5, 1.0], "radius": 0.12, "speed": 1.0 }, 36 | {"name": "crazyflie", "start": [-4.0, 2.0, 1.0], "goal": [4.0, -2.0, 1.0], "radius": 0.12, "speed": 1.0 }, 37 | {"name": "crazyflie", "start": [-4.0, 1.5, 1.0], "goal": [4.0, -1.5, 1.0], "radius": 0.12, "speed": 1.0 }, 38 | {"name": "crazyflie", "start": [-4.0, 1.0, 1.0], "goal": [4.0, -1.0, 1.0], "radius": 0.12, "speed": 1.0 }, 39 | {"name": "crazyflie", "start": [-4.0, 0.5, 1.0], "goal": [4.0, -0.5, 1.0], "radius": 0.12, "speed": 1.0 }, 40 | {"name": "crazyflie", "start": [-4.0, 0.0, 1.0], "goal": [4.0, 0.0, 1.0], "radius": 0.12, "speed": 1.0 }, 41 | {"name": "crazyflie", "start": [-4.0, -0.5, 1.0], "goal": [4.0, 0.5, 1.0], "radius": 0.12, "speed": 1.0 }, 42 | {"name": "crazyflie", "start": [-4.0, -1.0, 1.0], "goal": [4.0, 1.0, 1.0], "radius": 0.12, "speed": 1.0 }, 43 | {"name": "crazyflie", "start": [-4.0, -1.5, 1.0], "goal": [4.0, 1.5, 1.0], "radius": 0.12, "speed": 1.0 }, 44 | {"name": "crazyflie", "start": [-4.0, -2.0, 1.0], "goal": [4.0, 2.0, 1.0], "radius": 0.12, "speed": 1.0 }, 45 | {"name": "crazyflie", "start": [-4.0, -2.5, 1.0], "goal": [4.0, 2.5, 1.0], "radius": 0.12, "speed": 1.0 }, 46 | {"name": "crazyflie", "start": [-4.0, -3.0, 1.0], "goal": [4.0, 3.0, 1.0], "radius": 0.12, "speed": 1.0 }, 47 | {"name": "crazyflie", "start": [-4.0, -3.5, 1.0], "goal": [4.0, 3.5, 1.0], "radius": 0.12, "speed": 1.0 }, 48 | {"name": "crazyflie", "start": [-4.0, -4.0, 1.0], "goal": [4.0, 4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 49 | {"name": "crazyflie", "start": [-3.5, -4.0, 1.0], "goal": [3.5, 4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 50 | {"name": "crazyflie", "start": [-3.0, -4.0, 1.0], "goal": [3.0, 4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 51 | {"name": "crazyflie", "start": [-2.5, -4.0, 1.0], "goal": [2.5, 4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 52 | {"name": "crazyflie", "start": [-2.0, -4.0, 1.0], "goal": [2.0, 4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 53 | {"name": "crazyflie", "start": [-1.5, -4.0, 1.0], "goal": [1.5, 4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 54 | {"name": "crazyflie", "start": [-1.0, -4.0, 1.0], "goal": [1.0, 4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 55 | {"name": "crazyflie", "start": [-0.5, -4.0, 1.0], "goal": [0.5, 4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 56 | {"name": "crazyflie", "start": [0.0, -4.0, 1.0], "goal": [0.0, 4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 57 | {"name": "crazyflie", "start": [0.5, -4.0, 1.0], "goal": [-0.5, 4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 58 | {"name": "crazyflie", "start": [1.0, -4.0, 1.0], "goal": [-1.0, 4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 59 | {"name": "crazyflie", "start": [1.5, -4.0, 1.0], "goal": [-1.5, 4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 60 | {"name": "crazyflie", "start": [2.0, -4.0, 1.0], "goal": [-2.0, 4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 61 | {"name": "crazyflie", "start": [2.5, -4.0, 1.0], "goal": [-2.5, 4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 62 | {"name": "crazyflie", "start": [3.0, -4.0, 1.0], "goal": [-3.0, 4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 63 | {"name": "crazyflie", "start": [3.5, -4.0, 1.0], "goal": [-3.5, 4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 64 | {"name": "crazyflie", "start": [4.0, -4.0, 1.0], "goal": [-4.0, 4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 65 | {"name": "crazyflie", "start": [4.0, -3.5, 1.0], "goal": [-4.0, 3.5, 1.0], "radius": 0.12, "speed": 1.0 }, 66 | {"name": "crazyflie", "start": [4.0, -3.0, 1.0], "goal": [-4.0, 3.0, 1.0], "radius": 0.12, "speed": 1.0 }, 67 | {"name": "crazyflie", "start": [4.0, -2.5, 1.0], "goal": [-4.0, 2.5, 1.0], "radius": 0.12, "speed": 1.0 }, 68 | {"name": "crazyflie", "start": [4.0, -2.0, 1.0], "goal": [-4.0, 2.0, 1.0], "radius": 0.12, "speed": 1.0 }, 69 | {"name": "crazyflie", "start": [4.0, -1.5, 1.0], "goal": [-4.0, 1.5, 1.0], "radius": 0.12, "speed": 1.0 }, 70 | {"name": "crazyflie", "start": [4.0, -1.0, 1.0], "goal": [-4.0, 1.0, 1.0], "radius": 0.12, "speed": 1.0 }, 71 | {"name": "crazyflie", "start": [4.0, -0.5, 1.0], "goal": [-4.0, 0.5, 1.0], "radius": 0.12, "speed": 1.0 } 72 | ] 73 | } 74 | 75 | -------------------------------------------------------------------------------- /swarm_planner/missions/mission_64agents_15.json: -------------------------------------------------------------------------------- 1 | { 2 | "quadrotors": { 3 | "crazyflie": {"max_vel": [1.7, 1.7, 1.7], "max_acc": [6.2, 6.2, 6.2], "radius": 0.15, "speed": 1.0 }, 4 | "default": {"max_vel": [1.7, 1.7, 1.7], "max_acc": [6.2, 6.2, 6.2], "radius": 0.15, "speed": 1.0 } 5 | }, 6 | 7 | "agents": [ 8 | {"name": "crazyflie", "start": [4.0, 0.0, 1.0], "goal": [-4.0, 0.0, 1.0], "radius": 0.15, "speed": 1.0 }, 9 | {"name": "crazyflie", "start": [4.0, 0.5, 1.0], "goal": [-4.0, -0.5, 1.0], "radius": 0.15, "speed": 1.0 }, 10 | {"name": "crazyflie", "start": [4.0, 1.0, 1.0], "goal": [-4.0, -1.0, 1.0], "radius": 0.15, "speed": 1.0 }, 11 | {"name": "crazyflie", "start": [4.0, 1.5, 1.0], "goal": [-4.0, -1.5, 1.0], "radius": 0.15, "speed": 1.0 }, 12 | {"name": "crazyflie", "start": [4.0, 2.0, 1.0], "goal": [-4.0, -2.0, 1.0], "radius": 0.15, "speed": 1.0 }, 13 | {"name": "crazyflie", "start": [4.0, 2.5, 1.0], "goal": [-4.0, -2.5, 1.0], "radius": 0.15, "speed": 1.0 }, 14 | {"name": "crazyflie", "start": [4.0, 3.0, 1.0], "goal": [-4.0, -3.0, 1.0], "radius": 0.15, "speed": 1.0 }, 15 | {"name": "crazyflie", "start": [4.0, 3.5, 1.0], "goal": [-4.0, -3.5, 1.0], "radius": 0.15, "speed": 1.0 }, 16 | {"name": "crazyflie", "start": [4.0, 4.0, 1.0], "goal": [-4.0, -4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 17 | {"name": "crazyflie", "start": [3.5, 4.0, 1.0], "goal": [-3.5, -4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 18 | {"name": "crazyflie", "start": [3.0, 4.0, 1.0], "goal": [-3.0, -4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 19 | {"name": "crazyflie", "start": [2.5, 4.0, 1.0], "goal": [-2.5, -4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 20 | {"name": "crazyflie", "start": [2.0, 4.0, 1.0], "goal": [-2.0, -4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 21 | {"name": "crazyflie", "start": [1.5, 4.0, 1.0], "goal": [-1.5, -4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 22 | {"name": "crazyflie", "start": [1.0, 4.0, 1.0], "goal": [-1.0, -4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 23 | {"name": "crazyflie", "start": [0.5, 4.0, 1.0], "goal": [-0.5, -4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 24 | {"name": "crazyflie", "start": [0.0, 4.0, 1.0], "goal": [0.0, -4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 25 | {"name": "crazyflie", "start": [-0.5, 4.0, 1.0], "goal": [0.5, -4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 26 | {"name": "crazyflie", "start": [-1.0, 4.0, 1.0], "goal": [1.0, -4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 27 | {"name": "crazyflie", "start": [-1.5, 4.0, 1.0], "goal": [1.5, -4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 28 | {"name": "crazyflie", "start": [-2.0, 4.0, 1.0], "goal": [2.0, -4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 29 | {"name": "crazyflie", "start": [-2.5, 4.0, 1.0], "goal": [2.5, -4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 30 | {"name": "crazyflie", "start": [-3.0, 4.0, 1.0], "goal": [3.0, -4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 31 | {"name": "crazyflie", "start": [-3.5, 4.0, 1.0], "goal": [3.5, -4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 32 | {"name": "crazyflie", "start": [-4.0, 4.0, 1.0], "goal": [4.0, -4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 33 | {"name": "crazyflie", "start": [-4.0, 3.5, 1.0], "goal": [4.0, -3.5, 1.0], "radius": 0.15, "speed": 1.0 }, 34 | {"name": "crazyflie", "start": [-4.0, 3.0, 1.0], "goal": [4.0, -3.0, 1.0], "radius": 0.15, "speed": 1.0 }, 35 | {"name": "crazyflie", "start": [-4.0, 2.5, 1.0], "goal": [4.0, -2.5, 1.0], "radius": 0.15, "speed": 1.0 }, 36 | {"name": "crazyflie", "start": [-4.0, 2.0, 1.0], "goal": [4.0, -2.0, 1.0], "radius": 0.15, "speed": 1.0 }, 37 | {"name": "crazyflie", "start": [-4.0, 1.5, 1.0], "goal": [4.0, -1.5, 1.0], "radius": 0.15, "speed": 1.0 }, 38 | {"name": "crazyflie", "start": [-4.0, 1.0, 1.0], "goal": [4.0, -1.0, 1.0], "radius": 0.15, "speed": 1.0 }, 39 | {"name": "crazyflie", "start": [-4.0, 0.5, 1.0], "goal": [4.0, -0.5, 1.0], "radius": 0.15, "speed": 1.0 }, 40 | {"name": "crazyflie", "start": [-4.0, 0.0, 1.0], "goal": [4.0, 0.0, 1.0], "radius": 0.15, "speed": 1.0 }, 41 | {"name": "crazyflie", "start": [-4.0, -0.5, 1.0], "goal": [4.0, 0.5, 1.0], "radius": 0.15, "speed": 1.0 }, 42 | {"name": "crazyflie", "start": [-4.0, -1.0, 1.0], "goal": [4.0, 1.0, 1.0], "radius": 0.15, "speed": 1.0 }, 43 | {"name": "crazyflie", "start": [-4.0, -1.5, 1.0], "goal": [4.0, 1.5, 1.0], "radius": 0.15, "speed": 1.0 }, 44 | {"name": "crazyflie", "start": [-4.0, -2.0, 1.0], "goal": [4.0, 2.0, 1.0], "radius": 0.15, "speed": 1.0 }, 45 | {"name": "crazyflie", "start": [-4.0, -2.5, 1.0], "goal": [4.0, 2.5, 1.0], "radius": 0.15, "speed": 1.0 }, 46 | {"name": "crazyflie", "start": [-4.0, -3.0, 1.0], "goal": [4.0, 3.0, 1.0], "radius": 0.15, "speed": 1.0 }, 47 | {"name": "crazyflie", "start": [-4.0, -3.5, 1.0], "goal": [4.0, 3.5, 1.0], "radius": 0.15, "speed": 1.0 }, 48 | {"name": "crazyflie", "start": [-4.0, -4.0, 1.0], "goal": [4.0, 4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 49 | {"name": "crazyflie", "start": [-3.5, -4.0, 1.0], "goal": [3.5, 4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 50 | {"name": "crazyflie", "start": [-3.0, -4.0, 1.0], "goal": [3.0, 4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 51 | {"name": "crazyflie", "start": [-2.5, -4.0, 1.0], "goal": [2.5, 4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 52 | {"name": "crazyflie", "start": [-2.0, -4.0, 1.0], "goal": [2.0, 4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 53 | {"name": "crazyflie", "start": [-1.5, -4.0, 1.0], "goal": [1.5, 4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 54 | {"name": "crazyflie", "start": [-1.0, -4.0, 1.0], "goal": [1.0, 4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 55 | {"name": "crazyflie", "start": [-0.5, -4.0, 1.0], "goal": [0.5, 4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 56 | {"name": "crazyflie", "start": [0.0, -4.0, 1.0], "goal": [0.0, 4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 57 | {"name": "crazyflie", "start": [0.5, -4.0, 1.0], "goal": [-0.5, 4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 58 | {"name": "crazyflie", "start": [1.0, -4.0, 1.0], "goal": [-1.0, 4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 59 | {"name": "crazyflie", "start": [1.5, -4.0, 1.0], "goal": [-1.5, 4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 60 | {"name": "crazyflie", "start": [2.0, -4.0, 1.0], "goal": [-2.0, 4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 61 | {"name": "crazyflie", "start": [2.5, -4.0, 1.0], "goal": [-2.5, 4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 62 | {"name": "crazyflie", "start": [3.0, -4.0, 1.0], "goal": [-3.0, 4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 63 | {"name": "crazyflie", "start": [3.5, -4.0, 1.0], "goal": [-3.5, 4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 64 | {"name": "crazyflie", "start": [4.0, -4.0, 1.0], "goal": [-4.0, 4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 65 | {"name": "crazyflie", "start": [4.0, -3.5, 1.0], "goal": [-4.0, 3.5, 1.0], "radius": 0.15, "speed": 1.0 }, 66 | {"name": "crazyflie", "start": [4.0, -3.0, 1.0], "goal": [-4.0, 3.0, 1.0], "radius": 0.15, "speed": 1.0 }, 67 | {"name": "crazyflie", "start": [4.0, -2.5, 1.0], "goal": [-4.0, 2.5, 1.0], "radius": 0.15, "speed": 1.0 }, 68 | {"name": "crazyflie", "start": [4.0, -2.0, 1.0], "goal": [-4.0, 2.0, 1.0], "radius": 0.15, "speed": 1.0 }, 69 | {"name": "crazyflie", "start": [4.0, -1.5, 1.0], "goal": [-4.0, 1.5, 1.0], "radius": 0.15, "speed": 1.0 }, 70 | {"name": "crazyflie", "start": [4.0, -1.0, 1.0], "goal": [-4.0, 1.0, 1.0], "radius": 0.15, "speed": 1.0 }, 71 | {"name": "crazyflie", "start": [4.0, -0.5, 1.0], "goal": [-4.0, 0.5, 1.0], "radius": 0.15, "speed": 1.0 } 72 | ] 73 | } 74 | 75 | -------------------------------------------------------------------------------- /swarm_planner/missions/mission_64agents_20.json: -------------------------------------------------------------------------------- 1 | { 2 | "quadrotors": { 3 | "crazyflie": {"max_vel": [1.7, 1.7, 1.7], "max_acc": [6.2, 6.2, 6.2], "radius": 0.15, "speed": 1.0 }, 4 | "default": {"max_vel": [1.7, 1.7, 1.7], "max_acc": [6.2, 6.2, 6.2], "radius": 0.15, "speed": 1.0 } 5 | }, 6 | 7 | "agents": [ 8 | {"name": "crazyflie", "start": [4.0, 0.0, 1.0], "goal": [-4.0, 0.0, 1.0], "radius": 0.20, "speed": 1.0 }, 9 | {"name": "crazyflie", "start": [4.0, 0.5, 1.0], "goal": [-4.0, -0.5, 1.0], "radius": 0.20, "speed": 1.0 }, 10 | {"name": "crazyflie", "start": [4.0, 1.0, 1.0], "goal": [-4.0, -1.0, 1.0], "radius": 0.20, "speed": 1.0 }, 11 | {"name": "crazyflie", "start": [4.0, 1.5, 1.0], "goal": [-4.0, -1.5, 1.0], "radius": 0.20, "speed": 1.0 }, 12 | {"name": "crazyflie", "start": [4.0, 2.0, 1.0], "goal": [-4.0, -2.0, 1.0], "radius": 0.20, "speed": 1.0 }, 13 | {"name": "crazyflie", "start": [4.0, 2.5, 1.0], "goal": [-4.0, -2.5, 1.0], "radius": 0.20, "speed": 1.0 }, 14 | {"name": "crazyflie", "start": [4.0, 3.0, 1.0], "goal": [-4.0, -3.0, 1.0], "radius": 0.20, "speed": 1.0 }, 15 | {"name": "crazyflie", "start": [4.0, 3.5, 1.0], "goal": [-4.0, -3.5, 1.0], "radius": 0.20, "speed": 1.0 }, 16 | {"name": "crazyflie", "start": [4.0, 4.0, 1.0], "goal": [-4.0, -4.0, 1.0], "radius": 0.20, "speed": 1.0 }, 17 | {"name": "crazyflie", "start": [3.5, 4.0, 1.0], "goal": [-3.5, -4.0, 1.0], "radius": 0.20, "speed": 1.0 }, 18 | {"name": "crazyflie", "start": [3.0, 4.0, 1.0], "goal": [-3.0, -4.0, 1.0], "radius": 0.20, "speed": 1.0 }, 19 | {"name": "crazyflie", "start": [2.5, 4.0, 1.0], "goal": [-2.5, -4.0, 1.0], "radius": 0.20, "speed": 1.0 }, 20 | {"name": "crazyflie", "start": [2.0, 4.0, 1.0], "goal": [-2.0, -4.0, 1.0], "radius": 0.20, "speed": 1.0 }, 21 | {"name": "crazyflie", "start": [1.5, 4.0, 1.0], "goal": [-1.5, -4.0, 1.0], "radius": 0.20, "speed": 1.0 }, 22 | {"name": "crazyflie", "start": [1.0, 4.0, 1.0], "goal": [-1.0, -4.0, 1.0], "radius": 0.20, "speed": 1.0 }, 23 | {"name": "crazyflie", "start": [0.5, 4.0, 1.0], "goal": [-0.5, -4.0, 1.0], "radius": 0.20, "speed": 1.0 }, 24 | {"name": "crazyflie", "start": [0.0, 4.0, 1.0], "goal": [0.0, -4.0, 1.0], "radius": 0.20, "speed": 1.0 }, 25 | {"name": "crazyflie", "start": [-0.5, 4.0, 1.0], "goal": [0.5, -4.0, 1.0], "radius": 0.20, "speed": 1.0 }, 26 | {"name": "crazyflie", "start": [-1.0, 4.0, 1.0], "goal": [1.0, -4.0, 1.0], "radius": 0.20, "speed": 1.0 }, 27 | {"name": "crazyflie", "start": [-1.5, 4.0, 1.0], "goal": [1.5, -4.0, 1.0], "radius": 0.20, "speed": 1.0 }, 28 | {"name": "crazyflie", "start": [-2.0, 4.0, 1.0], "goal": [2.0, -4.0, 1.0], "radius": 0.20, "speed": 1.0 }, 29 | {"name": "crazyflie", "start": [-2.5, 4.0, 1.0], "goal": [2.5, -4.0, 1.0], "radius": 0.20, "speed": 1.0 }, 30 | {"name": "crazyflie", "start": [-3.0, 4.0, 1.0], "goal": [3.0, -4.0, 1.0], "radius": 0.20, "speed": 1.0 }, 31 | {"name": "crazyflie", "start": [-3.5, 4.0, 1.0], "goal": [3.5, -4.0, 1.0], "radius": 0.20, "speed": 1.0 }, 32 | {"name": "crazyflie", "start": [-4.0, 4.0, 1.0], "goal": [4.0, -4.0, 1.0], "radius": 0.20, "speed": 1.0 }, 33 | {"name": "crazyflie", "start": [-4.0, 3.5, 1.0], "goal": [4.0, -3.5, 1.0], "radius": 0.20, "speed": 1.0 }, 34 | {"name": "crazyflie", "start": [-4.0, 3.0, 1.0], "goal": [4.0, -3.0, 1.0], "radius": 0.20, "speed": 1.0 }, 35 | {"name": "crazyflie", "start": [-4.0, 2.5, 1.0], "goal": [4.0, -2.5, 1.0], "radius": 0.20, "speed": 1.0 }, 36 | {"name": "crazyflie", "start": [-4.0, 2.0, 1.0], "goal": [4.0, -2.0, 1.0], "radius": 0.20, "speed": 1.0 }, 37 | {"name": "crazyflie", "start": [-4.0, 1.5, 1.0], "goal": [4.0, -1.5, 1.0], "radius": 0.20, "speed": 1.0 }, 38 | {"name": "crazyflie", "start": [-4.0, 1.0, 1.0], "goal": [4.0, -1.0, 1.0], "radius": 0.20, "speed": 1.0 }, 39 | {"name": "crazyflie", "start": [-4.0, 0.5, 1.0], "goal": [4.0, -0.5, 1.0], "radius": 0.20, "speed": 1.0 }, 40 | {"name": "crazyflie", "start": [-4.0, 0.0, 1.0], "goal": [4.0, 0.0, 1.0], "radius": 0.20, "speed": 1.0 }, 41 | {"name": "crazyflie", "start": [-4.0, -0.5, 1.0], "goal": [4.0, 0.5, 1.0], "radius": 0.20, "speed": 1.0 }, 42 | {"name": "crazyflie", "start": [-4.0, -1.0, 1.0], "goal": [4.0, 1.0, 1.0], "radius": 0.20, "speed": 1.0 }, 43 | {"name": "crazyflie", "start": [-4.0, -1.5, 1.0], "goal": [4.0, 1.5, 1.0], "radius": 0.20, "speed": 1.0 }, 44 | {"name": "crazyflie", "start": [-4.0, -2.0, 1.0], "goal": [4.0, 2.0, 1.0], "radius": 0.20, "speed": 1.0 }, 45 | {"name": "crazyflie", "start": [-4.0, -2.5, 1.0], "goal": [4.0, 2.5, 1.0], "radius": 0.20, "speed": 1.0 }, 46 | {"name": "crazyflie", "start": [-4.0, -3.0, 1.0], "goal": [4.0, 3.0, 1.0], "radius": 0.20, "speed": 1.0 }, 47 | {"name": "crazyflie", "start": [-4.0, -3.5, 1.0], "goal": [4.0, 3.5, 1.0], "radius": 0.20, "speed": 1.0 }, 48 | {"name": "crazyflie", "start": [-4.0, -4.0, 1.0], "goal": [4.0, 4.0, 1.0], "radius": 0.20, "speed": 1.0 }, 49 | {"name": "crazyflie", "start": [-3.5, -4.0, 1.0], "goal": [3.5, 4.0, 1.0], "radius": 0.20, "speed": 1.0 }, 50 | {"name": "crazyflie", "start": [-3.0, -4.0, 1.0], "goal": [3.0, 4.0, 1.0], "radius": 0.20, "speed": 1.0 }, 51 | {"name": "crazyflie", "start": [-2.5, -4.0, 1.0], "goal": [2.5, 4.0, 1.0], "radius": 0.20, "speed": 1.0 }, 52 | {"name": "crazyflie", "start": [-2.0, -4.0, 1.0], "goal": [2.0, 4.0, 1.0], "radius": 0.20, "speed": 1.0 }, 53 | {"name": "crazyflie", "start": [-1.5, -4.0, 1.0], "goal": [1.5, 4.0, 1.0], "radius": 0.20, "speed": 1.0 }, 54 | {"name": "crazyflie", "start": [-1.0, -4.0, 1.0], "goal": [1.0, 4.0, 1.0], "radius": 0.20, "speed": 1.0 }, 55 | {"name": "crazyflie", "start": [-0.5, -4.0, 1.0], "goal": [0.5, 4.0, 1.0], "radius": 0.20, "speed": 1.0 }, 56 | {"name": "crazyflie", "start": [0.0, -4.0, 1.0], "goal": [0.0, 4.0, 1.0], "radius": 0.20, "speed": 1.0 }, 57 | {"name": "crazyflie", "start": [0.5, -4.0, 1.0], "goal": [-0.5, 4.0, 1.0], "radius": 0.20, "speed": 1.0 }, 58 | {"name": "crazyflie", "start": [1.0, -4.0, 1.0], "goal": [-1.0, 4.0, 1.0], "radius": 0.20, "speed": 1.0 }, 59 | {"name": "crazyflie", "start": [1.5, -4.0, 1.0], "goal": [-1.5, 4.0, 1.0], "radius": 0.20, "speed": 1.0 }, 60 | {"name": "crazyflie", "start": [2.0, -4.0, 1.0], "goal": [-2.0, 4.0, 1.0], "radius": 0.20, "speed": 1.0 }, 61 | {"name": "crazyflie", "start": [2.5, -4.0, 1.0], "goal": [-2.5, 4.0, 1.0], "radius": 0.20, "speed": 1.0 }, 62 | {"name": "crazyflie", "start": [3.0, -4.0, 1.0], "goal": [-3.0, 4.0, 1.0], "radius": 0.20, "speed": 1.0 }, 63 | {"name": "crazyflie", "start": [3.5, -4.0, 1.0], "goal": [-3.5, 4.0, 1.0], "radius": 0.20, "speed": 1.0 }, 64 | {"name": "crazyflie", "start": [4.0, -4.0, 1.0], "goal": [-4.0, 4.0, 1.0], "radius": 0.20, "speed": 1.0 }, 65 | {"name": "crazyflie", "start": [4.0, -3.5, 1.0], "goal": [-4.0, 3.5, 1.0], "radius": 0.20, "speed": 1.0 }, 66 | {"name": "crazyflie", "start": [4.0, -3.0, 1.0], "goal": [-4.0, 3.0, 1.0], "radius": 0.20, "speed": 1.0 }, 67 | {"name": "crazyflie", "start": [4.0, -2.5, 1.0], "goal": [-4.0, 2.5, 1.0], "radius": 0.20, "speed": 1.0 }, 68 | {"name": "crazyflie", "start": [4.0, -2.0, 1.0], "goal": [-4.0, 2.0, 1.0], "radius": 0.20, "speed": 1.0 }, 69 | {"name": "crazyflie", "start": [4.0, -1.5, 1.0], "goal": [-4.0, 1.5, 1.0], "radius": 0.20, "speed": 1.0 }, 70 | {"name": "crazyflie", "start": [4.0, -1.0, 1.0], "goal": [-4.0, 1.0, 1.0], "radius": 0.20, "speed": 1.0 }, 71 | {"name": "crazyflie", "start": [4.0, -0.5, 1.0], "goal": [-4.0, 0.5, 1.0], "radius": 0.20, "speed": 1.0 } 72 | ] 73 | } 74 | 75 | -------------------------------------------------------------------------------- /swarm_planner/missions/mission_8agents_10.json: -------------------------------------------------------------------------------- 1 | { 2 | "quadrotors": { 3 | "crazyflie": {"max_vel": [1.7, 1.7, 1.7], "max_acc": [6.2, 6.2, 6.2], "radius": 0.15, "speed": 1.0 }, 4 | "default": {"max_vel": [1.7, 1.7, 1.7], "max_acc": [6.2, 6.2, 6.2], "radius": 0.15, "speed": 1.0 } 5 | }, 6 | 7 | "agents": [ 8 | {"name": "crazyflie", "start": [4.0, 0.0, 1.0], "goal": [-4.0, 0.0, 1.0], "radius": 0.1, "speed": 1.0 }, 9 | {"name": "crazyflie", "start": [4.0, 4.0, 1.0], "goal": [-4.0, -4.0, 1.0], "radius": 0.1, "speed": 1.0 }, 10 | {"name": "crazyflie", "start": [0.0, 4.0, 1.0], "goal": [0.0, -4.0, 1.0], "radius": 0.1, "speed": 1.0 }, 11 | {"name": "crazyflie", "start": [-4.0, 4.0, 1.0], "goal": [4.0, -4.0, 1.0], "radius": 0.1, "speed": 1.0 }, 12 | {"name": "crazyflie", "start": [-4.0, 0.0, 1.0], "goal": [4.0, 0.0, 1.0], "radius": 0.1, "speed": 1.0 }, 13 | {"name": "crazyflie", "start": [-4.0, -4.0, 1.0], "goal": [4.0, 4.0, 1.0], "radius": 0.1, "speed": 1.0 }, 14 | {"name": "crazyflie", "start": [0.0, -4.0, 1.0], "goal": [0.0, 4.0, 1.0], "radius": 0.1, "speed": 1.0 }, 15 | {"name": "crazyflie", "start": [4.0, -4.0, 1.0], "goal": [-4.0, 4.0, 1.0], "radius": 0.1, "speed": 1.0 } 16 | ] 17 | } 18 | 19 | -------------------------------------------------------------------------------- /swarm_planner/missions/mission_8agents_12.json: -------------------------------------------------------------------------------- 1 | { 2 | "quadrotors": { 3 | "crazyflie": {"max_vel": [1.7, 1.7, 1.7], "max_acc": [6.2, 6.2, 6.2], "radius": 0.15, "speed": 1.0 }, 4 | "default": {"max_vel": [1.7, 1.7, 1.7], "max_acc": [6.2, 6.2, 6.2], "radius": 0.15, "speed": 1.0 } 5 | }, 6 | 7 | "agents": [ 8 | {"name": "crazyflie", "start": [4.0, 0.0, 1.0], "goal": [-4.0, 0.0, 1.0], "radius": 0.12, "speed": 1.0 }, 9 | {"name": "crazyflie", "start": [4.0, 4.0, 1.0], "goal": [-4.0, -4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 10 | {"name": "crazyflie", "start": [0.0, 4.0, 1.0], "goal": [0.0, -4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 11 | {"name": "crazyflie", "start": [-4.0, 4.0, 1.0], "goal": [4.0, -4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 12 | {"name": "crazyflie", "start": [-4.0, 0.0, 1.0], "goal": [4.0, 0.0, 1.0], "radius": 0.12, "speed": 1.0 }, 13 | {"name": "crazyflie", "start": [-4.0, -4.0, 1.0], "goal": [4.0, 4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 14 | {"name": "crazyflie", "start": [0.0, -4.0, 1.0], "goal": [0.0, 4.0, 1.0], "radius": 0.12, "speed": 1.0 }, 15 | {"name": "crazyflie", "start": [4.0, -4.0, 1.0], "goal": [-4.0, 4.0, 1.0], "radius": 0.12, "speed": 1.0 } 16 | ] 17 | } 18 | 19 | -------------------------------------------------------------------------------- /swarm_planner/missions/mission_8agents_120.json: -------------------------------------------------------------------------------- 1 | { 2 | "quadrotors": { 3 | "crazyflie": {"max_vel": [1.7, 1.7, 1.7], "max_acc": [6.2, 6.2, 6.2], "radius": 0.15, "speed": 1.0 }, 4 | "default": {"max_vel": [1.7, 1.7, 1.7], "max_acc": [6.2, 6.2, 6.2], "radius": 0.15, "speed": 1.0 } 5 | }, 6 | 7 | "agents": [ 8 | {"name": "crazyflie", "start": [4.0, 0.0, 1.0], "goal": [-4.0, 0.0, 1.0], "radius": 1.20, "speed": 1.0 }, 9 | {"name": "crazyflie", "start": [4.0, 4.0, 1.0], "goal": [-4.0, -4.0, 1.0], "radius": 1.20, "speed": 1.0 }, 10 | {"name": "crazyflie", "start": [0.0, 4.0, 1.0], "goal": [0.0, -4.0, 1.0], "radius": 1.20, "speed": 1.0 }, 11 | {"name": "crazyflie", "start": [-4.0, 4.0, 1.0], "goal": [4.0, -4.0, 1.0], "radius": 1.20, "speed": 1.0 }, 12 | {"name": "crazyflie", "start": [-4.0, 0.0, 1.0], "goal": [4.0, 0.0, 1.0], "radius": 1.20, "speed": 1.0 }, 13 | {"name": "crazyflie", "start": [-4.0, -4.0, 1.0], "goal": [4.0, 4.0, 1.0], "radius": 1.20, "speed": 1.0 }, 14 | {"name": "crazyflie", "start": [0.0, -4.0, 1.0], "goal": [0.0, 4.0, 1.0], "radius": 1.20, "speed": 1.0 }, 15 | {"name": "crazyflie", "start": [4.0, -4.0, 1.0], "goal": [-4.0, 4.0, 1.0], "radius": 1.20, "speed": 1.0 } 16 | ] 17 | } 18 | 19 | -------------------------------------------------------------------------------- /swarm_planner/missions/mission_8agents_15.json: -------------------------------------------------------------------------------- 1 | { 2 | "quadrotors": { 3 | "crazyflie": {"max_vel": [1.7, 1.7, 1.7], "max_acc": [6.2, 6.2, 6.2], "radius": 0.15, "speed": 1.0 }, 4 | "default": {"max_vel": [1.7, 1.7, 1.7], "max_acc": [6.2, 6.2, 6.2], "radius": 0.15, "speed": 1.0 } 5 | }, 6 | 7 | "agents": [ 8 | {"name": "crazyflie", "start": [4.0, 0.0, 1.0], "goal": [-4.0, 0.0, 1.0], "radius": 0.15, "speed": 1.0 }, 9 | {"name": "crazyflie", "start": [4.0, 4.0, 1.0], "goal": [-4.0, -4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 10 | {"name": "crazyflie", "start": [0.0, 4.0, 1.0], "goal": [0.0, -4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 11 | {"name": "crazyflie", "start": [-4.0, 4.0, 1.0], "goal": [4.0, -4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 12 | {"name": "crazyflie", "start": [-4.0, 0.0, 1.0], "goal": [4.0, 0.0, 1.0], "radius": 0.15, "speed": 1.0 }, 13 | {"name": "crazyflie", "start": [-4.0, -4.0, 1.0], "goal": [4.0, 4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 14 | {"name": "crazyflie", "start": [0.0, -4.0, 1.0], "goal": [0.0, 4.0, 1.0], "radius": 0.15, "speed": 1.0 }, 15 | {"name": "crazyflie", "start": [4.0, -4.0, 1.0], "goal": [-4.0, 4.0, 1.0], "radius": 0.15, "speed": 1.0 } 16 | ] 17 | } 18 | 19 | -------------------------------------------------------------------------------- /swarm_planner/missions/mission_8agents_30.json: -------------------------------------------------------------------------------- 1 | { 2 | "quadrotors": { 3 | "crazyflie": {"max_vel": [1.7, 1.7, 1.7], "max_acc": [6.2, 6.2, 6.2], "radius": 0.15, "speed": 1.0 }, 4 | "default": {"max_vel": [1.7, 1.7, 1.7], "max_acc": [6.2, 6.2, 6.2], "radius": 0.15, "speed": 1.0 } 5 | }, 6 | 7 | "agents": [ 8 | {"name": "crazyflie", "start": [4.0, 0.0, 1.0], "goal": [-4.0, 0.0, 1.0], "radius": 0.30, "speed": 1.0 }, 9 | {"name": "crazyflie", "start": [4.0, 4.0, 1.0], "goal": [-4.0, -4.0, 1.0], "radius": 0.30, "speed": 1.0 }, 10 | {"name": "crazyflie", "start": [0.0, 4.0, 1.0], "goal": [0.0, -4.0, 1.0], "radius": 0.30, "speed": 1.0 }, 11 | {"name": "crazyflie", "start": [-4.0, 4.0, 1.0], "goal": [4.0, -4.0, 1.0], "radius": 0.30, "speed": 1.0 }, 12 | {"name": "crazyflie", "start": [-4.0, 0.0, 1.0], "goal": [4.0, 0.0, 1.0], "radius": 0.30, "speed": 1.0 }, 13 | {"name": "crazyflie", "start": [-4.0, -4.0, 1.0], "goal": [4.0, 4.0, 1.0], "radius": 0.30, "speed": 1.0 }, 14 | {"name": "crazyflie", "start": [0.0, -4.0, 1.0], "goal": [0.0, 4.0, 1.0], "radius": 0.30, "speed": 1.0 }, 15 | {"name": "crazyflie", "start": [4.0, -4.0, 1.0], "goal": [-4.0, 4.0, 1.0], "radius": 0.30, "speed": 1.0 } 16 | ] 17 | } 18 | 19 | -------------------------------------------------------------------------------- /swarm_planner/missions/mission_8agents_60.json: -------------------------------------------------------------------------------- 1 | { 2 | "quadrotors": { 3 | "crazyflie": {"max_vel": [1.7, 1.7, 1.7], "max_acc": [6.2, 6.2, 6.2], "radius": 0.15, "speed": 1.0 }, 4 | "default": {"max_vel": [1.7, 1.7, 1.7], "max_acc": [6.2, 6.2, 6.2], "radius": 0.15, "speed": 1.0 } 5 | }, 6 | 7 | "agents": [ 8 | {"name": "crazyflie", "start": [4.0, 0.0, 1.0], "goal": [-4.0, 0.0, 1.0], "radius": 0.60, "speed": 1.0 }, 9 | {"name": "crazyflie", "start": [4.0, 4.0, 1.0], "goal": [-4.0, -4.0, 1.0], "radius": 0.60, "speed": 1.0 }, 10 | {"name": "crazyflie", "start": [0.0, 4.0, 1.0], "goal": [0.0, -4.0, 1.0], "radius": 0.60, "speed": 1.0 }, 11 | {"name": "crazyflie", "start": [-4.0, 4.0, 1.0], "goal": [4.0, -4.0, 1.0], "radius": 0.60, "speed": 1.0 }, 12 | {"name": "crazyflie", "start": [-4.0, 0.0, 1.0], "goal": [4.0, 0.0, 1.0], "radius": 0.60, "speed": 1.0 }, 13 | {"name": "crazyflie", "start": [-4.0, -4.0, 1.0], "goal": [4.0, 4.0, 1.0], "radius": 0.60, "speed": 1.0 }, 14 | {"name": "crazyflie", "start": [0.0, -4.0, 1.0], "goal": [0.0, 4.0, 1.0], "radius": 0.60, "speed": 1.0 }, 15 | {"name": "crazyflie", "start": [4.0, -4.0, 1.0], "goal": [-4.0, 4.0, 1.0], "radius": 0.60, "speed": 1.0 } 16 | ] 17 | } 18 | 19 | -------------------------------------------------------------------------------- /swarm_planner/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | swarm_planner 4 | 0.0.1 5 | swarm trajectory generator 6 | Jungwon Park 7 | BSD 8 | 9 | catkin 10 | 11 | roscpp 12 | roslib 13 | sensor_msgs 14 | std_msgs 15 | ecbs 16 | octomap 17 | 18 | roscpp 19 | roslib 20 | sensor_msgs 21 | std_msgs 22 | message_runtime 23 | ecbs 24 | octomap 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /swarm_planner/setting: -------------------------------------------------------------------------------- 1 | sudo apt-get install ros-melodic-octomap-* 2 | 3 | //sudo apt-get install ros-melodic-rviz-visual-tools 4 | 5 | sudo apt-get install python-matplotlib python-numpy python2.7-dev 6 | 7 | 8 | 9 | $ sudo apt-get install ros-melodic-rqt 10 | $ sudo apt-get install ros-melodic-rqt-common-plugins -------------------------------------------------------------------------------- /swarm_planner/src/random_map_generator.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | //ROS 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | //Octomap 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | #include 31 | 32 | 33 | using namespace std; 34 | 35 | pcl::search::KdTree kdtreeLocalMap; 36 | 37 | random_device rd; 38 | default_random_engine eng(rd()); 39 | uniform_real_distribution rand_x; 40 | uniform_real_distribution rand_y; 41 | uniform_real_distribution rand_w; 42 | uniform_real_distribution rand_h; 43 | 44 | ros::Publisher all_map_pub; 45 | 46 | vector _state; 47 | 48 | int obs_num; 49 | double margin; 50 | double x_min, y_min, z_min, x_max, y_max, z_max; 51 | double r_min, r_max, h_min, h_max, resolution; 52 | 53 | sensor_msgs::PointCloud2 globalMap_pcd; 54 | pcl::PointCloud cloudMap; 55 | 56 | void RandomMapGenerate(const SwarmPlanning::Mission& mission) 57 | { 58 | double numel_e = 0.00001; 59 | pcl::PointXYZ pt_random; 60 | 61 | rand_x = uniform_real_distribution(x_min, x_max); 62 | rand_y = uniform_real_distribution(y_min, y_max); 63 | rand_w = uniform_real_distribution(r_min, r_max); 64 | rand_h = uniform_real_distribution(h_min, h_max); 65 | 66 | int obs_iter = 0; 67 | while(obs_iter < obs_num) 68 | { 69 | double x, y, w, h; 70 | x = rand_x(eng); 71 | y = rand_y(eng); 72 | w = rand_w(eng); 73 | 74 | bool quadInObs = false; 75 | for (int qi = 0; qi < mission.qn; qi++) { 76 | if (sqrt(pow(x - mission.startState[qi][0], 2) + pow(y - mission.startState[qi][1], 2)) < mission.quad_size[qi] + w + margin || 77 | sqrt(pow(x - mission.goalState[qi][0], 2) + pow(y - mission.goalState[qi][1], 2)) < mission.quad_size[qi] + w + margin){ 78 | quadInObs = true; 79 | break; 80 | } 81 | } 82 | if(quadInObs){ 83 | continue; 84 | } 85 | x = floor(x/resolution) * resolution + resolution / 2.0; 86 | y = floor(y/resolution) * resolution + resolution / 2.0; 87 | 88 | int widNum = ceil(w/resolution); 89 | 90 | for(int r = -widNum/2.0; r < widNum/2.0; r ++ ) { 91 | for (int s = -widNum / 2.0; s < widNum / 2.0; s++) { 92 | h = rand_h(eng); 93 | int heiNum = ceil(h / resolution); 94 | for (int t = 0; t < heiNum; t++) { 95 | pt_random.x = x + (r + 0.5) * resolution + numel_e; 96 | pt_random.y = y + (s + 0.5) * resolution + numel_e; 97 | pt_random.z = (t + 0.5) * resolution + numel_e; 98 | cloudMap.points.push_back(pt_random); 99 | } 100 | } 101 | } 102 | 103 | obs_iter++; 104 | } 105 | 106 | cloudMap.width = cloudMap.points.size(); 107 | cloudMap.height = 1; 108 | cloudMap.is_dense = true; 109 | 110 | ROS_WARN("Finished generate random map "); 111 | 112 | kdtreeLocalMap.setInputCloud( cloudMap.makeShared() ); 113 | } 114 | 115 | void pubSensedPoints() 116 | { 117 | pcl::toROSMsg(cloudMap, globalMap_pcd); 118 | globalMap_pcd.header.frame_id = "world"; 119 | all_map_pub.publish(globalMap_pcd); 120 | } 121 | 122 | 123 | int main (int argc, char** argv) { 124 | ros::init (argc, argv, "random_map_generator"); 125 | ros::NodeHandle n( "~" ); 126 | ros::V_string args; 127 | ros::removeROSArgs(argc, argv, args); 128 | 129 | all_map_pub = n.advertise("all_map", 1); 130 | 131 | n.param("world/x_min", x_min, -5); 132 | n.param("world/y_min", y_min, -5); 133 | n.param("world/z_min", z_min, 0); 134 | n.param("world/x_max", x_max, 5); 135 | n.param("world/y_max", y_max, 5); 136 | n.param("world/z_max", z_max, 2.5); 137 | n.param("world/margin", margin, 1.5); 138 | 139 | n.param("world/obs_num", obs_num, 6); 140 | n.param("world/resolution", resolution, 0.1); 141 | n.param("world/obs_r_min", r_min, 0.3); 142 | n.param("world/obs_r_max", r_max, 0.8); 143 | n.param("world/obs_h_min", h_min, 1.0); 144 | n.param("world/obs_h_max", h_max, 2.5); 145 | 146 | SwarmPlanning::Mission mission; 147 | if(!mission.setMission(n)){ 148 | return -1; 149 | } 150 | 151 | // generate map msg 152 | RandomMapGenerate(mission); 153 | 154 | ros::Rate rate(10); 155 | int count = 0; 156 | while (ros::ok()) 157 | { 158 | if(count < 100) { 159 | pubSensedPoints(); 160 | count++; 161 | } 162 | ros::spinOnce(); 163 | rate.sleep(); 164 | } 165 | } 166 | -------------------------------------------------------------------------------- /swarm_planner/src/swarm_traj_planner_rbp.cpp: -------------------------------------------------------------------------------- 1 | // ROS 2 | #include 3 | 4 | // Octomap 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | // Parameters 11 | #include 12 | #include 13 | #include 14 | 15 | // Submodules 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | using namespace SwarmPlanning; 22 | 23 | bool has_octomap = false; 24 | bool has_path = false; 25 | std::shared_ptr octree_obj; 26 | 27 | void octomapCallback(const octomap_msgs::Octomap& octomap_msg) 28 | { 29 | if(has_octomap) 30 | return; 31 | 32 | octree_obj.reset(dynamic_cast(octomap_msgs::fullMsgToMap(octomap_msg))); 33 | 34 | has_octomap = true; 35 | } 36 | 37 | int main(int argc, char* argv[]) { 38 | ROS_INFO("Swarm Trajectory Planner"); 39 | ros::init (argc, argv, "swarm_traj_planner_rbp"); 40 | ros::NodeHandle nh( "~" ); 41 | ros::Subscriber octomap_sub = nh.subscribe( "/octomap_full", 1, octomapCallback ); 42 | 43 | // Mission 44 | Mission mission; 45 | if(!mission.setMission(nh)){ 46 | return -1; 47 | } 48 | 49 | // ROS Parameters 50 | Param param; 51 | if(!param.setROSParam(nh)){ 52 | return -1; 53 | } 54 | param.setColor(mission.qn); 55 | 56 | // Submodules 57 | SwarmPlanning::PlanResult planResult; 58 | std::shared_ptr distmap_obj; 59 | std::shared_ptr initTrajPlanner_obj; 60 | std::shared_ptr corridor_obj; 61 | std::shared_ptr RBPPlanner_obj; 62 | std::shared_ptr RBPPublisher_obj; 63 | 64 | // Main Loop 65 | ros::Rate rate(20); 66 | Timer timer_total; 67 | Timer timer_step; 68 | double start_time, current_time; 69 | while (ros::ok()) { 70 | if (has_octomap && !has_path) { 71 | timer_total.reset(); 72 | 73 | // Build 3D Euclidean Distance Field 74 | timer_step.reset(); 75 | { 76 | float maxDist = 1; 77 | octomap::point3d min_point3d(param.world_x_min, param.world_y_min, param.world_z_min); 78 | octomap::point3d max_point3d(param.world_x_max, param.world_y_max, param.world_z_max); 79 | distmap_obj.reset(new DynamicEDTOctomap(maxDist, octree_obj.get(), min_point3d, max_point3d, false)); 80 | distmap_obj.get()->update(); 81 | } 82 | timer_step.stop(); 83 | ROS_INFO_STREAM("distmap runtime: " << timer_step.elapsedSeconds()); 84 | 85 | // Step 1: Plan Initial Trajectory 86 | timer_step.reset(); 87 | { 88 | initTrajPlanner_obj.reset(new ECBSPlanner(distmap_obj, mission, param)); 89 | if (!initTrajPlanner_obj.get()->update(param.log, &planResult)) { 90 | return -1; 91 | } 92 | } 93 | timer_step.stop(); 94 | ROS_INFO_STREAM("Initial Trajectory Planner runtime: " << timer_step.elapsedSeconds()); 95 | 96 | // Step 2: Generate SFC, RSFC 97 | timer_step.reset(); 98 | { 99 | corridor_obj.reset(new Corridor(distmap_obj, mission, param)); 100 | if (!corridor_obj.get()->update(param.log, &planResult)) { 101 | return -1; 102 | } 103 | } 104 | timer_step.stop(); 105 | ROS_INFO_STREAM("BoxGenerator runtime: " << timer_step.elapsedSeconds()); 106 | 107 | // Step 3: Formulate QP problem and solving it to generate trajectory for quadrotor swarm 108 | timer_step.reset(); 109 | { 110 | RBPPlanner_obj.reset(new RBPPlanner(mission, param)); 111 | if (!RBPPlanner_obj.get()->update(param.log, &planResult)) { 112 | return -1; 113 | } 114 | } 115 | timer_step.stop(); 116 | ROS_INFO_STREAM("SwarmPlanner runtime: " << timer_step.elapsedSeconds()); 117 | 118 | timer_total.stop(); 119 | ROS_INFO_STREAM("Overall runtime: " << timer_total.elapsedSeconds()); 120 | 121 | // Plot Planning Result 122 | RBPPublisher_obj.reset(new RBPPublisher(nh, planResult, mission, param)); 123 | RBPPublisher_obj->plot(param.log); 124 | 125 | start_time = ros::Time::now().toSec(); 126 | has_path = true; 127 | } 128 | if(has_path) { 129 | // Publish Swarm Trajectory 130 | current_time = ros::Time::now().toSec() - start_time; 131 | RBPPublisher_obj.get()->update(current_time); 132 | RBPPublisher_obj.get()->publish(); 133 | } 134 | ros::spinOnce(); 135 | rate.sleep(); 136 | } 137 | 138 | return 0; 139 | } -------------------------------------------------------------------------------- /swarm_planner/src/swarm_traj_planner_rbp_flat.cpp: -------------------------------------------------------------------------------- 1 | // ROS 2 | #include 3 | 4 | // Octomap 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | // Parameters 11 | #include 12 | #include 13 | #include 14 | 15 | // Submodules 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | using namespace SwarmPlanning; 22 | 23 | bool has_octomap = false; 24 | bool has_path = false; 25 | std::shared_ptr octree_obj; 26 | 27 | void octomapCallback(const octomap_msgs::Octomap& octomap_msg) 28 | { 29 | if(has_octomap) 30 | return; 31 | 32 | octree_obj.reset(dynamic_cast(octomap_msgs::fullMsgToMap(octomap_msg))); 33 | 34 | has_octomap = true; 35 | } 36 | 37 | int main(int argc, char* argv[]) { 38 | ROS_INFO("Swarm Trajectory Planner"); 39 | ros::init (argc, argv, "swarm_traj_planner_rbp_flat"); 40 | ros::NodeHandle nh( "~" ); 41 | ros::Subscriber octomap_sub = nh.subscribe( "/octomap_full", 1, octomapCallback ); 42 | 43 | // Mission 44 | Mission mission; 45 | if(!mission.setMission(nh)){ 46 | return -1; 47 | } 48 | 49 | // ROS Parameters 50 | Param param; 51 | if(!param.setROSParam(nh)){ 52 | return -1; 53 | } 54 | param.setColor(mission.qn); 55 | 56 | // Submodules 57 | SwarmPlanning::PlanResult planResult; 58 | std::shared_ptr distmap_obj; 59 | std::shared_ptr initTrajPlanner_obj; 60 | std::shared_ptr corridor_obj; 61 | std::shared_ptr RBPPlanner_obj; 62 | std::shared_ptr RBPPublisher_obj; 63 | 64 | // Main Loop 65 | ros::Rate rate(20); 66 | Timer timer_total; 67 | Timer timer_step; 68 | double start_time, current_time; 69 | while (ros::ok()) { 70 | if (has_octomap && !has_path) { 71 | timer_total.reset(); 72 | 73 | // Build 3D Euclidean Distance Field 74 | timer_step.reset(); 75 | { 76 | float maxDist = 1; 77 | octomap::point3d min_point3d(param.world_x_min, param.world_y_min, param.world_z_min); 78 | octomap::point3d max_point3d(param.world_x_max, param.world_y_max, param.world_z_max); 79 | distmap_obj.reset(new DynamicEDTOctomap(maxDist, octree_obj.get(), min_point3d, max_point3d, false)); 80 | distmap_obj.get()->update(); 81 | } 82 | timer_step.stop(); 83 | ROS_INFO_STREAM("distmap runtime: " << timer_step.elapsedSeconds()); 84 | 85 | // Step 1: Plan Initial Trajectory 86 | timer_step.reset(); 87 | { 88 | initTrajPlanner_obj.reset(new ECBSPlanner(distmap_obj, mission, param)); 89 | if (!initTrajPlanner_obj.get()->update(param.log, &planResult)) { 90 | return -1; 91 | } 92 | } 93 | timer_step.stop(); 94 | ROS_INFO_STREAM("Initial Trajectory Planner runtime: " << timer_step.elapsedSeconds()); 95 | 96 | // Step 2: Generate SFC, RSFC 97 | timer_step.reset(); 98 | { 99 | corridor_obj.reset(new Corridor(distmap_obj, mission, param)); 100 | if (!corridor_obj.get()->update_flat_box(param.log, &planResult)) { 101 | return -1; 102 | } 103 | } 104 | timer_step.stop(); 105 | ROS_INFO_STREAM("BoxGenerator runtime: " << timer_step.elapsedSeconds()); 106 | 107 | // Step 3: Formulate QP problem and solving it to generate trajectory for quadrotor swarm 108 | timer_step.reset(); 109 | { 110 | RBPPlanner_obj.reset(new RBPPlanner(mission, param)); 111 | if (!RBPPlanner_obj.get()->update(param.log, &planResult)) { 112 | return -1; 113 | } 114 | } 115 | timer_step.stop(); 116 | ROS_INFO_STREAM("SwarmPlanner runtime: " << timer_step.elapsedSeconds()); 117 | 118 | timer_total.stop(); 119 | ROS_INFO_STREAM("Overall runtime: " << timer_total.elapsedSeconds()); 120 | 121 | // Plot Planning Result 122 | RBPPublisher_obj.reset(new RBPPublisher(nh, planResult, mission, param)); 123 | RBPPublisher_obj->plot(param.log); 124 | 125 | start_time = ros::Time::now().toSec(); 126 | has_path = true; 127 | } 128 | if(has_path) { 129 | // Publish Swarm Trajectory 130 | current_time = ros::Time::now().toSec() - start_time; 131 | RBPPublisher_obj.get()->update(current_time); 132 | RBPPublisher_obj.get()->publish(); 133 | } 134 | ros::spinOnce(); 135 | rate.sleep(); 136 | } 137 | 138 | return 0; 139 | } -------------------------------------------------------------------------------- /swarm_planner/src/swarm_traj_planner_rbp_test_all.cpp: -------------------------------------------------------------------------------- 1 | // Configuration 2 | #include 3 | #include 4 | #include 5 | 6 | // Octomap 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | // Submodule 13 | #include 14 | #include 15 | #include 16 | 17 | using namespace SwarmPlanning; 18 | 19 | int main(int argc, char* argv[]) { 20 | ROS_INFO("Swarm Trajectory Planner"); 21 | ros::init (argc, argv, "swarm_traj_planner_rbp_test_all"); 22 | ros::NodeHandle nh( "~" ); 23 | 24 | // Set Mission 25 | Mission mission; 26 | if(!mission.setMission(nh)){ 27 | return -1; 28 | } 29 | 30 | // Set ROS Parameters 31 | Param param; 32 | if(!param.setROSParam(nh)){ 33 | return -1; 34 | } 35 | param.setColor(mission.qn); 36 | 37 | std::shared_ptr octree_obj; 38 | 39 | // Submodules 40 | std::shared_ptr distmap_obj; 41 | std::shared_ptr initTrajPlanner_obj; 42 | std::shared_ptr corridor_obj; 43 | std::shared_ptr RBPPlanner_obj; 44 | 45 | // Main Loop 46 | Timer timer_total; 47 | Timer timer_step; 48 | 49 | for(int i = 1; i <= 50; i++) { 50 | ROS_INFO_STREAM("Map: map" << i << ".bt"); 51 | octree_obj.reset(new octomap::OcTree(param.package_path + "/worlds/map" + std::to_string(i) + ".bt")); 52 | SwarmPlanning::PlanResult planResult; 53 | timer_total.reset(); 54 | 55 | // Build 3D Euclidean Distance Field 56 | timer_step.reset(); 57 | 58 | float maxDist = 1; 59 | octomap::point3d min_point3d(param.world_x_min, param.world_y_min, param.world_z_min); 60 | octomap::point3d max_point3d(param.world_x_max, param.world_y_max, param.world_z_max); 61 | 62 | distmap_obj.reset(new DynamicEDTOctomap(maxDist, octree_obj.get(), min_point3d, max_point3d, false)); 63 | distmap_obj.get()->update(); 64 | 65 | timer_step.stop(); 66 | ROS_INFO_STREAM("Euclidean Distmap runtime: " << timer_step.elapsedSeconds()); 67 | 68 | // Step 1: Plan Initial Trajectory 69 | timer_step.reset(); 70 | { 71 | initTrajPlanner_obj.reset(new ECBSPlanner(distmap_obj, mission, param)); 72 | if (!initTrajPlanner_obj.get()->update(param.log, &planResult)) { 73 | return -1; 74 | } 75 | } 76 | timer_step.stop(); 77 | ROS_INFO_STREAM("Initial Trajectory Planner runtime: " << timer_step.elapsedSeconds()); 78 | 79 | // Step 2: Generate SFC, RSFC 80 | timer_step.reset(); 81 | { 82 | corridor_obj.reset(new Corridor(distmap_obj, mission, param)); 83 | if (!corridor_obj.get()->update(param.log, &planResult)) { 84 | return -1; 85 | } 86 | } 87 | timer_step.stop(); 88 | ROS_INFO_STREAM("BoxGenerator runtime: " << timer_step.elapsedSeconds()); 89 | 90 | // Step 3: Formulate QP problem and solving it to generate trajectory for quadrotor swarm 91 | timer_step.reset(); 92 | { 93 | RBPPlanner_obj.reset(new RBPPlanner(mission, param)); 94 | if (!RBPPlanner_obj.get()->update(param.log, &planResult)) { 95 | return -1; 96 | } 97 | } 98 | timer_step.stop(); 99 | ROS_INFO_STREAM("SwarmPlanner runtime: " << timer_step.elapsedSeconds()); 100 | 101 | timer_total.stop(); 102 | ROS_INFO_STREAM("Overall runtime: " << timer_total.elapsedSeconds()); 103 | } 104 | 105 | return 0; 106 | } -------------------------------------------------------------------------------- /swarm_planner/src/swarm_traj_planner_scp.cpp: -------------------------------------------------------------------------------- 1 | // ROS 2 | #include 3 | 4 | // Useful tools 5 | #include 6 | #include 7 | #include 8 | 9 | // Submodule 10 | #include 11 | #include 12 | #include 13 | 14 | using namespace SwarmPlanning; 15 | 16 | bool has_path = false; 17 | 18 | int main(int argc, char* argv[]) { 19 | ROS_INFO("Swarm Trajectory Planner - Sequential Convex Programming"); 20 | ros::init (argc, argv, "swarm_traj_planner_scp"); 21 | ros::NodeHandle nh( "~" ); 22 | ros::V_string args; 23 | ros::removeROSArgs(argc, argv, args); 24 | 25 | // Set Mission 26 | Mission mission; 27 | if(!mission.setMission(nh)){ 28 | return -1; 29 | } 30 | mission.applyNoise(0.01); 31 | 32 | // Set ROS Parameters 33 | Param param; 34 | if(!param.setROSParam(nh)){ 35 | return -1; 36 | } 37 | param.setColor(mission.qn); 38 | 39 | 40 | // Submodules 41 | std::shared_ptr SCPPlanner_obj; 42 | std::shared_ptr SCPPublisher_obj; 43 | std::shared_ptr SCPPlotter_obj; 44 | 45 | // Main Loop 46 | ros::Rate rate(20); 47 | Timer timer_total; 48 | Timer timer_step; 49 | double start_time, current_time; 50 | while (ros::ok()) { 51 | if (!has_path) { 52 | timer_total.reset(); 53 | 54 | // Plan Swarm Trajectory 55 | timer_step.reset(); 56 | 57 | SCPPlanner_obj.reset(new SCPPlanner(mission, param)); 58 | if(!SCPPlanner_obj.get()->update(param.log)){ 59 | return -1; 60 | } 61 | 62 | timer_total.stop(); 63 | ROS_INFO_STREAM("Overall runtime: " << timer_total.elapsedSeconds()); 64 | 65 | // Initialize Trajectory Publisher 66 | SCPPublisher_obj.reset(new SCPPublisher(nh, SCPPlanner_obj, mission, param)); 67 | 68 | // Plot Planning Result 69 | SCPPlotter_obj.reset(new SCPPlotter(SCPPlanner_obj, mission, param)); 70 | SCPPlotter_obj->plot(); 71 | 72 | start_time = ros::Time::now().toSec(); 73 | has_path = true; 74 | } 75 | if(has_path) { 76 | // Publish Swarm Trajectory 77 | current_time = ros::Time::now().toSec() - start_time; 78 | SCPPublisher_obj.get()->update(current_time); 79 | SCPPublisher_obj.get()->publish(); 80 | } 81 | ros::spinOnce(); 82 | rate.sleep(); 83 | } 84 | 85 | return 0; 86 | } -------------------------------------------------------------------------------- /swarm_planner/third_party/ecbs/.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | # BasedOnStyle: Google 4 | AccessModifierOffset: -1 5 | AlignAfterOpenBracket: Align 6 | AlignConsecutiveAssignments: false 7 | AlignConsecutiveDeclarations: false 8 | AlignEscapedNewlinesLeft: true 9 | AlignOperands: true 10 | AlignTrailingComments: true 11 | AllowAllParametersOfDeclarationOnNextLine: true 12 | AllowShortBlocksOnASingleLine: false 13 | AllowShortCaseLabelsOnASingleLine: false 14 | AllowShortFunctionsOnASingleLine: All 15 | AllowShortIfStatementsOnASingleLine: true 16 | AllowShortLoopsOnASingleLine: true 17 | AlwaysBreakAfterDefinitionReturnType: None 18 | AlwaysBreakAfterReturnType: None 19 | AlwaysBreakBeforeMultilineStrings: true 20 | AlwaysBreakTemplateDeclarations: true 21 | BinPackArguments: true 22 | BinPackParameters: true 23 | BraceWrapping: 24 | AfterClass: false 25 | AfterControlStatement: false 26 | AfterEnum: false 27 | AfterFunction: false 28 | AfterNamespace: false 29 | AfterObjCDeclaration: false 30 | AfterStruct: false 31 | AfterUnion: false 32 | BeforeCatch: false 33 | BeforeElse: false 34 | IndentBraces: false 35 | BreakBeforeBinaryOperators: None 36 | BreakBeforeBraces: Attach 37 | BreakBeforeTernaryOperators: true 38 | BreakConstructorInitializersBeforeComma: false 39 | ColumnLimit: 80 40 | CommentPragmas: '^ IWYU pragma:' 41 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 42 | ConstructorInitializerIndentWidth: 4 43 | ContinuationIndentWidth: 4 44 | Cpp11BracedListStyle: true 45 | DerivePointerAlignment: true 46 | DisableFormat: false 47 | ExperimentalAutoDetectBinPacking: false 48 | ForEachMacros: [ foreach, Q_FOREACH, BOOST_FOREACH ] 49 | IncludeCategories: 50 | - Regex: '^<.*\.h>' 51 | Priority: 1 52 | - Regex: '^<.*' 53 | Priority: 2 54 | - Regex: '.*' 55 | Priority: 3 56 | IndentCaseLabels: true 57 | IndentWidth: 2 58 | IndentWrappedFunctionNames: false 59 | KeepEmptyLinesAtTheStartOfBlocks: false 60 | MacroBlockBegin: '' 61 | MacroBlockEnd: '' 62 | MaxEmptyLinesToKeep: 1 63 | NamespaceIndentation: None 64 | ObjCBlockIndentWidth: 2 65 | ObjCSpaceAfterProperty: false 66 | ObjCSpaceBeforeProtocolList: false 67 | PenaltyBreakBeforeFirstCallParameter: 1 68 | PenaltyBreakComment: 300 69 | PenaltyBreakFirstLessLess: 120 70 | PenaltyBreakString: 1000 71 | PenaltyExcessCharacter: 1000000 72 | PenaltyReturnTypeOnItsOwnLine: 200 73 | PointerAlignment: Left 74 | ReflowComments: true 75 | SortIncludes: true 76 | SpaceAfterCStyleCast: false 77 | SpaceBeforeAssignmentOperators: true 78 | SpaceBeforeParens: ControlStatements 79 | SpaceInEmptyParentheses: false 80 | SpacesBeforeTrailingComments: 2 81 | SpacesInAngles: false 82 | SpacesInContainerLiterals: true 83 | SpacesInCStyleCastParentheses: false 84 | SpacesInParentheses: false 85 | SpacesInSquareBrackets: false 86 | Standard: Auto 87 | TabWidth: 8 88 | UseTab: Never 89 | ... 90 | 91 | -------------------------------------------------------------------------------- /swarm_planner/third_party/ecbs/.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | __pycache__/ 3 | -------------------------------------------------------------------------------- /swarm_planner/third_party/ecbs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | project(ecbs) 4 | 5 | find_package(catkin REQUIRED COMPONENTS rospy std_msgs sensor_msgs) 6 | find_package(Boost 1.58 REQUIRED COMPONENTS program_options) 7 | find_package(PkgConfig) 8 | pkg_check_modules(YamlCpp yaml-cpp) 9 | 10 | catkin_package( 11 | INCLUDE_DIRS include 12 | CATKIN_DEPENDS 13 | rospy 14 | std_msgs 15 | sensor_msgs) 16 | 17 | # check if Doxygen is installed 18 | find_package(Doxygen) 19 | 20 | if (DOXYGEN_FOUND) 21 | # set input and output files 22 | set(DOXYGEN_IN ${CMAKE_CURRENT_SOURCE_DIR}/doc/Doxyfile.in) 23 | set(DOXYGEN_OUT ${CMAKE_CURRENT_BINARY_DIR}/Doxyfile) 24 | 25 | # request to configure the file 26 | configure_file(${DOXYGEN_IN} ${DOXYGEN_OUT} @ONLY) 27 | message("Doxygen build started") 28 | 29 | # note the option ALL which allows to build the docs together with the application 30 | add_custom_target( docs 31 | COMMAND ${DOXYGEN_EXECUTABLE} ${DOXYGEN_OUT} 32 | WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR} 33 | COMMENT "Generating API documentation with Doxygen" 34 | VERBATIM ) 35 | else (DOXYGEN_FOUND) 36 | message("Doxygen need to be installed to generate the doxygen documentation") 37 | endif (DOXYGEN_FOUND) 38 | 39 | # clang-tidy target (linter & static code analysis) 40 | add_custom_target(clang-tidy 41 | COMMAND CMAKE_EXPORT_COMPILE_COMMANDS=ON run-clang-tidy-3.8.py ${CMAKE_CURRENT_SOURCE_DIR}) 42 | 43 | # clang-format 44 | set(ALL_SOURCE_FILES 45 | ${CMAKE_CURRENT_SOURCE_DIR}/include/a_star_epsilon.hpp 46 | ${CMAKE_CURRENT_SOURCE_DIR}/include/ecbs.hpp 47 | ${CMAKE_CURRENT_SOURCE_DIR}/include/neighbor.hpp 48 | ${CMAKE_CURRENT_SOURCE_DIR}/include/planresult.hpp 49 | ${CMAKE_CURRENT_SOURCE_DIR}/src/a_star_epsilon.cpp 50 | ${CMAKE_CURRENT_SOURCE_DIR}/src/ecbs.cpp 51 | ../../include/timer.hpp 52 | ) 53 | 54 | add_custom_target(clang-format 55 | COMMAND clang-format -i ${ALL_SOURCE_FILES} 56 | ) 57 | 58 | # Enable C++14 and warnings 59 | set(CMAKE_CXX_STANDARD 14) 60 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 61 | set(CMAKE_CXX_EXTENSIONS OFF) 62 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra") 63 | 64 | include_directories( 65 | include 66 | ) 67 | 68 | # Src 69 | 70 | ### a_star_epsilon 71 | #add_executable(a_star_epsilon 72 | # src/a_star_epsilon.cpp 73 | #) 74 | #target_link_libraries(a_star_epsilon 75 | # ${Boost_LIBRARIES} 76 | #) 77 | 78 | ### ecbs 79 | #add_executable(ecbs 80 | # src/ecbs.cpp 81 | #) 82 | #target_link_libraries(ecbs 83 | # ${Boost_LIBRARIES} 84 | # yaml-cpp 85 | #) 86 | ### a_star 87 | #add_executable(a_star 88 | # src/a_star.cpp 89 | #) 90 | #target_link_libraries(a_star 91 | # ${Boost_LIBRARIES} 92 | #) 93 | # 94 | ### cbs 95 | #add_executable(cbs 96 | # src/cbs.cpp 97 | #) 98 | #target_link_libraries(cbs 99 | # ${Boost_LIBRARIES} 100 | # yaml-cpp 101 | #) -------------------------------------------------------------------------------- /swarm_planner/third_party/ecbs/LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2017 whoenig 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /swarm_planner/third_party/ecbs/doc/libMultiRobotPlanning.md: -------------------------------------------------------------------------------- 1 | # libMultiRobotPlanning {#mainpage} 2 | 3 | This library contains implementations of search algorithms in C++(14). 4 | The library is header only and uses templates. 5 | 6 | ## Supported Algorithms 7 | 8 | ### Single-Robot 9 | 10 | Algorithm | Optimality | 11 | ------------|--------------------| 12 | [A*](@ref libMultiRobotPlanning::AStar) | optimal | 13 | [A*_epsilon](@ref libMultiRobotPlanning::AStarEpsilon) | w-bounded suboptimal | 14 | [SIPP](@ref libMultiRobotPlanning::SIPP) | optimal | 15 | 16 | ### Multi-Robot 17 | 18 | Algorithm | Optimality | 19 | ------------|--------------------| 20 | [Conflict-Based Search (CBS)](@ref libMultiRobotPlanning::CBS) | optimal (sum-of-cost) | 21 | [Enhanced Conflict-Based Search (ECBS)](@ref libMultiRobotPlanning::ECBS) | w-bounded suboptimal (sum-of-cost) | 22 | [Conflict-Based Search with Optimal Task Assignment (CBS-TA)](@ref libMultiRobotPlanning::CBSTA) | optimal (sum-of-cost) | 23 | [Enhanced Conflict-Based Search with Optimal Task Assignment (ECBS-TA)](@ref libMultiRobotPlanning::ECBSTA) | w-bounded suboptimal (sum-of-cost) | 24 | [Prioritized Planning with SIPP] | No | 25 | 26 | ### Assignment 27 | 28 | Algorithm | Optimality | 29 | ------------|--------------------| 30 | [Best Assignment (Network flow based)](@ref libMultiRobotPlanning::Assignment) | optimal (sum-of-cost) | 31 | [Next Best Assignment](@ref libMultiRobotPlanning::NextBestAssignment) | series of optimal solutions (sum-of-cost) | 32 | -------------------------------------------------------------------------------- /swarm_planner/third_party/ecbs/include/.a_star_epsilon.hpp.swp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/third_party/ecbs/include/.a_star_epsilon.hpp.swp -------------------------------------------------------------------------------- /swarm_planner/third_party/ecbs/include/.ecbs.hpp.swp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/third_party/ecbs/include/.ecbs.hpp.swp -------------------------------------------------------------------------------- /swarm_planner/third_party/ecbs/include/neighbor.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | namespace libMultiRobotPlanning { 4 | 5 | /*! \brief Represents state transations 6 | 7 | This class represents a transition from a start state applying an action 8 | with the given cost. 9 | 10 | \tparam State Custom state for the search. Needs to be copy'able 11 | \tparam Action Custom action for the search. Needs to be copy'able 12 | \tparam Cost Custom Cost type (integer or floating point types) 13 | */ 14 | template 15 | struct Neighbor { 16 | Neighbor(const State& state, const Action& action, Cost cost) 17 | : state(state), action(action), cost(cost) {} 18 | 19 | //! neighboring state 20 | State state; 21 | //! action to get to the neighboring state 22 | Action action; 23 | //! cost to get to the neighboring state 24 | Cost cost; 25 | }; 26 | 27 | } // namespace libMultiRobotPlanning 28 | -------------------------------------------------------------------------------- /swarm_planner/third_party/ecbs/include/planresult.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace libMultiRobotPlanning { 6 | 7 | /*! \brief Represents the path for an agent 8 | 9 | This class is used to store the result of a planner for a single agent. 10 | It has both the ordered list of states that need to be traversed as well as 11 | the ordered 12 | list of actions together with their respective costs 13 | 14 | \tparam State Custom state for the search. Needs to be copy'able 15 | \tparam Action Custom action for the search. Needs to be copy'able 16 | \tparam Cost Custom Cost type (integer or floating point types) 17 | */ 18 | template 19 | struct PlanResult { 20 | //! states and their gScore 21 | std::vector > states; 22 | //! actions and their cost 23 | std::vector > actions; 24 | //! actual cost of the result 25 | Cost cost; 26 | //! lower bound of the cost (for suboptimal solvers) 27 | Cost fmin; 28 | }; 29 | 30 | } // namespace libMultiRobotPlanning 31 | -------------------------------------------------------------------------------- /swarm_planner/third_party/ecbs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ecbs 4 | 0.0.1 5 | libMultiRobotPlanning ROS wrapper 6 | Jungwon Park 7 | BSD 8 | 9 | catkin 10 | 11 | roscpp 12 | rospy 13 | sensor_msgs 14 | std_msgs 15 | 16 | roscpp 17 | rospy 18 | sensor_msgs 19 | std_msgs 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /swarm_planner/third_party/ecbs/src/a_star.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | using libMultiRobotPlanning::AStar; 10 | using libMultiRobotPlanning::Neighbor; 11 | using libMultiRobotPlanning::PlanResult; 12 | 13 | struct State { 14 | State(int x, int y) : x(x), y(y) {} 15 | 16 | State(const State&) = default; 17 | State(State&&) = default; 18 | State& operator=(const State&) = default; 19 | State& operator=(State&&) = default; 20 | 21 | bool operator==(const State& other) const { 22 | return std::tie(x, y) == std::tie(other.x, other.y); 23 | } 24 | 25 | friend std::ostream& operator<<(std::ostream& os, const State& s) { 26 | return os << "(" << s.x << "," << s.y << ")"; 27 | } 28 | 29 | int x; 30 | int y; 31 | }; 32 | 33 | namespace std { 34 | template <> 35 | struct hash { 36 | size_t operator()(const State& s) const { 37 | size_t seed = 0; 38 | boost::hash_combine(seed, s.x); 39 | boost::hash_combine(seed, s.y); 40 | return seed; 41 | } 42 | }; 43 | } // namespace std 44 | 45 | enum class Action { 46 | Up, 47 | Down, 48 | Left, 49 | Right, 50 | }; 51 | 52 | std::ostream& operator<<(std::ostream& os, const Action& a) { 53 | switch (a) { 54 | case Action::Up: 55 | os << "Up"; 56 | break; 57 | case Action::Down: 58 | os << "Down"; 59 | break; 60 | case Action::Left: 61 | os << "Left"; 62 | break; 63 | case Action::Right: 64 | os << "Right"; 65 | break; 66 | } 67 | return os; 68 | } 69 | 70 | class Environment { 71 | public: 72 | Environment(size_t dimx, size_t dimy, std::unordered_set obstacles, 73 | State goal) 74 | : m_dimx(dimx), 75 | m_dimy(dimy), 76 | m_obstacles(std::move(obstacles)), 77 | m_goal(std::move(goal)) // NOLINT 78 | {} 79 | 80 | int admissibleHeuristic(const State& s) { 81 | return std::abs(s.x - m_goal.x) + std::abs(s.y - m_goal.y); 82 | } 83 | 84 | bool isSolution(const State& s) { return s == m_goal; } 85 | 86 | void getNeighbors(const State& s, 87 | std::vector >& neighbors) { 88 | neighbors.clear(); 89 | 90 | State up(s.x, s.y + 1); 91 | if (stateValid(up)) { 92 | neighbors.emplace_back(Neighbor(up, Action::Up, 1)); 93 | } 94 | State down(s.x, s.y - 1); 95 | if (stateValid(down)) { 96 | neighbors.emplace_back( 97 | Neighbor(down, Action::Down, 1)); 98 | } 99 | State left(s.x - 1, s.y); 100 | if (stateValid(left)) { 101 | neighbors.emplace_back( 102 | Neighbor(left, Action::Left, 1)); 103 | } 104 | State right(s.x + 1, s.y); 105 | if (stateValid(right)) { 106 | neighbors.emplace_back( 107 | Neighbor(right, Action::Right, 1)); 108 | } 109 | } 110 | 111 | void onExpandNode(const State& /*s*/, int /*fScore*/, int /*gScore*/) {} 112 | 113 | void onDiscover(const State& /*s*/, int /*fScore*/, int /*gScore*/) {} 114 | 115 | public: 116 | bool stateValid(const State& s) { 117 | return s.x >= 0 && s.x < m_dimx && s.y >= 0 && s.y < m_dimy && 118 | m_obstacles.find(s) == m_obstacles.end(); 119 | } 120 | 121 | private: 122 | int m_dimx; 123 | int m_dimy; 124 | std::unordered_set m_obstacles; 125 | State m_goal; 126 | }; 127 | 128 | int main(int argc, char* argv[]) { 129 | namespace po = boost::program_options; 130 | // Declare the supported options. 131 | po::options_description desc("Allowed options"); 132 | int startX, startY, goalX, goalY; 133 | std::string mapFile; 134 | std::string outputFile; 135 | desc.add_options()("help", "produce help message")( 136 | "startX", po::value(&startX)->required(), 137 | "start position x-component")("startY", 138 | po::value(&startY)->required(), 139 | "start position y-component")( 140 | "goalX", po::value(&goalX)->required(), "goal position x-component")( 141 | "goalY", po::value(&goalY)->required(), "goal position y-component")( 142 | "map,m", po::value(&mapFile)->required(), "input map (txt)")( 143 | "output,o", po::value(&outputFile)->required(), 144 | "output file (YAML)"); 145 | 146 | try { 147 | po::variables_map vm; 148 | po::store(po::parse_command_line(argc, argv, desc), vm); 149 | po::notify(vm); 150 | 151 | if (vm.count("help") != 0u) { 152 | std::cout << desc << "\n"; 153 | return 0; 154 | } 155 | } catch (po::error& e) { 156 | std::cerr << e.what() << std::endl << std::endl; 157 | std::cerr << desc << std::endl; 158 | return 1; 159 | } 160 | 161 | std::unordered_set obstacles; 162 | 163 | std::ifstream map(mapFile); 164 | int dimX = 0; 165 | int y = 0; 166 | while (map.good()) { 167 | std::string line; 168 | std::getline(map, line); 169 | int x = 0; 170 | for (char c : line) { 171 | if (c == '#') { 172 | obstacles.insert(State(x, y)); 173 | } 174 | ++x; 175 | } 176 | dimX = std::max(dimX, x); 177 | ++y; 178 | } 179 | std::cout << dimX << " " << y << std::endl; 180 | 181 | bool success = false; 182 | 183 | State goal(goalX, goalY); 184 | State start(startX, startY); 185 | Environment env(dimX, y - 1, obstacles, goal); 186 | 187 | AStar astar(env); 188 | 189 | PlanResult solution; 190 | 191 | if (env.stateValid(start)) { 192 | success = astar.search(start, solution); 193 | } 194 | 195 | std::ofstream out(outputFile); 196 | if (success) { 197 | std::cout << "Planning successful! Total cost: " << solution.cost 198 | << std::endl; 199 | for (size_t i = 0; i < solution.actions.size(); ++i) { 200 | std::cout << solution.states[i].second << ": " << solution.states[i].first 201 | << "->" << solution.actions[i].first 202 | << "(cost: " << solution.actions[i].second << ")" << std::endl; 203 | } 204 | std::cout << solution.states.back().second << ": " 205 | << solution.states.back().first << std::endl; 206 | 207 | out << "schedule:" << std::endl; 208 | out << " agent1:" << std::endl; 209 | for (size_t i = 0; i < solution.states.size(); ++i) { 210 | out << " - x: " << solution.states[i].first.x << std::endl 211 | << " y: " << solution.states[i].first.y << std::endl 212 | << " t: " << i << std::endl; 213 | } 214 | } else { 215 | std::cout << "Planning NOT successful!" << std::endl; 216 | } 217 | 218 | return 0; 219 | } 220 | -------------------------------------------------------------------------------- /swarm_planner/worlds/ICRA2020_64agents_presentation.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/ICRA2020_64agents_presentation.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/IROS2019.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/IROS2019.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/IROS2019_1.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/IROS2019_1.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/IROS2019_2.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/IROS2019_2.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/empty.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/empty.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/map1.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/map1.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/map10.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/map10.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/map11.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/map11.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/map12.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/map12.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/map13.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/map13.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/map14.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/map14.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/map15.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/map15.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/map16.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/map16.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/map17.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/map17.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/map18.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/map18.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/map19.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/map19.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/map2.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/map2.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/map20.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/map20.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/map21.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/map21.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/map22.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/map22.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/map23.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/map23.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/map24.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/map24.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/map25.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/map25.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/map26.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/map26.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/map27.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/map27.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/map28.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/map28.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/map29.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/map29.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/map3.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/map3.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/map30.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/map30.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/map31.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/map31.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/map32.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/map32.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/map33.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/map33.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/map34.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/map34.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/map35.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/map35.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/map36.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/map36.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/map37.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/map37.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/map38.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/map38.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/map39.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/map39.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/map4.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/map4.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/map40.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/map40.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/map41.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/map41.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/map42.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/map42.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/map43.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/map43.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/map44.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/map44.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/map45.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/map45.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/map46.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/map46.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/map47.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/map47.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/map48.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/map48.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/map49.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/map49.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/map5.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/map5.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/map50.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/map50.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/map6.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/map6.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/map7.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/map7.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/map8.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/map8.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/map9.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/map9.bt -------------------------------------------------------------------------------- /swarm_planner/worlds/map_reduced_tmp3.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/swarm_simulator/dc3f272158132cda4e1c319c7bd1a965d7bf9c40/swarm_planner/worlds/map_reduced_tmp3.bt --------------------------------------------------------------------------------