├── client ├── .gitkeep ├── embedding │ ├── CMakeLists.txt │ ├── mpga │ │ ├── CMakeLists.txt │ │ ├── best_100.dat │ │ └── main.cpp │ └── galib │ │ ├── CMakeLists.txt │ │ ├── best_499.dat │ │ ├── main.cpp │ │ └── evolution.dat ├── loop_functions │ ├── CMakeLists.txt │ └── mpga_loop_functions │ │ ├── mpga_loop_functions.cpp │ │ ├── CMakeLists.txt │ │ ├── mpga_loop_functions.h │ │ ├── mpga_phototaxis_loop_functions.h │ │ ├── mpga_phototaxis_loop_functions.cpp │ │ ├── mpga.h │ │ └── mpga.cpp ├── controllers │ ├── CMakeLists.txt │ ├── footbot_diffusion │ │ ├── CMakeLists.txt │ │ ├── footbot_diffusion.h │ │ └── footbot_diffusion.cpp │ └── footbot_nn │ │ ├── CMakeLists.txt │ │ ├── nn │ │ ├── perceptron.h │ │ ├── ctrnn_multilayer.h │ │ ├── neural_network.h │ │ ├── neural_network.cpp │ │ ├── perceptron.cpp │ │ └── ctrnn_multilayer.cpp │ │ ├── footbot_nn_controller.h │ │ └── footbot_nn_controller.cpp ├── CMakeLists.txt └── experiments │ └── diffusion_1.argos ├── server ├── .gitkeep ├── CMakeLists.txt ├── inc │ ├── parser.h │ ├── ADG_server.h │ └── ADG.h └── src │ ├── ADG.cpp │ ├── ADG_server.cpp │ └── parser.cpp ├── ArgosConfig ├── __init__.py └── ToArgos.py ├── 2d-map.png ├── empty-8-8.map ├── .gitmodules ├── .gitignore ├── empty-8-8.scen ├── LICENSE ├── create_map.py ├── random-32-32-20.map ├── README.md ├── CMakeLists.txt ├── run_sim.py ├── warehouse-10-20-10-2-1.map ├── example_paths_xy.txt ├── example_paths_yx.txt └── random-32-32-20-random-1.scen /client/.gitkeep: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /server/.gitkeep: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /ArgosConfig/__init__.py: -------------------------------------------------------------------------------- 1 | from .ToArgos import * 2 | -------------------------------------------------------------------------------- /2d-map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-mapf/smart/HEAD/2d-map.png -------------------------------------------------------------------------------- /client/embedding/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(mpga) 2 | if(GALIB_FOUND) 3 | add_subdirectory(galib) 4 | endif(GALIB_FOUND) 5 | -------------------------------------------------------------------------------- /empty-8-8.map: -------------------------------------------------------------------------------- 1 | type octile 2 | height 8 3 | width 8 4 | map 5 | ........ 6 | ........ 7 | ........ 8 | ........ 9 | ........ 10 | ........ 11 | ........ 12 | ........ -------------------------------------------------------------------------------- /client/loop_functions/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | include_directories(${CMAKE_SOURCE_DIR}/client) 2 | include_directories(${CMAKE_SOURCE_DIR}) 3 | 4 | add_subdirectory(mpga_loop_functions) 5 | 6 | -------------------------------------------------------------------------------- /client/embedding/mpga/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(mpga_phototaxis main.cpp) 2 | target_link_libraries(mpga_phototaxis 3 | footbot_nn 4 | mpga_phototaxis_loop_functions 5 | argos3core_simulator) 6 | -------------------------------------------------------------------------------- /client/controllers/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | include_directories(${CMAKE_SOURCE_DIR}/controllers) 2 | include_directories(${CMAKE_SOURCE_DIR}/client/controllers) 3 | 4 | add_subdirectory(footbot_diffusion) 5 | add_subdirectory(footbot_nn) 6 | -------------------------------------------------------------------------------- /client/embedding/galib/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(galib_phototaxis main.cpp) 2 | 3 | target_link_libraries(galib_phototaxis 4 | ${GALIB_LIBRARIES} 5 | footbot_nn 6 | galib_phototaxis_loop_functions 7 | argos3core_simulator) 8 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "client/externalDependencies/rpclib"] 2 | path = client/externalDependencies/rpclib 3 | url = https://github.com/rpclib/rpclib 4 | [submodule "planner/LNS"] 5 | path = planner/LNS 6 | url = https://github.com/lunjohnzhang/MAPF-LNS2.git 7 | -------------------------------------------------------------------------------- /client/controllers/footbot_diffusion/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(footbot_diffusion MODULE footbot_diffusion.h footbot_diffusion.cpp) 2 | target_link_libraries(footbot_diffusion 3 | argos3core_simulator 4 | argos3plugin_simulator_footbot 5 | argos3plugin_simulator_genericrobot 6 | rpc) 7 | -------------------------------------------------------------------------------- /client/loop_functions/mpga_loop_functions/mpga_loop_functions.cpp: -------------------------------------------------------------------------------- 1 | #include "mpga_loop_functions.h" 2 | 3 | CMPGALoopFunctions::CMPGALoopFunctions() : 4 | m_unTrial(0) {} 5 | 6 | UInt32 CMPGALoopFunctions::GetTrial() const { 7 | return m_unTrial; 8 | } 9 | 10 | void CMPGALoopFunctions::SetTrial(UInt32 un_trial) { 11 | m_unTrial = un_trial; 12 | } 13 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .idea/ 2 | .DS_Store 3 | .vscode 4 | */build 5 | */cmake 6 | ./server/ADG/ADG 7 | ./server/ADG/ADG_client 8 | ./server/ADG/ADG_server 9 | ./server/ADG/external 10 | cmake-build-debug/ 11 | client/client_output/* 12 | server_output/* 13 | build/ 14 | ! server_output/.gitkeep 15 | .cache/ 16 | ArgosConfig/__pycache__ 17 | output.argos 18 | outputPath.txt 19 | stats.csv -------------------------------------------------------------------------------- /client/controllers/footbot_nn/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(footbot_nn SHARED 2 | nn/neural_network.h 3 | nn/neural_network.cpp 4 | nn/perceptron.h 5 | nn/perceptron.cpp 6 | nn/ctrnn_multilayer.h 7 | nn/ctrnn_multilayer.cpp 8 | footbot_nn_controller.h 9 | footbot_nn_controller.cpp 10 | ) 11 | target_link_libraries(footbot_nn 12 | argos3core_simulator 13 | argos3plugin_simulator_footbot 14 | argos3plugin_simulator_genericrobot) 15 | -------------------------------------------------------------------------------- /empty-8-8.scen: -------------------------------------------------------------------------------- 1 | version 1 2 | 7 empty-8-8.map 8 8 0 0 7 1 31.31370850 3 | 2 empty-8-8.map 8 8 1 0 24 22 10.24264069 4 | 6 empty-8-8.map 8 8 1 6 28 23 27.48528137 5 | 4 empty-8-8.map 8 8 4 6 16 28 17.07106781 6 | 6 empty-8-8.map 8 8 7 2 7 18 27.48528137 7 | 5 empty-8-8.map 8 8 0 1 5 8 22.82842712 8 | 3 empty-8-8.map 8 8 7 6 12 28 13.24264069 9 | 2 empty-8-8.map 8 8 7 7 25 28 8.24264069 10 | 0 empty-8-8.map 8 8 0 4 17 11 2.82842712 11 | 3 empty-8-8.map 8 8 6 0 0 3 13.82842712 -------------------------------------------------------------------------------- /server/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | 3 | project(ADG_Server_Project) 4 | 5 | set(CMAKE_CXX_STANDARD 20) 6 | set(CMAKE_CXX_STANDARD_REQUIRED True) 7 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread -g -O3 -ffast-math") 8 | find_package(Boost 1.49.0 REQUIRED COMPONENTS program_options system filesystem) 9 | SET(CMAKE_BUILD_TYPE "RELEASE") 10 | 11 | include_directories("inc") 12 | 13 | file(GLOB SOURCES "src/*.cpp") 14 | 15 | add_executable(ADG_server ${SOURCES}) 16 | 17 | target_link_libraries(ADG_server PUBLIC ${Boost_LIBRARIES} PRIVATE rpc pthread) 18 | -------------------------------------------------------------------------------- /client/controllers/footbot_nn/nn/perceptron.h: -------------------------------------------------------------------------------- 1 | #ifndef PERCEPTRON_H 2 | #define PERCEPTRON_H 3 | 4 | #include "neural_network.h" 5 | 6 | class CPerceptron : public CNeuralNetwork { 7 | 8 | public: 9 | 10 | CPerceptron(); 11 | virtual ~CPerceptron(); 12 | 13 | virtual void Init(TConfigurationNode& t_tree); 14 | virtual void Destroy(); 15 | 16 | virtual void LoadNetworkParameters(const std::string& str_filename ); 17 | virtual void LoadNetworkParameters(const UInt32 un_num_params, 18 | const Real* pf_params ); 19 | virtual void ComputeOutputs(); 20 | 21 | private: 22 | 23 | UInt32 m_unNumberOfWeights; 24 | Real* m_pfWeights; 25 | 26 | }; 27 | 28 | #endif 29 | -------------------------------------------------------------------------------- /client/loop_functions/mpga_loop_functions/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(mpga SHARED 2 | mpga.h mpga.cpp 3 | mpga_loop_functions.h mpga_loop_functions.cpp) 4 | target_link_libraries(mpga 5 | argos3core_simulator) 6 | if(NOT APPLE) 7 | target_link_libraries(mpga rt) 8 | endif(NOT APPLE) 9 | 10 | 11 | add_library(mpga_phototaxis_loop_functions SHARED 12 | mpga_phototaxis_loop_functions.h mpga_phototaxis_loop_functions.cpp) 13 | 14 | target_link_libraries(mpga_phototaxis_loop_functions 15 | footbot_nn 16 | argos3core_simulator 17 | argos3plugin_simulator_dynamics2d 18 | argos3plugin_simulator_entities 19 | argos3plugin_simulator_footbot 20 | argos3plugin_simulator_genericrobot 21 | argos3plugin_simulator_media 22 | mpga) 23 | -------------------------------------------------------------------------------- /client/embedding/galib/best_499.dat: -------------------------------------------------------------------------------- 1 | 98 -1.43653 7.16738 6.56878 -2.63837 -10 -0.638123 5.93384 8.58933 -1.0199 -1.88378 -1.93229 2.80587 -0.574501 -2.09195 -6.00061 -6.37288 2.91025 -9.09188 10 -4.62919 6.45953 7.84714 4.08689 -9.72328 2.41131 9.33566 9.19318 1.3382 9.8567 0.00638865 10 2.58671 -0.776294 -2.04926 -5.42514 5.8954 -2.93687 -1.78221 -9.61552 -10 5.79133 -10 7.88713 3.05683 4.60093 10 0.259784 10 9.92049 -5.70221 -3.21433 9.9033 6.05802 6.52961 -7.13085 0.893811 1.54725 -6.05006 -2.32772 -5.18624 -5.17859 -7.95315 1.52641 -6.75208 7.64795 6.21874 -8.27696 1.53463 0.718409 0.683558 -10 1.24405 7.22578 -2.85866 3.50036 7.16349 1.85823 -5.18851 10 7.75738 -10 7.57892 -8.93348 1.0915 -8.42362 8.48132 -6.18351 -6.61759 1.78616 -4.10811 -9.13002 -1.47998 8.81032 9.54057 -1.96425 -3.8113 1.80542 -0.228807 2 | -------------------------------------------------------------------------------- /client/embedding/mpga/best_100.dat: -------------------------------------------------------------------------------- 1 | 98 0.226472 -1.31795 6.65908 -2.73952 2.30849 4.35429 -3.62576 -4.004 2.14046 -4.27118 -0.371472 -1.42637 8.07011 -6.06339 -5.48112 0.467921 4.38297 -5.51311 4.84457 4.2248 0.488706 3.99388 4.48504 9.63178 0.207065 -4.33942 4.43998 6.29274 -3.75031 9.30136 9.21502 -2.90859 9.27785 -3.15692 3.73035 -2.92324 -5.59855 -6.54395 -8.66451 -4.91053 -7.40048 -5.70314 -6.6052 7.55502 -8.88437 8.52443 1.0883 -3.58737 7.72462 -2.83269 8.4869 -3.15982 8.05053 6.48352 -4.33417 -4.85702 -6.77811 -4.53682 -0.666931 5.37878 -3.34261 -4.34809 -5.20477 -5.9206 -8.57737 0.800249 6.09297 -5.77877 -7.75768 -8.15085 0.646152 -9.75725 5.45488 5.87782 3.05765 -4.12361 -0.28628 7.34566 -4.57423 4.57036 -7.8009 7.55817 -5.72845 -6.30286 -4.50393 6.79482 -2.46672 6.65655 7.47367 -1.24747 -2.31204 6.42955 0.58851 -3.01451 3.91519 1.08868 2.74897 8.46202 2 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright 2025 Carnegie Mellon University 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the “Software”), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | 7 | The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 8 | 9 | THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. -------------------------------------------------------------------------------- /client/loop_functions/mpga_loop_functions/mpga_loop_functions.h: -------------------------------------------------------------------------------- 1 | #ifndef MPGA_LOOP_FUNCTIONS_H 2 | #define MPGA_LOOP_FUNCTIONS_H 3 | 4 | #include 5 | 6 | using namespace argos; 7 | 8 | 9 | class CMPGALoopFunctions : public CLoopFunctions { 10 | 11 | public: 12 | 13 | CMPGALoopFunctions(); 14 | 15 | /** 16 | * Class destructor. 17 | */ 18 | virtual ~CMPGALoopFunctions() {} 19 | 20 | /** 21 | * Configures the trial using the passed genome. 22 | * @param pf_genome The genome. 23 | */ 24 | virtual void ConfigureFromGenome(const Real* pf_genome) = 0; 25 | 26 | /** 27 | * Returns the current trial. 28 | */ 29 | virtual UInt32 GetTrial() const; 30 | 31 | /** 32 | * Sets the current trial. 33 | * @param un_trial The trial number. 34 | */ 35 | virtual void SetTrial(UInt32 un_trial); 36 | 37 | /** 38 | * Returns the score associated to the current trial. 39 | */ 40 | virtual Real Score() = 0; 41 | 42 | private: 43 | 44 | UInt32 m_unTrial; 45 | 46 | }; 47 | 48 | #endif 49 | -------------------------------------------------------------------------------- /create_map.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | 4 | # Define the map from the given text 5 | map_text = [ 6 | "..........@.....", 7 | "@...@.@@........", 8 | "..............@.", 9 | ".....@.@........", 10 | ".@@...........@@", 11 | "......@..@.@..@@", 12 | "......@..@@.....", 13 | ".@...@.........." 14 | ] 15 | 16 | # Define the map size 17 | height = len(map_text) 18 | width = len(map_text[0]) 19 | 20 | # Create an empty numpy array for the image (0 for open space, 1 for obstacles) 21 | map_array = np.zeros((height, width)) 22 | 23 | # Convert map text to numerical array (1 for obstacles, 0 for open spaces) 24 | for i, row in enumerate(map_text): 25 | for j, cell in enumerate(row): 26 | if cell == '@': 27 | map_array[i, j] = 1 # Mark obstacles 28 | 29 | # Plot the map 30 | plt.figure(figsize=(width / 2, height / 2)) 31 | plt.imshow(map_array, cmap="gray_r", origin="upper") 32 | 33 | # Remove axis labels 34 | plt.xticks([]) 35 | plt.yticks([]) 36 | plt.tight_layout() 37 | 38 | # Show the map 39 | # plt.show() 40 | plt.savefig("2d-map.png") -------------------------------------------------------------------------------- /client/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8.0) 2 | project(argos3_controller) 3 | 4 | if(CMAKE_BUILD_TYPE STREQUAL "") 5 | set(CMAKE_BUILD_TYPE "Release" CACHE STRING "Choose the type of build, options are: None Debug Release RelWithDebInfo MinSizeRel ..." FORCE) 6 | endif(CMAKE_BUILD_TYPE STREQUAL "") 7 | 8 | set(CMAKE_MACOSX_RPATH 0) 9 | 10 | set(CMAKE_MODULE_PATH $ENV{CMAKE_MODULE_PATH} ${CMAKE_SOURCE_DIR}/cmake) 11 | set(CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH} ${CMAKE_SOURCE_DIR}/cmake) 12 | 13 | set(CMAKE_CXX_STANDARD 17) 14 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 15 | set(CMAKE_CXX_EXTENSIONS OFF) 16 | 17 | find_package(ARGoS REQUIRED) 18 | include_directories(${CMAKE_SOURCE_DIR} ${ARGOS_INCLUDE_DIRS} ${CMAKE_SOURCE_DIR}/client) 19 | link_directories(${ARGOS_LIBRARY_DIR}) 20 | link_libraries(${ARGOS_LDFLAGS}) 21 | 22 | find_package(GALIB) 23 | if(GALIB_FOUND) 24 | include_directories(${GALIB_INCLUDE_DIRS}) 25 | endif(GALIB_FOUND) 26 | 27 | add_subdirectory(externalDependencies/rpclib) 28 | include_directories(externalDependencies/rpclib/include) 29 | 30 | add_subdirectory(controllers) 31 | 32 | add_subdirectory(loop_functions) 33 | 34 | add_subdirectory(embedding) 35 | -------------------------------------------------------------------------------- /client/loop_functions/mpga_loop_functions/mpga_phototaxis_loop_functions.h: -------------------------------------------------------------------------------- 1 | #ifndef MPGA_PHOTOTAXIS_LOOP_FUNCTIONS_H 2 | #define MPGA_PHOTOTAXIS_LOOP_FUNCTIONS_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | static const size_t GENOME_SIZE = 98; 10 | 11 | using namespace argos; 12 | 13 | class CMPGAPhototaxisLoopFunctions : public CMPGALoopFunctions { 14 | 15 | public: 16 | 17 | CMPGAPhototaxisLoopFunctions(); 18 | virtual ~CMPGAPhototaxisLoopFunctions(); 19 | 20 | virtual void Init(TConfigurationNode& t_node); 21 | virtual void Reset(); 22 | 23 | virtual void ConfigureFromGenome(const Real* pf_genome); 24 | virtual Real Score(); 25 | 26 | private: 27 | 28 | struct SInitSetup { 29 | CVector3 Position; 30 | CQuaternion Orientation; 31 | }; 32 | 33 | std::vector m_vecInitSetup; 34 | CFootBotEntity* m_pcFootBot; 35 | CFootBotNNController* m_pcController; 36 | Real* m_pfControllerParams; 37 | CRandom::CRNG* m_pcRNG; 38 | 39 | }; 40 | 41 | #endif 42 | -------------------------------------------------------------------------------- /random-32-32-20.map: -------------------------------------------------------------------------------- 1 | type octile 2 | height 32 3 | width 32 4 | map 5 | ..........@......@...@.@........ 6 | @...@.@@...........@.@...@...... 7 | ..............@........@@...@... 8 | .....@.@..........@..........@.. 9 | .@@...........@@.@......@..@.... 10 | ......@..@.@..@@......@....@..@@ 11 | ......@..@@........@.......@...@ 12 | .@...@...............@@...@..@.. 13 | ......@.........@.......@.@..... 14 | ..@.@............@.@.......@@... 15 | @.@.........@.@.......@...@..... 16 | ............@.....@.@.@.......@. 17 | ..@.....@.@.@@.@..........@..... 18 | .@................@.@.....@...@. 19 | ...@@@.......@.........@........ 20 | ........@....@.@.@..@........... 21 | ..@...@........@....@..@@@....@. 22 | .@......@@@..@.@..@.@..@@....@T@ 23 | @.........@........@..@......@@@ 24 | ......@...@..@..@.@...@...@..... 25 | .@.@........@.........@@.@...@.. 26 | .......@.@........@..@@@..@@@... 27 | ..@.@..@....@......@..........@. 28 | .......@.....@....@...@.@.@@.... 29 | .................@...@.@..@.@... 30 | ................@........@..@..@ 31 | .@......@.@.........@@.........@ 32 | ..@..@..@....................... 33 | @..@@......@...@...@.........@@@ 34 | @....@....@....@.@.@...@......@@ 35 | @@.@..@@.@.@@.....@........@.... 36 | @@.............@..@.........@... 37 | -------------------------------------------------------------------------------- /server/inc/parser.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | using namespace std; 12 | 13 | struct Point { 14 | int x, y; 15 | double time; 16 | }; 17 | 18 | struct Step { 19 | int x, y, orientation; 20 | double time; 21 | }; 22 | 23 | struct Action { 24 | int robot_id; 25 | double time; 26 | double orientation; 27 | char type; 28 | std::pair start; 29 | std::pair goal; 30 | int nodeID; 31 | }; 32 | 33 | enum PlanType { 34 | DISCRETE, 35 | CONTINUOUS 36 | }; 37 | 38 | int getOrientation(int x1, int y1, int x2, int y2); 39 | void processAgentActions(const vector& points, vector& steps, int agentId); 40 | vector parseLine(const string& line); 41 | std::vector> processActions(const std::vector>& raw_steps, bool flipped_coord); 42 | bool parseEntirePlan(const std::string& input_file, std::vector>& plans, 43 | double& raw_cost, bool flipped_coord = true, PlanType file_type = PlanType::CONTINUOUS); 44 | void raiseError(const string &msg); 45 | -------------------------------------------------------------------------------- /server/inc/ADG_server.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include "ADG.h" 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include "json.hpp" 18 | using json = nlohmann::json; 19 | 20 | #include 21 | 22 | std::mutex globalMutex; 23 | 24 | class ADG_Server{ 25 | public: 26 | ADG_Server(std::string& path_filename, std::string& target_output_filename, 27 | std::string map_name, std::string scen_name, std::string method_name, bool flip_coord); 28 | void saveStats(); 29 | 30 | std::shared_ptr adg; 31 | std::map robotIDTOStartIndex; 32 | std::map startIndexToRobotID; 33 | std::vector> plans; 34 | std::vector> outgoingEdgesByRobot; 35 | std::vector agent_finish_time; 36 | std::vector agent_finish_sim_step; 37 | int latest_arr_sim_step = 0; 38 | bool all_agents_finished = false; 39 | std::vector agents_finish; 40 | std::string output_filename; 41 | std::string curr_map_name; 42 | std::string curr_scen_name; 43 | std::string curr_method_name; 44 | int numRobots = 0; 45 | int step_cnt = 0; 46 | 47 | private: 48 | std::string path_filename_; 49 | double raw_plan_cost = -1.0; 50 | }; 51 | -------------------------------------------------------------------------------- /client/controllers/footbot_nn/nn/ctrnn_multilayer.h: -------------------------------------------------------------------------------- 1 | #ifndef CTRNN_MULTILAYER_H 2 | #define CTRNN_MULTILAYER_H 3 | 4 | #include "neural_network.h" 5 | #include 6 | 7 | class CCtrnnMultilayer : public CNeuralNetwork { 8 | 9 | public: 10 | 11 | CCtrnnMultilayer(); 12 | virtual ~CCtrnnMultilayer(); 13 | 14 | virtual void Init(TConfigurationNode& t_tree); 15 | virtual void Reset(); 16 | virtual void Destroy(); 17 | 18 | virtual void LoadNetworkParameters(const std::string& str_filename); 19 | virtual void LoadNetworkParameters(const UInt32 un_num_params, 20 | const Real* pf_params ); 21 | virtual void ComputeOutputs(); 22 | 23 | 24 | inline UInt32 GetNumberOfHiddenNodes() { return m_unNumberOfHiddenNodes; } 25 | inline const Real* GetHiddenStates() { return m_pfHiddenStates; } 26 | inline const Real* GetHiddenTaus() { return m_pfHiddenTaus; } 27 | inline const Real* GetHiddenBias() { return m_pfHiddenBiases; } 28 | inline const Real* GetOutputBias() { return m_pfOutputBiases; } 29 | 30 | protected: 31 | 32 | Real* m_pfInputToHiddenWeights; 33 | 34 | Real* m_pfHiddenBiases; 35 | Real* m_pfHiddenTaus; 36 | Real* m_pfHiddenDeltaStates; 37 | Real* m_pfHiddenStates; 38 | 39 | Real* m_pfHiddenToHiddenWeights; 40 | Real* m_pfHiddenToOutputWeights; 41 | 42 | Real* m_pfOutputBiases; 43 | Real* m_pfOutputTaus; 44 | 45 | UInt32 m_unNumberOfHiddenNodes; 46 | Real m_fTimeStep; 47 | 48 | CRange m_cWeightsBounds; 49 | CRange m_cBiasesBounds; 50 | CRange m_cTausBounds; 51 | 52 | }; 53 | 54 | #endif 55 | -------------------------------------------------------------------------------- /client/controllers/footbot_nn/footbot_nn_controller.h: -------------------------------------------------------------------------------- 1 | #ifndef FOOTBOT_NN_CONTROLLER 2 | #define FOOTBOT_NN_CONTROLLER 3 | 4 | /* 5 | * Include some necessary headers. 6 | */ 7 | /* Definition of the CCI_Controller class. */ 8 | #include 9 | /* Definition of the differential steering actuator */ 10 | #include 11 | /* Definition of the foot-bot proximity sensor */ 12 | #include 13 | /* Definition of the foot-bot light sensor */ 14 | #include 15 | /* Definition of the perceptron */ 16 | #include "nn/perceptron.h" 17 | 18 | /* 19 | * All the ARGoS stuff in the 'argos' namespace. 20 | * With this statement, you save typing argos:: every time. 21 | */ 22 | using namespace argos; 23 | 24 | /* 25 | * A controller is simply an implementation of the CCI_Controller class. 26 | * In this case, we also inherit from the CPerceptron class. We use 27 | * virtual inheritance so that matching methods in the CCI_Controller 28 | * and CPerceptron don't get messed up. 29 | */ 30 | class CFootBotNNController : public CCI_Controller { 31 | 32 | public: 33 | 34 | CFootBotNNController(); 35 | virtual ~CFootBotNNController(); 36 | 37 | void Init(TConfigurationNode& t_node); 38 | void ControlStep(); 39 | void Reset(); 40 | void Destroy(); 41 | 42 | inline CPerceptron& GetPerceptron() { 43 | return m_cPerceptron; 44 | } 45 | 46 | private: 47 | 48 | /* Pointer to the differential steering actuator */ 49 | CCI_DifferentialSteeringActuator* m_pcWheels; 50 | /* Pointer to the foot-bot proximity sensor */ 51 | CCI_FootBotProximitySensor* m_pcProximity; 52 | /* Pointer to the foot-bot light sensor */ 53 | CCI_FootBotLightSensor* m_pcLight; 54 | /* The perceptron neural network */ 55 | CPerceptron m_cPerceptron; 56 | /* Wheel speeds */ 57 | Real m_fLeftSpeed, m_fRightSpeed; 58 | 59 | }; 60 | 61 | #endif 62 | -------------------------------------------------------------------------------- /client/embedding/mpga/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | void FlushIndividual(const CMPGA::SIndividual& s_ind, 7 | UInt32 un_generation) { 8 | std::ostringstream cOSS; 9 | cOSS << "best_" << un_generation << ".dat"; 10 | std::ofstream cOFS(cOSS.str().c_str(), std::ios::out | std::ios::trunc); 11 | cOFS << GENOME_SIZE; 12 | for(UInt32 i = 0; i < GENOME_SIZE; ++i) { 13 | cOFS << " " << s_ind.Genome[i]; 14 | } 15 | cOFS << std::endl; 16 | } 17 | 18 | Real ScoreAggregator(const std::vector& vec_scores) { 19 | Real fScore = vec_scores[0]; 20 | for(size_t i = 1; i < vec_scores.size(); ++i) { 21 | fScore = Max(fScore, vec_scores[i]); 22 | } 23 | return fScore; 24 | } 25 | 26 | int main() { 27 | CMPGA cGA(CRange(-10.0,10.0), 28 | GENOME_SIZE, 29 | 5, 30 | 0.05, 31 | 5, 32 | 100, 33 | false, 34 | "experiments/mpga.argos", 35 | &ScoreAggregator, 36 | 12345 37 | ); 38 | cGA.Evaluate(); 39 | argos::LOG << "Generation #" << cGA.GetGeneration() << "..."; 40 | argos::LOG << " scores:"; 41 | for(UInt32 i = 0; i < cGA.GetPopulation().size(); ++i) { 42 | argos::LOG << " " << cGA.GetPopulation()[i]->Score; 43 | } 44 | LOG << std::endl; 45 | LOG.Flush(); 46 | while(!cGA.Done()) { 47 | cGA.NextGen(); 48 | cGA.Evaluate(); 49 | argos::LOG << "Generation #" << cGA.GetGeneration() << "..."; 50 | argos::LOG << " scores:"; 51 | for(UInt32 i = 0; i < cGA.GetPopulation().size(); ++i) { 52 | argos::LOG << " " << cGA.GetPopulation()[i]->Score; 53 | } 54 | if(cGA.GetGeneration() % 5 == 0) { 55 | argos::LOG << " [Flushing genome... "; 56 | FlushIndividual(*cGA.GetPopulation()[0], 57 | cGA.GetGeneration()); 58 | argos::LOG << "done.]"; 59 | } 60 | LOG << std::endl; 61 | LOG.Flush(); 62 | } 63 | return 0; 64 | } 65 | -------------------------------------------------------------------------------- /client/controllers/footbot_nn/nn/neural_network.h: -------------------------------------------------------------------------------- 1 | #ifndef NEURAL_NETWORK_H 2 | #define NEURAL_NETWORK_H 3 | 4 | #include 5 | 6 | using namespace argos; 7 | 8 | class CNeuralNetwork { 9 | 10 | public: 11 | 12 | CNeuralNetwork(); 13 | virtual ~CNeuralNetwork(); 14 | 15 | virtual void Init(TConfigurationNode& t_node); 16 | virtual void Reset(); 17 | virtual void Destroy(); 18 | 19 | virtual void LoadNetworkParameters(const std::string& str_filename) = 0; 20 | virtual void LoadNetworkParameters(const UInt32 un_num_params, 21 | const Real* pf_params) = 0; 22 | virtual void ComputeOutputs() = 0; 23 | 24 | inline UInt32 GetNumberOfInputs() { 25 | return m_unNumberOfInputs; 26 | } 27 | inline void SetNumberOfInputs(UInt32 un_inputs) { 28 | m_unNumberOfInputs = un_inputs; 29 | } 30 | 31 | inline const Real* GetInputs() { 32 | return m_pfInputs; 33 | } 34 | void SetInput(UInt32 un_input_num, 35 | Real f_input_value) { 36 | m_pfInputs[un_input_num] = f_input_value; 37 | } 38 | 39 | void SetInputRange(UInt32 un_input_start, 40 | UInt32 un_num_values, 41 | Real* pf_input_values ) { 42 | ::memcpy(m_pfInputs + un_input_start, 43 | pf_input_values, 44 | un_num_values); 45 | } 46 | 47 | inline UInt32 GetNumberOfOutputs() { 48 | return m_unNumberOfOutputs; 49 | } 50 | inline void SetNumberOfOutputs(UInt32 un_outputs) { 51 | m_unNumberOfOutputs = un_outputs; 52 | } 53 | 54 | inline const Real* GetOutputs() { 55 | return m_pfOutputs; 56 | } 57 | inline Real GetOutput(UInt32 un_num_output) { 58 | return m_pfOutputs[un_num_output]; 59 | } 60 | 61 | virtual void SetOnlineParameters(const UInt32 un_num_params, 62 | const Real* pf_params); 63 | 64 | protected: 65 | 66 | UInt32 m_unNumberOfInputs; 67 | UInt32 m_unNumberOfOutputs; 68 | 69 | Real* m_pfInputs; 70 | Real* m_pfOutputs; 71 | 72 | std::string m_strParameterFile; 73 | 74 | }; 75 | 76 | #endif 77 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ![Banner](https://github.com/user-attachments/assets/c43d8b2d-16e2-45c4-999e-b25d45b9cb1b) 2 | 3 | [Open the app](https://smart-mapf.github.io/demo/) 4 | 5 | --- 6 | 7 | # Scalable Multi-Agent Realistic Testbed (SMART) 8 | 9 | SMART is an efficient soft-tool that bridge the gap between Multi-Agent Path Finding (MAPF) and real-world application. More details can be found in our paper: 10 | Advancing MAPF towards the Real World: A Scalable Multi-Agent Realistic Testbed (SMART) [1]. 11 | 12 | 13 | ## Requirements 14 | 15 | ### Ubuntu 22.04 16 | 17 | ### RPC 18 | This repo requires [RPC](https://github.com/rpclib/rpclib) for communication 19 | between server and clients. 20 | 21 | ### Argos 22 | Install Argos 3, please refer to this [Link](https://www.argos-sim.info/core.php) for instruction. 23 | 24 | 25 | ## Compile instructions 26 | - To build the project, first navigate to the home folder Then: 27 | ``` 28 | git submodule init 29 | git submodule update 30 | mkdir build 31 | cd build 32 | cmake .. 33 | make -j 34 | ``` 35 | 36 | 37 | If a linking error occurs, please remove the conda from your path (e.g. in your ~/.bashrc). 38 | 39 | To produce fast but not debuggable code, type: 40 | ```angular2html 41 | cmake -DCMAKE_BUILD_TYPE=Release .. 42 | make 43 | cd .. 44 | ``` 45 | 46 | 47 | ## Running guide for entire pipeline 48 | 49 | ### Running with visualization 50 | ``` 51 | python run_sim.py --map_name=random-32-32-20.map --scen_name=random-32-32-20-random-1.scen --num_agents=50 --path_filename=example_paths_xy.txt --flip_coord=0 52 | ``` 53 | ``` 54 | python run_sim.py --map_name=random-32-32-20.map --scen_name=random-32-32-20-random-1.scen --num_agents=50 --path_filename=example_paths_yx.txt --flip_coord=1 55 | ``` 56 | 57 | ### Running in headless mode 58 | ``` 59 | python run_sim.py --map_name=random-32-32-20.map --scen_name=random-32-32-20-random-1.scen --num_agents=50 --path_filename=example_paths_xy.txt --flip_coord=0 --headless=True 60 | ``` 61 | ``` 62 | python run_sim.py --map_name=random-32-32-20.map --scen_name=random-32-32-20-random-1.scen --num_agents=50 --path_filename=example_paths_yx.txt --flip_coord=1 --headless=True 63 | ``` 64 | 65 | 66 | ## Reference 67 | [1] Yan, Jingtian, Zhifei Li, William Kang, Kevin Zheng, Yulun Zhang, Zhe Chen, Yue Zhang, Daniel Harabor, Stephen Smith, and Jiaoyang Li. "Advancing MAPF towards the Real World: A Scalable Multi-Agent Realistic Testbed (SMART)." arXiv preprint arXiv:2503.04798 (2025). 68 | -------------------------------------------------------------------------------- /client/embedding/galib/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | float LaunchARGoS(GAGenome& c_genome) { 7 | GARealGenome& cRealGenome = dynamic_cast(c_genome); 8 | static argos::CSimulator& cSimulator = argos::CSimulator::GetInstance(); 9 | static CGALibPhototaxisLoopFunctions& cLoopFunctions = dynamic_cast(cSimulator.GetLoopFunctions()); 10 | Real fDistance = 0.0f; 11 | for(size_t i = 0; i < 5; ++i) { 12 | cLoopFunctions.SetTrial(i); 13 | cSimulator.Reset(); 14 | cLoopFunctions.ConfigureFromGenome(cRealGenome); 15 | cSimulator.Execute(); 16 | fDistance = Max(fDistance, cLoopFunctions.Performance()); 17 | } 18 | return fDistance; 19 | } 20 | 21 | void FlushBest(const GARealGenome& c_genome, size_t un_generation) { 22 | std::ostringstream cOSS; 23 | cOSS << "best_" << un_generation << ".dat"; 24 | std::ofstream cOFS(cOSS.str().c_str(), std::ios::out | std::ios::trunc); 25 | cOFS << GENOME_SIZE 26 | << " " 27 | << c_genome 28 | << std::endl; 29 | } 30 | 31 | int main(int argc, char** argv) { 32 | GAAlleleSet cAlleleSet(-10.0f, 10.0f); 33 | GARealGenome cGenome(GENOME_SIZE, cAlleleSet, LaunchARGoS); 34 | GASimpleGA cGA(cGenome); 35 | cGA.minimize(); 36 | cGA.populationSize(5); 37 | cGA.nGenerations(500); 38 | cGA.pMutation(0.05f); 39 | cGA.pCrossover(0.15f); 40 | cGA.scoreFilename("evolution.dat"); 41 | cGA.flushFrequency(1); 42 | 43 | argos::CSimulator& cSimulator = argos::CSimulator::GetInstance(); 44 | cSimulator.SetExperimentFileName("experiments/galib.argos"); 45 | cSimulator.LoadExperiment(); 46 | 47 | cGA.initialize(12345); 48 | do { 49 | argos::LOG << "Generation #" << cGA.generation() << "..."; 50 | cGA.step(); 51 | argos::LOG << "done."; 52 | if(cGA.generation() % cGA.flushFrequency() == 0) { 53 | argos::LOG << " Flushing..."; 54 | cGA.flushScores(); 55 | FlushBest(dynamic_cast(cGA.statistics().bestIndividual()), cGA.generation()); 56 | argos::LOG << "done."; 57 | } 58 | LOG << std::endl; 59 | LOG.Flush(); 60 | } 61 | while(! cGA.done()); 62 | 63 | cSimulator.Destroy(); 64 | 65 | return 0; 66 | } 67 | -------------------------------------------------------------------------------- /client/loop_functions/mpga_loop_functions/mpga_phototaxis_loop_functions.cpp: -------------------------------------------------------------------------------- 1 | #include "mpga_phototaxis_loop_functions.h" 2 | 3 | CMPGAPhototaxisLoopFunctions::CMPGAPhototaxisLoopFunctions() : 4 | m_vecInitSetup(5), 5 | m_pcFootBot(NULL), 6 | m_pcController(NULL), 7 | m_pfControllerParams(new Real[GENOME_SIZE]), 8 | m_pcRNG(NULL) {} 9 | 10 | CMPGAPhototaxisLoopFunctions::~CMPGAPhototaxisLoopFunctions() { 11 | delete[] m_pfControllerParams; 12 | } 13 | 14 | void CMPGAPhototaxisLoopFunctions::Init(TConfigurationNode& t_node) { 15 | m_pcRNG = CRandom::CreateRNG("argos"); 16 | 17 | m_pcFootBot = new CFootBotEntity( 18 | "fb", 19 | "fnn" 20 | ); 21 | AddEntity(*m_pcFootBot); 22 | m_pcController = &dynamic_cast(m_pcFootBot->GetControllableEntity().GetController()); 23 | 24 | CRadians cOrient; 25 | for(size_t i = 0; i < 5; ++i) { 26 | m_vecInitSetup[i].Position.FromSphericalCoords( 27 | 4.5f, 28 | CRadians::PI_OVER_TWO, 29 | static_cast(i+1) * CRadians::PI / 12.0f 30 | ); 31 | cOrient = m_pcRNG->Uniform(CRadians::UNSIGNED_RANGE); 32 | m_vecInitSetup[i].Orientation.FromEulerAngles( 33 | cOrient, 34 | CRadians::ZERO, 35 | CRadians::ZERO 36 | ); 37 | } 38 | 39 | try { 40 | UInt32 unTrial; 41 | GetNodeAttribute(t_node, "trial", unTrial); 42 | SetTrial(unTrial); 43 | Reset(); 44 | } 45 | catch(CARGoSException& ex) {} 46 | } 47 | 48 | void CMPGAPhototaxisLoopFunctions::Reset() { 49 | if(!MoveEntity( 50 | m_pcFootBot->GetEmbodiedEntity(), 51 | m_vecInitSetup[GetTrial()].Position, 52 | m_vecInitSetup[GetTrial()].Orientation, 53 | false 54 | )) { 55 | LOGERR << "Can't move robot in <" 56 | << m_vecInitSetup[GetTrial()].Position 57 | << ">, <" 58 | << m_vecInitSetup[GetTrial()].Orientation 59 | << ">" 60 | << std::endl; 61 | } 62 | } 63 | 64 | void CMPGAPhototaxisLoopFunctions::ConfigureFromGenome(const Real* pf_genome) { 65 | for(size_t i = 0; i < GENOME_SIZE; ++i) { 66 | m_pfControllerParams[i] = pf_genome[i]; 67 | } 68 | m_pcController->GetPerceptron().SetOnlineParameters(GENOME_SIZE, m_pfControllerParams); 69 | } 70 | 71 | Real CMPGAPhototaxisLoopFunctions::Score() { 72 | return m_pcFootBot->GetEmbodiedEntity().GetOriginAnchor().Position.Length(); 73 | } 74 | 75 | REGISTER_LOOP_FUNCTIONS(CMPGAPhototaxisLoopFunctions, "mpga_phototaxis_loop_functions") 76 | -------------------------------------------------------------------------------- /client/controllers/footbot_nn/nn/neural_network.cpp: -------------------------------------------------------------------------------- 1 | #include "neural_network.h" 2 | #include 3 | 4 | /****************************************/ 5 | /****************************************/ 6 | 7 | CNeuralNetwork::CNeuralNetwork() : 8 | m_unNumberOfInputs(0), 9 | m_unNumberOfOutputs(0), 10 | m_pfInputs(NULL), 11 | m_pfOutputs(NULL) {} 12 | 13 | /****************************************/ 14 | /****************************************/ 15 | 16 | CNeuralNetwork::~CNeuralNetwork() { 17 | if(m_pfInputs) delete[] m_pfInputs; 18 | if(m_pfOutputs) delete[] m_pfOutputs; 19 | } 20 | 21 | /****************************************/ 22 | /****************************************/ 23 | 24 | void CNeuralNetwork::Init(TConfigurationNode& t_node) { 25 | // Get the number of inputs, and initialise the input vector 26 | try { 27 | GetNodeAttribute(t_node, "num_inputs", m_unNumberOfInputs); 28 | } 29 | catch(CARGoSException& ex) { 30 | THROW_ARGOSEXCEPTION_NESTED("missing number of inputs for the neural network controller.", ex); 31 | } 32 | 33 | m_pfInputs = new Real[m_unNumberOfInputs]; 34 | ::memset(m_pfInputs, 0, sizeof(Real) * m_unNumberOfInputs); 35 | 36 | // Get the number of outputs, and initialise the output vector 37 | try { 38 | GetNodeAttribute(t_node, "num_outputs", m_unNumberOfOutputs); 39 | } 40 | catch(CARGoSException& ex) { 41 | THROW_ARGOSEXCEPTION_NESTED("missing number of outputs for the neural network controller.", ex); 42 | } 43 | 44 | m_pfOutputs = new Real[m_unNumberOfOutputs]; 45 | ::memset(m_pfOutputs, 0, sizeof(Real) * m_unNumberOfOutputs); 46 | 47 | // name of the parameter file 48 | GetNodeAttributeOrDefault( t_node, "parameter_file", m_strParameterFile, m_strParameterFile); 49 | } 50 | 51 | 52 | /****************************************/ 53 | /****************************************/ 54 | 55 | void CNeuralNetwork::Reset() { 56 | // reset the input vector 57 | ::memset(m_pfInputs, 0, sizeof(Real) * m_unNumberOfInputs); 58 | // reset the output vector 59 | ::memset(m_pfOutputs, 0, sizeof(Real) * m_unNumberOfOutputs); 60 | } 61 | 62 | 63 | /****************************************/ 64 | /****************************************/ 65 | 66 | void CNeuralNetwork::Destroy() { 67 | if( m_pfInputs ) delete[] m_pfInputs; 68 | m_pfInputs = NULL; 69 | m_unNumberOfInputs = 0; 70 | 71 | if( m_pfOutputs ) delete[] m_pfOutputs; 72 | m_pfOutputs = NULL; 73 | m_unNumberOfOutputs = 0; 74 | } 75 | 76 | 77 | /****************************************/ 78 | /****************************************/ 79 | 80 | void CNeuralNetwork::SetOnlineParameters(const UInt32 un_num_params, 81 | const Real* pf_params ) { 82 | LoadNetworkParameters(un_num_params, pf_params); 83 | } 84 | 85 | /****************************************/ 86 | /****************************************/ 87 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16) 2 | 3 | # Main project 4 | project(Lifelong_SMART VERSION 1.0.0) 5 | 6 | # Set global C++ standard (use C++17 as minimum to satisfy all components) 7 | set(CMAKE_CXX_STANDARD 17) 8 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 9 | set(CMAKE_CXX_EXTENSIONS OFF) 10 | 11 | # Set build type to Release by default if not specified 12 | if(NOT CMAKE_BUILD_TYPE) 13 | set(CMAKE_BUILD_TYPE Release) 14 | endif() 15 | 16 | # Global compiler flags 17 | # Ensure Release builds use optimizations and disable assertions via NDEBUG 18 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -ffast-math -pthread -DNDEBUG") 19 | # Define NDEBUG for Release build so plain C `assert()` is disabled. 20 | if(CMAKE_BUILD_TYPE STREQUAL "Release") 21 | add_compile_definitions(NDEBUG) 22 | endif() 23 | 24 | # Print build configuration 25 | message(STATUS "Build type: ${CMAKE_BUILD_TYPE}") 26 | message(STATUS "C++ standard: ${CMAKE_CXX_STANDARD}") 27 | 28 | # Global options 29 | option(BUILD_SERVER "Build the ADG server component" ON) 30 | option(BUILD_CLIENT "Build the ARGoS client component" ON) 31 | option(BUILD_ALL "Build all components" ON) 32 | 33 | # Find common dependencies 34 | find_package(Boost 1.49.0 REQUIRED COMPONENTS program_options system filesystem) 35 | 36 | # Add subdirectories based on options 37 | if(BUILD_ALL OR BUILD_SERVER) 38 | message(STATUS "Adding server component...") 39 | add_subdirectory(server) 40 | endif() 41 | 42 | if(BUILD_ALL OR BUILD_CLIENT) 43 | message(STATUS "Adding client component...") 44 | # Check if ARGoS is available before building client 45 | find_package(ARGoS QUIET) 46 | if(ARGoS_FOUND) 47 | add_subdirectory(client) 48 | message(STATUS "ARGoS found - client component will be built") 49 | else() 50 | message(WARNING "ARGoS not found - skipping client component. Install ARGoS3 to build client.") 51 | endif() 52 | endif() 53 | 54 | # Create a custom target to build all executables 55 | add_custom_target(all_executables 56 | COMMENT "Building all executables in the project" 57 | ) 58 | 59 | # Add dependencies for the custom target 60 | if(TARGET ADG_server) 61 | add_dependencies(all_executables ADG_server) 62 | endif() 63 | 64 | if(TARGET mpga_phototaxis) 65 | add_dependencies(all_executables mpga_phototaxis) 66 | endif() 67 | 68 | if(TARGET galib_phototaxis) 69 | add_dependencies(all_executables galib_phototaxis) 70 | endif() 71 | 72 | if(TARGET lns) 73 | add_dependencies(all_executables lns) 74 | endif() 75 | 76 | if(TARGET pbs) 77 | add_dependencies(all_executables pbs) 78 | endif() 79 | 80 | if(TARGET benchmark_pbs) 81 | add_dependencies(all_executables benchmark_pbs) 82 | endif() 83 | 84 | # Print summary 85 | message(STATUS "") 86 | message(STATUS "=== Lifelong-SMART Build Configuration ===") 87 | message(STATUS "Server component: ${BUILD_SERVER}") 88 | message(STATUS "Client component: ${BUILD_CLIENT}") 89 | message(STATUS "Build type: ${CMAKE_BUILD_TYPE}") 90 | message(STATUS "==========================================") -------------------------------------------------------------------------------- /client/controllers/footbot_nn/footbot_nn_controller.cpp: -------------------------------------------------------------------------------- 1 | #include "footbot_nn_controller.h" 2 | 3 | /****************************************/ 4 | /****************************************/ 5 | 6 | static CRange NN_OUTPUT_RANGE(0.0f, 1.0f); 7 | static CRange WHEEL_ACTUATION_RANGE(-5.0f, 5.0f); 8 | 9 | /****************************************/ 10 | /****************************************/ 11 | 12 | CFootBotNNController::CFootBotNNController() { 13 | } 14 | 15 | /****************************************/ 16 | /****************************************/ 17 | 18 | CFootBotNNController::~CFootBotNNController() { 19 | } 20 | 21 | /****************************************/ 22 | /****************************************/ 23 | 24 | void CFootBotNNController::Init(TConfigurationNode& t_node) { 25 | /* 26 | * Get sensor/actuator handles 27 | */ 28 | try { 29 | m_pcWheels = GetActuator("differential_steering"); 30 | m_pcProximity = GetSensor ("footbot_proximity" ); 31 | m_pcLight = GetSensor ("footbot_light" ); 32 | } 33 | catch(CARGoSException& ex) { 34 | THROW_ARGOSEXCEPTION_NESTED("Error initializing sensors/actuators", ex); 35 | } 36 | 37 | /* Initialize the perceptron */ 38 | try { 39 | m_cPerceptron.Init(t_node); 40 | } 41 | catch(CARGoSException& ex) { 42 | THROW_ARGOSEXCEPTION_NESTED("Error initializing the perceptron network", ex); 43 | } 44 | } 45 | 46 | /****************************************/ 47 | /****************************************/ 48 | 49 | void CFootBotNNController::ControlStep() { 50 | /* Get sensory data */ 51 | const CCI_FootBotProximitySensor::TReadings& tProx = m_pcProximity->GetReadings(); 52 | const CCI_FootBotLightSensor::TReadings& tLight = m_pcLight->GetReadings(); 53 | /* Fill NN inputs from sensory data */ 54 | for(size_t i = 0; i < tProx.size(); ++i) { 55 | m_cPerceptron.SetInput(i, tProx[i].Value); 56 | } 57 | for(size_t i = 0; i < tLight.size(); ++i) { 58 | m_cPerceptron.SetInput(tProx.size()+i, tLight[i].Value); 59 | } 60 | /* Compute NN outputs */ 61 | m_cPerceptron.ComputeOutputs(); 62 | /* 63 | * Apply NN outputs to actuation 64 | * The NN outputs are in the range [0,1] 65 | * To allow for backtracking, we remap this range 66 | * into [-5:5] linearly. 67 | */ 68 | NN_OUTPUT_RANGE.MapValueIntoRange( 69 | m_fLeftSpeed, // value to write 70 | m_cPerceptron.GetOutput(0), // value to read 71 | WHEEL_ACTUATION_RANGE // target range (here [-5:5]) 72 | ); 73 | NN_OUTPUT_RANGE.MapValueIntoRange( 74 | m_fRightSpeed, // value to write 75 | m_cPerceptron.GetOutput(1), // value to read 76 | WHEEL_ACTUATION_RANGE // target range (here [-5:5]) 77 | ); 78 | m_pcWheels->SetLinearVelocity( 79 | m_fLeftSpeed, 80 | m_fRightSpeed); 81 | } 82 | 83 | /****************************************/ 84 | /****************************************/ 85 | 86 | void CFootBotNNController::Reset() { 87 | m_cPerceptron.Reset(); 88 | } 89 | 90 | /****************************************/ 91 | /****************************************/ 92 | 93 | void CFootBotNNController::Destroy() { 94 | m_cPerceptron.Destroy(); 95 | } 96 | 97 | /****************************************/ 98 | /****************************************/ 99 | 100 | REGISTER_CONTROLLER(CFootBotNNController, "footbot_nn_controller") 101 | -------------------------------------------------------------------------------- /server/inc/ADG.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include "parser.h" 14 | 15 | typedef std::vector, std::pair>> SIM_PLAN; 16 | 17 | struct Edge { 18 | int from_agent_id; 19 | int to_agent_id; 20 | int from_node_id; 21 | int to_node_id; 22 | bool valid = true; 23 | Edge(int from_agent, int to_agent, int from_node, int to_node): from_agent_id(from_agent), to_agent_id(to_agent), 24 | from_node_id(from_node), to_node_id(to_node) {} 25 | }; 26 | 27 | struct ADG_STATS { 28 | int type1EdgeCount = 0; 29 | int type2EdgeCount = 0; 30 | int moveActionCount = 0; 31 | int rotateActionCount = 0; 32 | int consecutiveMoveSequences = 0; 33 | int totalNodes = 0; 34 | std::set> conflict_pairs; 35 | }; 36 | 37 | struct ADGNode { 38 | Action action; 39 | int node_id; 40 | std::vector> incomeEdges; 41 | std::vector> outEdges; 42 | bool has_valid_in_edge = true; 43 | }; 44 | 45 | class ADG { 46 | public: 47 | ADG(const std::vector>& plans); 48 | [[nodiscard]] int numRobots() const { 49 | return num_robots; 50 | } 51 | 52 | [[nodiscard]] int numNodes() const { 53 | return total_nodes_cnt; 54 | } 55 | 56 | std::pair, std::map> createRobotIDToStartIndexMaps(); 57 | bool getAvailableNodes(int robot_id, std::vector& available_nodes); 58 | bool updateFinishedNode(int robot_id, int node_id); 59 | void setEnqueueNodes(int robot_id, std::vector& enqueue_nodes); 60 | SIM_PLAN getPlan(int agent_id); 61 | bool isAgentFinished(int robot_id) { 62 | return finished_node_idx[robot_id] >= graph[robot_id].size()-1; 63 | } 64 | 65 | void printProgress() 66 | { 67 | for (int agent_id = 0; agent_id < num_robots; agent_id++) { 68 | std::cout << "Agent " << agent_id << ", ID: " << robotIDToStartIndex[agent_id] << " with plan size " << graph[agent_id].size() << ": "; 69 | findConstraining(agent_id); 70 | for (int i = 0; i <= finished_node_idx[agent_id]; i++) { 71 | std::cout << "#"; 72 | } 73 | for (auto elem: enqueue_nodes_idx[agent_id]) { 74 | std::cout << '0'; 75 | } 76 | int unstart; 77 | if (enqueue_nodes_idx[agent_id].empty()) { 78 | unstart = finished_node_idx[agent_id]; 79 | } else { 80 | unstart = enqueue_nodes_idx[agent_id].back(); 81 | } 82 | for (int i = unstart+1; i < graph[agent_id].size(); i++) { 83 | std::cout << "*"; 84 | } 85 | std::cout << std::endl; 86 | } 87 | std::cout << std::endl; 88 | } 89 | 90 | private: 91 | void printActions(const std::vector, std::pair>>& actions); 92 | void findConstraining(int robot_id); 93 | 94 | public: 95 | std::vector< int > finished_node_idx; 96 | std::vector< std::deque > enqueue_nodes_idx; 97 | ADG_STATS adg_stats; 98 | 99 | private: 100 | std::vector> graph; 101 | int num_robots = 0; 102 | int total_nodes_cnt = 0; 103 | std::map robotIDToStartIndex; 104 | std::map startIndexToRobotID; 105 | }; 106 | -------------------------------------------------------------------------------- /run_sim.py: -------------------------------------------------------------------------------- 1 | import os 2 | import argparse 3 | import ArgosConfig 4 | import subprocess 5 | import time 6 | 7 | def parse_arguments(): 8 | parser = argparse.ArgumentParser(description="Argument parser for map_name and scen_name.") 9 | parser.add_argument("--map_name", type=str, required=True, help="Name of the map file") 10 | parser.add_argument("--scen_name", type=str, required=True, help="Name of the scenario file") 11 | parser.add_argument("--num_agents", type=int, required=True, help="Number of agents in the scenario") 12 | parser.add_argument("--headless", type=bool, required=False, default=False, help="Simulator run in headless mode") 13 | parser.add_argument("--argos_config_name", type=str, required=False, default="output.argos", help="Name of the argos config file") 14 | parser.add_argument("--path_filename", type=str, required=False, default="example_paths.txt", help="Name of the output path file") 15 | parser.add_argument("--stats_name", type=str, required=False, default="stats.csv", help="Name of the statistics file for simulator") 16 | parser.add_argument("--port_num", type=int, required=False, default=8182, help="Port number for sim and client") 17 | parser.add_argument("--flip_coord", type=int, required=False, default=True, help="input format of the mapf planner, 0 if xy, 1 if yx") 18 | 19 | return parser.parse_args() 20 | 21 | 22 | def run_planner(command: list): 23 | 24 | try: 25 | result = subprocess.run(command, capture_output=True, text=True, check=True) 26 | 27 | if result.stderr: 28 | print("Errors:") 29 | print(result.stderr) 30 | return 0 31 | 32 | if "Succeed" in result.stdout: 33 | return 1 34 | else: 35 | if "Fail" in result.stdout or "Timeout" in result.stdout: 36 | return 0 37 | else: 38 | return 1 39 | 40 | except subprocess.CalledProcessError as e: 41 | print(f"Execution failed: {e}") 42 | print(f"Error output: {e.stderr}") 43 | return 0 44 | 45 | def check_file(file_path:str): 46 | if not os.path.exists(file_path): 47 | print(f"{file_path} not exists!") 48 | return False 49 | return True 50 | 51 | 52 | def run_experiment(args): 53 | server_command, client_command = args 54 | server_process = subprocess.Popen(server_command) 55 | 56 | time.sleep(1) 57 | # os.chdir("client") 58 | 59 | client_process = subprocess.Popen(client_command) 60 | 61 | client_process.wait() 62 | # os.chdir("..") 63 | server_process.wait() 64 | 65 | if __name__ == "__main__": 66 | args = parse_arguments() 67 | print(f"Map Name: {args.map_name}") 68 | print(f"Scenario Name: {args.scen_name}") 69 | 70 | 71 | scen_file_path = args.scen_name 72 | map_file_path = args.map_name 73 | config_filename = args.argos_config_name 74 | curr_num_agent = args.num_agents 75 | port_num=args.port_num 76 | path_filename = args.path_filename 77 | sim_stats_filename = args.stats_name 78 | 79 | if not check_file(map_file_path): 80 | print("Map file not exists!") 81 | exit(-1) 82 | if not check_file(scen_file_path): 83 | print("Scenario file not exists!") 84 | exit(-1) 85 | 86 | print("Creating Argos config file ...") 87 | robot_init_pos, scen_num_agent = ArgosConfig.read_scen(scen_file_path) 88 | map_data, width, height = ArgosConfig.parse_map_file(map_file_path) 89 | if scen_num_agent < curr_num_agent: 90 | print("Number of agents exceed maximum number. exiting ...") 91 | exit(-1) 92 | ArgosConfig.create_Argos(map_data, config_filename, width, height, robot_init_pos, curr_num_agent, port_num, not args.headless) 93 | print("Argos config file created.") 94 | 95 | print("Running simulator ...") 96 | server_executable_path = "build/server/ADG_server" 97 | server_command = [server_executable_path, "-p", path_filename, "-n", str(port_num), "-o", 98 | sim_stats_filename, "-m", map_file_path, "-s", str(scen_file_path), f"--method_name=LNS2", f"--flip_coord={args.flip_coord}"] 99 | print(server_command) 100 | client_command = ["argos3", "-c", f"./{config_filename}"] 101 | print(client_command) 102 | # exit(0) 103 | run_experiment((server_command, client_command)) -------------------------------------------------------------------------------- /client/controllers/footbot_nn/nn/perceptron.cpp: -------------------------------------------------------------------------------- 1 | #include "perceptron.h" 2 | 3 | #include 4 | #include 5 | 6 | /****************************************/ 7 | /****************************************/ 8 | 9 | CPerceptron::CPerceptron() : 10 | m_unNumberOfWeights(0), 11 | m_pfWeights(NULL) {} 12 | 13 | /****************************************/ 14 | /****************************************/ 15 | 16 | CPerceptron::~CPerceptron() { 17 | if(m_pfWeights) delete[] m_pfWeights; 18 | } 19 | 20 | /****************************************/ 21 | /****************************************/ 22 | 23 | void CPerceptron::Init(TConfigurationNode& t_tree) { 24 | /* First perform common initialisation from base class */ 25 | CNeuralNetwork::Init(t_tree); 26 | 27 | if( m_strParameterFile != "" ) { 28 | try{ 29 | LoadNetworkParameters(m_strParameterFile); 30 | } 31 | catch(CARGoSException& ex) { 32 | THROW_ARGOSEXCEPTION_NESTED("cannot load parameters from file.", ex); 33 | } 34 | } 35 | } 36 | 37 | /****************************************/ 38 | /****************************************/ 39 | 40 | void CPerceptron::Destroy() { 41 | if( m_pfWeights ) delete[] m_pfWeights; 42 | m_pfWeights = NULL; 43 | m_unNumberOfWeights = 0; 44 | } 45 | 46 | /****************************************/ 47 | /****************************************/ 48 | 49 | void CPerceptron::LoadNetworkParameters(const std::string& str_filename) { 50 | 51 | // open the input file 52 | std::ifstream cIn(str_filename.c_str(), std::ios::in); 53 | if( !cIn ) { 54 | THROW_ARGOSEXCEPTION("Cannot open parameter file '" << str_filename << "' for reading"); 55 | } 56 | 57 | // first parameter is the number of real-valued weights 58 | UInt32 un_length = 0; 59 | if( !(cIn >> un_length) ) { 60 | THROW_ARGOSEXCEPTION("Cannot read data from file '" << str_filename << "'"); 61 | } 62 | 63 | // check consistency between paramter file and xml declaration 64 | m_unNumberOfWeights = (m_unNumberOfInputs + 1) * m_unNumberOfOutputs; 65 | if( un_length != m_unNumberOfWeights ) { 66 | THROW_ARGOSEXCEPTION("Number of parameter mismatch: '" 67 | << str_filename 68 | << "' contains " 69 | << un_length 70 | << " parameters, while " 71 | << m_unNumberOfWeights 72 | << " were expected from the XML configuration file"); 73 | } 74 | 75 | // create weights vector and load it from file 76 | if(m_pfWeights == NULL) m_pfWeights = new Real[m_unNumberOfWeights]; 77 | for(size_t i = 0; i < m_unNumberOfWeights; ++i) { 78 | if( !(cIn >> m_pfWeights[i] ) ) { 79 | THROW_ARGOSEXCEPTION("Cannot read data from file '" << str_filename << "'"); 80 | } 81 | } 82 | } 83 | 84 | /****************************************/ 85 | /****************************************/ 86 | 87 | void CPerceptron::LoadNetworkParameters(const UInt32 un_num_params, 88 | const Real* pf_params) { 89 | // check consistency between parameters and xml declaration 90 | m_unNumberOfWeights = (m_unNumberOfInputs + 1) * m_unNumberOfOutputs; 91 | if(un_num_params != m_unNumberOfWeights) { 92 | THROW_ARGOSEXCEPTION("Number of parameter mismatch: '" 93 | << "passed " 94 | << un_num_params 95 | << " parameters, while " 96 | << m_unNumberOfWeights 97 | << " were expected from the XML configuration file"); 98 | } 99 | 100 | // create weights vector and load it from file 101 | if(m_pfWeights == NULL) m_pfWeights = new Real[m_unNumberOfWeights]; 102 | for(size_t i = 0; i < m_unNumberOfWeights; ++i ) { 103 | m_pfWeights[i] = pf_params[i]; 104 | } 105 | } 106 | 107 | /****************************************/ 108 | /****************************************/ 109 | 110 | void CPerceptron::ComputeOutputs() { 111 | for(size_t i = 0; i < m_unNumberOfOutputs; ++i) { 112 | // Add the bias (weighted by the first weight to the i'th output node) 113 | m_pfOutputs[i] = m_pfWeights[i * (m_unNumberOfInputs + 1)]; 114 | 115 | for(size_t j = 0; j < m_unNumberOfInputs; ++j) { 116 | // Compute the weight number 117 | size_t ji = i * (m_unNumberOfInputs + 1) + (j + 1); 118 | // Add the weighted input 119 | m_pfOutputs[i] += m_pfWeights[ji] * m_pfInputs[j]; 120 | 121 | } 122 | 123 | // Apply the transfer function (sigmoid with output in [0,1]) 124 | m_pfOutputs[i] = 1.0f / ( 1.0f + ::exp( -m_pfOutputs[i]) ); 125 | } 126 | } 127 | 128 | /****************************************/ 129 | /****************************************/ 130 | -------------------------------------------------------------------------------- /client/controllers/footbot_diffusion/footbot_diffusion.h: -------------------------------------------------------------------------------- 1 | #ifndef FOOTBOT_DIFFUSION_H 2 | #define FOOTBOT_DIFFUSION_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | using namespace argos; 29 | using outputTuple = std::tuple, std::pair>; 30 | 31 | #define EPS 0.02f 32 | #define MOVE_DIS 0.5f 33 | 34 | struct Action { 35 | Real x; 36 | Real y; 37 | Real angle; 38 | std::deque nodeIDS; 39 | enum Type { 40 | MOVE, 41 | TURN, 42 | STOP 43 | } type; 44 | }; 45 | 46 | struct Pos { 47 | Real x; 48 | Real y; 49 | 50 | bool operator<(const Pos &other) const { 51 | if (x != other.x) return x < other.x; 52 | return y < other.y; 53 | } 54 | }; 55 | 56 | class PIDController { 57 | public: 58 | PIDController(Real kp, Real ki, Real kd) 59 | : kp_(kp), ki_(ki), kd_(kd), prevError_(0), integral_(0) {} 60 | 61 | Real calculate(Real error) { 62 | integral_ += error * dt; 63 | Real derivative = (error - prevError_) / dt; 64 | prevError_ = error; 65 | 66 | return kp_ * error + ki_ * integral_ + kd_ * derivative; 67 | } 68 | 69 | private: 70 | Real kp_, ki_, kd_; 71 | Real prevError_; 72 | Real integral_; 73 | Real dt = 0.1; 74 | }; 75 | 76 | class CFootBotDiffusion : public CCI_Controller { 77 | public: 78 | CFootBotDiffusion(); 79 | virtual ~CFootBotDiffusion() {} 80 | virtual void Init(TConfigurationNode &t_node); 81 | virtual void ControlStep(); 82 | virtual void Reset() {} 83 | virtual void Destroy() {} 84 | int getRobotID() const { return agent_idx_in_server; } 85 | 86 | private: 87 | CCI_DifferentialSteeringActuator *m_pcWheels; 88 | CCI_FootBotProximitySensor *m_pcProximity; 89 | CCI_PositioningSensor *m_pcPosSens; 90 | int m_tickPerSec = 10; 91 | std::string robot_id; 92 | 93 | CDegrees m_cAlpha; 94 | int port_number; 95 | Real m_rotateWheelVelocity; 96 | Real m_angularVelocity; 97 | Real m_linearAcceleration; 98 | Real m_currVelocity; 99 | Real m_fWheelVelocity; 100 | CRange m_cGoStraightAngleRange; 101 | std::string m_outputDir; 102 | 103 | Real pidLinear(Real error); 104 | std::pair Move(CVector3& targetPos, CVector3& currPos, Real currAngle, Real tolerance); 105 | void MoveForward(Real x, Real y, Real tolerance); 106 | std::pair inline pidAngular(Real error); 107 | std::pair Turn(Real targetAngle, Real currAngle, Real tolerance); 108 | void CalculateVelocity(Real x, Real y); 109 | Real ChangeCoordinateFromMapToArgos(Real x); 110 | Real ChangeCoordinateFromArgosToMap(Real x); 111 | void insertActions(std::vector actions); 112 | double getReferenceSpeed(double dist); 113 | void updateQueue(); 114 | void setWheels(Real left_v, Real right_v) { 115 | m_pcWheels->SetLinearVelocity(left_v*100.0, right_v*100.0); // Convert to cm/s 116 | } 117 | 118 | std::deque q; 119 | std::queue velocityQueue; 120 | int moveForwardFlag; 121 | int count = 0; 122 | Real offset = 15.0; 123 | std::shared_ptr client; 124 | 125 | std::shared_ptr angular_pid_ptr; 126 | std::shared_ptr linear_pid_ptr; 127 | 128 | Real dt = 0.1; 129 | Real prev_turn_error=0.0; 130 | Real integral_turn_error=0.0; 131 | Real kp_turn_ = 0.008; 132 | Real ki_turn_ = 0.0; 133 | Real kd_turn_ = 0.001; 134 | 135 | Real prevLeftVelocity_ = 0.0; 136 | Real prevRightVelocity_ = 0.0; 137 | Real prevVelocity_ = 0.0; 138 | Real prev_move_error=0.0; 139 | Real integral_move_error=0.0; 140 | Real kp_move_ = 1.0; 141 | Real ki_move_ = 0.0; 142 | Real kd_move_ = 0.1; 143 | std::string debug_id = "-1"; 144 | int agent_idx_in_server = -1; 145 | 146 | 147 | int lineExistFlag = 0; 148 | bool terminateFlag = false; 149 | 150 | // std::ofstream outputFile; 151 | // std::string outputDir; 152 | 153 | bool is_initialized = false; 154 | }; 155 | 156 | #endif 157 | -------------------------------------------------------------------------------- /client/loop_functions/mpga_loop_functions/mpga.h: -------------------------------------------------------------------------------- 1 | #ifndef MPGA_H 2 | #define MPGA_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | using namespace argos; 10 | class CMPGA { 11 | 12 | public: 13 | 14 | /** 15 | * Information about an individual. 16 | */ 17 | struct SIndividual { 18 | std::vector Genome; 19 | Real Score; 20 | }; 21 | 22 | /** A population is a list of parameter sets */ 23 | typedef std::vector TPopulation; 24 | 25 | /** Score aggregator function pointer */ 26 | typedef Real (*TScoreAggregator)(const std::vector&); 27 | 28 | public: 29 | 30 | /** 31 | * Class constructor. 32 | * @param c_allele_range The range of each allele of the genome. 33 | * @param un_genome_size The size of the genome of an individual. 34 | * @param un_pop_size The size of the population. 35 | * @param f_mutation_prob The mutation probability. 36 | * @param un_num_trials The number of trials per individual. 37 | * @param un_generations The max number of generations. 38 | * @param b_maximize true to maximize, false to minimize. 39 | * @param str_argosconf Path of the .argos file to use. 40 | * @param t_score_aggregator The function used to aggregate the trial scores. 41 | * @param un_random_seed The random seed. 42 | */ 43 | CMPGA(const CRange& c_allele_range, 44 | UInt32 un_genome_size, 45 | UInt32 un_pop_size, 46 | Real f_mutation_prob, 47 | UInt32 un_num_trials, 48 | UInt32 un_generations, 49 | bool b_maximize, 50 | const std::string& str_argosconf, 51 | TScoreAggregator t_score_aggregator, 52 | UInt32 un_random_seed); 53 | 54 | /** 55 | * Class destructor. 56 | */ 57 | ~CMPGA(); 58 | 59 | /** 60 | * Returns the current population. 61 | */ 62 | const TPopulation& GetPopulation() const; 63 | 64 | /** 65 | * Returns the current generation. 66 | */ 67 | UInt32 GetGeneration() const; 68 | 69 | /** 70 | * Tidies up memory and close files. 71 | * Used internally, don't call it from user code. 72 | */ 73 | virtual void Cleanup(); 74 | 75 | /** 76 | * Runs the trials to evaluate the current population. 77 | */ 78 | virtual void Evaluate(); 79 | 80 | /** 81 | * Executes the next generation. 82 | */ 83 | virtual void NextGen(); 84 | 85 | /** 86 | * Returns true if the evolution is finished, false otherwise. 87 | */ 88 | virtual bool Done() const; 89 | 90 | private: 91 | 92 | /** Executes the slave process that manages ARGoS */ 93 | virtual void LaunchARGoS(UInt32 un_slave_id); 94 | 95 | /** 96 | * Discards all the individuals apart from the top two. 97 | */ 98 | virtual void Selection(); 99 | 100 | /** 101 | * Performs crossover among the best individuals. 102 | */ 103 | virtual void Crossover(); 104 | 105 | /** 106 | * Performs mutation 107 | */ 108 | virtual void Mutation(); 109 | 110 | private: 111 | 112 | /** Shared memory manager for data exchange between master and slaves */ 113 | class CSharedMem { 114 | 115 | public: 116 | 117 | /** 118 | * Class constructor. 119 | * @param un_genome_size The size of the genome of an individual. 120 | * @param un_pop_size The size of the population. 121 | */ 122 | CSharedMem(UInt32 un_genome_size, 123 | UInt32 un_pop_size); 124 | 125 | /** 126 | * Class destructor. 127 | */ 128 | ~CSharedMem(); 129 | 130 | /** 131 | * Returns the genome of an individual. 132 | * @param un_individual The individual. 133 | */ 134 | Real* GetGenome(UInt32 un_individual); 135 | 136 | /** 137 | * Sets the genome of an individual. 138 | * @param un_individual The individual. 139 | * @param pf_genome The genome. 140 | */ 141 | void SetGenome(UInt32 un_individual, 142 | const Real* pf_genome); 143 | 144 | /** 145 | * Returns the score of an individual. 146 | * @param un_individual The individual. 147 | */ 148 | Real GetScore(UInt32 un_individual); 149 | 150 | /** 151 | * Sets the score of an individual. 152 | * @param un_individual The individual. 153 | * @param f_score The score. 154 | */ 155 | void SetScore(UInt32 un_individual, 156 | Real f_score); 157 | 158 | private: 159 | 160 | /** Genome size */ 161 | UInt32 m_unGenomeSize; 162 | 163 | /** Population size */ 164 | UInt32 m_unPopSize; 165 | 166 | /** File descriptor for shared memory area */ 167 | int m_nSharedMemFD; 168 | 169 | /** Pointer to the shared memory area */ 170 | Real* m_pfSharedMem; 171 | 172 | }; 173 | 174 | protected: 175 | 176 | /** Current population */ 177 | TPopulation m_tPopulation; 178 | 179 | /** Current generation */ 180 | UInt32 m_unCurrentGeneration; 181 | 182 | /** The range of each allele in the genome */ 183 | CRange m_cAlleleRange; 184 | 185 | /** Genome size */ 186 | UInt32 m_unGenomeSize; 187 | 188 | /** Population size */ 189 | UInt32 m_unPopSize; 190 | 191 | /** Mutation probability */ 192 | Real m_fMutationProb; 193 | 194 | /** Number of trials per individual */ 195 | UInt32 m_unNumTrials; 196 | 197 | /** Number of generations */ 198 | UInt32 m_unGenerations; 199 | 200 | /** Path to the .argos file to use */ 201 | std::string m_strARGoSConf; 202 | 203 | /** The function used to aggregate the scores */ 204 | TScoreAggregator m_tScoreAggregator; 205 | 206 | /** PID of the master process */ 207 | pid_t MasterPID; 208 | 209 | /** PIDs of the slave processes */ 210 | std::vector SlavePIDs; 211 | 212 | /** The shared memory manager */ 213 | CSharedMem* m_pcSharedMem; 214 | 215 | /** Random number generator */ 216 | CRandom::CRNG* m_pcRNG; 217 | 218 | /** Comparison function to sort the population */ 219 | bool (*m_cIndComparator)(const SIndividual*, 220 | const SIndividual*); 221 | }; 222 | 223 | #endif 224 | -------------------------------------------------------------------------------- /client/experiments/diffusion_1.argos: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 16 | 17 | 26 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 43 | 44 | 52 | 54 | 73 | 74 | 75 | 76 | 84 | 85 | 86 | 87 | 88 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 143 | 144 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 161 | 162 | 163 | 164 | 165 | 166 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | -------------------------------------------------------------------------------- /client/loop_functions/mpga_loop_functions/mpga.cpp: -------------------------------------------------------------------------------- 1 | #include "mpga.h" 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include "mpga_loop_functions.h" 13 | 14 | static const std::string SHARED_MEMORY_FILE = "/MPGA_SHARED_MEMORY_" + ToString(getpid()); 15 | 16 | bool SortHighToLow(const CMPGA::SIndividual* pc_a, 17 | const CMPGA::SIndividual* pc_b) { 18 | return pc_a->Score > pc_b->Score; 19 | } 20 | 21 | bool SortLowToHigh(const CMPGA::SIndividual* pc_a, 22 | const CMPGA::SIndividual* pc_b) { 23 | return pc_b->Score > pc_a->Score; 24 | } 25 | 26 | CMPGA::CMPGA(const CRange& c_allele_range, 27 | UInt32 un_genome_size, 28 | UInt32 un_pop_size, 29 | Real f_mutation_prob, 30 | UInt32 un_num_trials, 31 | UInt32 un_generations, 32 | bool b_maximize, 33 | const std::string& str_argosconf, 34 | TScoreAggregator t_score_aggregator, 35 | UInt32 un_random_seed) : 36 | m_unCurrentGeneration(0), 37 | m_cAlleleRange(c_allele_range), 38 | m_unGenomeSize(un_genome_size), 39 | m_unPopSize(un_pop_size), 40 | m_fMutationProb(f_mutation_prob), 41 | m_unNumTrials(un_num_trials), 42 | m_unGenerations(un_generations), 43 | m_strARGoSConf(str_argosconf), 44 | m_tScoreAggregator(t_score_aggregator), 45 | MasterPID(::getpid()), 46 | m_cIndComparator(b_maximize ? SortHighToLow : SortLowToHigh) { 47 | m_pcSharedMem = new CSharedMem(un_genome_size, 48 | un_pop_size); 49 | for(UInt32 i = 0; i < m_unPopSize; ++i) { 50 | SlavePIDs.push_back(::fork()); 51 | if(SlavePIDs.back() == 0) { 52 | LaunchARGoS(i); 53 | } 54 | } 55 | CRandom::CreateCategory("ga", un_random_seed); 56 | m_pcRNG = CRandom::CreateRNG("ga"); 57 | SIndividual* psInd; 58 | for(size_t p = 0; p < m_unPopSize; ++p) { 59 | psInd = new SIndividual; 60 | psInd->Score = -1.0; 61 | for(size_t g = 0; g < m_unGenomeSize; ++g) { 62 | psInd->Genome.push_back(m_pcRNG->Uniform(m_cAlleleRange)); 63 | } 64 | m_tPopulation.push_back(psInd); 65 | } 66 | ::sleep(3); 67 | } 68 | 69 | CMPGA::~CMPGA() { 70 | for(UInt32 i = 0; i < m_unPopSize; ++i) { 71 | ::kill(SlavePIDs[i], SIGTERM); 72 | } 73 | while(!m_tPopulation.empty()) { 74 | delete m_tPopulation.back(); 75 | m_tPopulation.pop_back(); 76 | } 77 | CRandom::RemoveCategory("ga"); 78 | Cleanup(); 79 | } 80 | 81 | const CMPGA::TPopulation& CMPGA::GetPopulation() const { 82 | return m_tPopulation; 83 | } 84 | 85 | UInt32 CMPGA::GetGeneration() const { 86 | return m_unCurrentGeneration; 87 | } 88 | 89 | void CMPGA::Cleanup() { 90 | delete m_pcSharedMem; 91 | } 92 | 93 | void CMPGA::Evaluate() { 94 | for(UInt32 i = 0; i < m_unPopSize; ++i) { 95 | m_pcSharedMem->SetGenome(i, &(m_tPopulation[i]->Genome[0])); 96 | ::kill(SlavePIDs[i], SIGCONT); 97 | } 98 | UInt32 unTrialsLeft = m_unPopSize; 99 | int nSlaveInfo; 100 | pid_t tSlavePID; 101 | while(unTrialsLeft > 0) { 102 | tSlavePID = ::waitpid(-1, &nSlaveInfo, WUNTRACED); 103 | if(!WIFSTOPPED(nSlaveInfo)) { 104 | LOGERR << "[FATAL] Slave process with PID " << tSlavePID << " exited, can't continue. Check file ARGoS_LOGERR_" << tSlavePID << " for more information." << std::endl; 105 | LOG.Flush(); 106 | LOGERR.Flush(); 107 | Cleanup(); 108 | ::exit(1); 109 | } 110 | --unTrialsLeft; 111 | } 112 | for(UInt32 i = 0; i < m_unPopSize; ++i) { 113 | m_tPopulation[i]->Score = m_pcSharedMem->GetScore(i); 114 | } 115 | std::sort(m_tPopulation.begin(), 116 | m_tPopulation.end(), 117 | m_cIndComparator); 118 | } 119 | 120 | void CMPGA::NextGen() { 121 | ++m_unCurrentGeneration; 122 | Selection(); 123 | Crossover(); 124 | Mutation(); 125 | } 126 | 127 | bool CMPGA::Done() const { 128 | return m_unCurrentGeneration >= m_unGenerations; 129 | } 130 | 131 | static CMPGA* GA_INSTANCE; 132 | 133 | void SlaveHandleSIGTERM(int) { 134 | argos::CSimulator::GetInstance().Destroy(); 135 | argos::LOG.Flush(); 136 | argos::LOGERR.Flush(); 137 | GA_INSTANCE->Cleanup(); 138 | } 139 | 140 | void CMPGA::LaunchARGoS(UInt32 un_slave_id) { 141 | GA_INSTANCE = this; 142 | ::signal(SIGTERM, SlaveHandleSIGTERM); 143 | argos::CSimulator& cSimulator = argos::CSimulator::GetInstance(); 144 | try { 145 | cSimulator.SetExperimentFileName(m_strARGoSConf); 146 | cSimulator.LoadExperiment(); 147 | LOG.Flush(); 148 | LOGERR.Flush(); 149 | } 150 | catch(CARGoSException& ex) { 151 | LOGERR << ex.what() << std::endl; 152 | ::raise(SIGTERM); 153 | } 154 | CMPGALoopFunctions& cLoopFunctions = dynamic_cast(cSimulator.GetLoopFunctions()); 155 | std::vector vecScores(m_unNumTrials, 0.0); 156 | while(1) { 157 | ::raise(SIGTSTP); 158 | cLoopFunctions.ConfigureFromGenome(m_pcSharedMem->GetGenome(un_slave_id)); 159 | for(size_t i = 0; i < m_unNumTrials; ++i) { 160 | cLoopFunctions.SetTrial(i); 161 | cSimulator.Reset(); 162 | cSimulator.Execute(); 163 | vecScores[i] = cLoopFunctions.Score(); 164 | LOG.Flush(); 165 | LOGERR.Flush(); 166 | } 167 | ; 168 | m_pcSharedMem->SetScore(un_slave_id, m_tScoreAggregator(vecScores)); 169 | } 170 | } 171 | 172 | void CMPGA::Selection() { 173 | while(m_tPopulation.size() > 2) { 174 | delete m_tPopulation.back(); 175 | m_tPopulation.pop_back(); 176 | } 177 | } 178 | 179 | void CMPGA::Crossover() { 180 | SIndividual* psParent1 = m_tPopulation[0]; 181 | SIndividual* psParent2 = m_tPopulation[1]; 182 | UInt32 unCut; 183 | SIndividual* psInd; 184 | for(UInt32 i = 2; i < m_unPopSize; ++i) { 185 | unCut = m_pcRNG->Uniform(CRange(1, m_unGenomeSize-1)); 186 | psInd = new SIndividual; 187 | for(UInt32 j = 0; j < unCut; ++j) { 188 | psInd->Genome.push_back(psParent1->Genome[j]); 189 | } 190 | for(UInt32 j = unCut; j < m_unGenomeSize; ++j) { 191 | psInd->Genome.push_back(psParent2->Genome[j]); 192 | } 193 | m_tPopulation.push_back(psInd); 194 | } 195 | } 196 | 197 | void CMPGA::Mutation() { 198 | for(UInt32 i = 2; i < m_unPopSize; ++i) { 199 | for(UInt32 a = 0; a < m_unGenomeSize; ++a) { 200 | if(m_pcRNG->Bernoulli(m_fMutationProb)) 201 | m_tPopulation[i]->Genome[a] = m_pcRNG->Uniform(m_cAlleleRange); 202 | } 203 | } 204 | } 205 | 206 | CMPGA::CSharedMem::CSharedMem(UInt32 un_genome_size, 207 | UInt32 un_pop_size) : 208 | m_unGenomeSize(un_genome_size), 209 | m_unPopSize(un_pop_size) { 210 | m_nSharedMemFD = ::shm_open(SHARED_MEMORY_FILE.c_str(), 211 | O_RDWR | O_CREAT, 212 | S_IRUSR | S_IWUSR); 213 | if(m_nSharedMemFD < 0) { 214 | ::perror(SHARED_MEMORY_FILE.c_str()); 215 | exit(1); 216 | } 217 | size_t unShareMemSize = m_unPopSize * (m_unGenomeSize+1) * sizeof(Real); 218 | ::ftruncate(m_nSharedMemFD, unShareMemSize); 219 | m_pfSharedMem = reinterpret_cast( 220 | ::mmap(NULL, 221 | unShareMemSize, 222 | PROT_READ | PROT_WRITE, 223 | MAP_SHARED, 224 | m_nSharedMemFD, 225 | 0)); 226 | if(m_pfSharedMem == MAP_FAILED) { 227 | ::perror("shared memory"); 228 | exit(1); 229 | } 230 | } 231 | 232 | CMPGA::CSharedMem::~CSharedMem() { 233 | munmap(m_pfSharedMem, m_unPopSize * (m_unGenomeSize+1) * sizeof(Real)); 234 | close(m_nSharedMemFD); 235 | shm_unlink(SHARED_MEMORY_FILE.c_str()); 236 | } 237 | 238 | Real* CMPGA::CSharedMem::GetGenome(UInt32 un_individual) { 239 | return m_pfSharedMem + un_individual * (m_unGenomeSize+1); 240 | } 241 | 242 | void CMPGA::CSharedMem::SetGenome(UInt32 un_individual, 243 | const Real* pf_genome) { 244 | ::memcpy(m_pfSharedMem + un_individual * (m_unGenomeSize+1), 245 | pf_genome, 246 | m_unGenomeSize * sizeof(Real)); 247 | } 248 | 249 | Real CMPGA::CSharedMem::GetScore(UInt32 un_individual) { 250 | return m_pfSharedMem[un_individual * (m_unGenomeSize+1) + m_unGenomeSize]; 251 | } 252 | 253 | void CMPGA::CSharedMem::SetScore(UInt32 un_individual, 254 | Real f_score) { 255 | m_pfSharedMem[un_individual * (m_unGenomeSize+1) + m_unGenomeSize] = f_score; 256 | } 257 | -------------------------------------------------------------------------------- /client/embedding/galib/evolution.dat: -------------------------------------------------------------------------------- 1 | 0 6.88167 2 | 1 6.88061 3 | 2 6.67534 4 | 3 6.71241 5 | 4 6.71493 6 | 5 6.53047 7 | 6 5.74905 8 | 7 4.62814 9 | 8 4.62338 10 | 9 4.61507 11 | 10 4.61006 12 | 11 4.55304 13 | 12 4.6161 14 | 13 4.58444 15 | 14 4.58542 16 | 15 4.58658 17 | 16 4.57509 18 | 17 4.52327 19 | 18 4.5227 20 | 19 4.5647 21 | 20 4.52733 22 | 21 4.528 23 | 22 4.52818 24 | 23 4.49591 25 | 24 4.48673 26 | 25 4.40629 27 | 26 4.38174 28 | 27 4.38526 29 | 28 4.29715 30 | 29 4.26513 31 | 30 4.22813 32 | 31 4.2149 33 | 32 4.29285 34 | 33 4.37173 35 | 34 4.33609 36 | 35 4.33695 37 | 36 4.35261 38 | 37 4.52013 39 | 38 4.15985 40 | 39 4.13286 41 | 40 4.2523 42 | 41 4.18363 43 | 42 4.07261 44 | 43 4.2072 45 | 44 4.20578 46 | 45 3.93477 47 | 46 3.97515 48 | 47 3.89815 49 | 48 3.90245 50 | 49 3.87339 51 | 50 3.9397 52 | 51 3.99504 53 | 52 3.99504 54 | 53 4.00599 55 | 54 4.94796 56 | 55 3.68959 57 | 56 3.94311 58 | 57 3.5778 59 | 58 3.96784 60 | 59 3.58222 61 | 60 3.58392 62 | 61 3.8314 63 | 62 3.63586 64 | 63 3.58523 65 | 64 3.65317 66 | 65 3.63101 67 | 66 3.44871 68 | 67 3.46344 69 | 68 3.37709 70 | 69 3.42808 71 | 70 3.44123 72 | 71 3.44483 73 | 72 3.65485 74 | 73 3.41202 75 | 74 3.42332 76 | 75 3.47384 77 | 76 3.31832 78 | 77 3.5315 79 | 78 3.53266 80 | 79 3.33907 81 | 80 3.31119 82 | 81 3.34981 83 | 82 3.31113 84 | 83 3.15445 85 | 84 3.13914 86 | 85 3.11155 87 | 86 3.72 88 | 87 3.30948 89 | 88 3.22153 90 | 89 2.95329 91 | 90 2.90669 92 | 91 2.96108 93 | 92 2.98567 94 | 93 2.97936 95 | 94 3.06382 96 | 95 2.98599 97 | 96 5.38572 98 | 97 2.85601 99 | 98 2.53456 100 | 99 2.80207 101 | 100 2.58 102 | 101 2.64263 103 | 102 2.63271 104 | 103 2.72625 105 | 104 2.44706 106 | 105 2.46782 107 | 106 2.48618 108 | 107 2.50628 109 | 108 2.5095 110 | 109 2.33084 111 | 110 2.34545 112 | 111 2.33798 113 | 112 2.30441 114 | 113 2.2748 115 | 114 2.32007 116 | 115 2.28105 117 | 116 2.71365 118 | 117 2.72029 119 | 118 2.74704 120 | 119 2.36532 121 | 120 2.34208 122 | 121 2.22803 123 | 122 3.04624 124 | 123 2.25655 125 | 124 2.21748 126 | 125 2.17159 127 | 126 2.14136 128 | 127 2.78513 129 | 128 2.18579 130 | 129 2.09749 131 | 130 2.04548 132 | 131 2.03499 133 | 132 2.36942 134 | 133 2.22937 135 | 134 2.02472 136 | 135 2.00646 137 | 136 2.01141 138 | 137 1.94517 139 | 138 1.95481 140 | 139 1.96876 141 | 140 2.05356 142 | 141 2.04047 143 | 142 2.05517 144 | 143 2.13543 145 | 144 2.04435 146 | 145 1.85505 147 | 146 1.87518 148 | 147 1.88652 149 | 148 2.147 150 | 149 1.91323 151 | 150 1.91488 152 | 151 1.90606 153 | 152 1.87977 154 | 153 1.85965 155 | 154 1.68188 156 | 155 3.18762 157 | 156 3.74589 158 | 157 3.73151 159 | 158 1.75469 160 | 159 1.7596 161 | 160 1.69923 162 | 161 1.67437 163 | 162 1.67154 164 | 163 1.68702 165 | 164 1.55936 166 | 165 1.63906 167 | 166 1.92389 168 | 167 1.60581 169 | 168 1.69635 170 | 169 1.54077 171 | 170 1.53686 172 | 171 1.5809 173 | 172 1.53333 174 | 173 1.55263 175 | 174 1.53941 176 | 175 2.93058 177 | 176 1.57063 178 | 177 1.55464 179 | 178 1.44062 180 | 179 1.43343 181 | 180 1.96796 182 | 181 1.96692 183 | 182 1.93713 184 | 183 1.4865 185 | 184 1.74136 186 | 185 1.4506 187 | 186 1.37312 188 | 187 1.37282 189 | 188 1.30537 190 | 189 1.61896 191 | 190 1.38728 192 | 191 1.38446 193 | 192 1.34527 194 | 193 1.36628 195 | 194 1.40819 196 | 195 1.56714 197 | 196 1.55945 198 | 197 1.3821 199 | 198 1.34511 200 | 199 1.35653 201 | 200 1.65756 202 | 201 1.37723 203 | 202 1.47026 204 | 203 1.37728 205 | 204 1.37649 206 | 205 1.30236 207 | 206 1.30101 208 | 207 1.29248 209 | 208 1.36716 210 | 209 1.29528 211 | 210 1.067 212 | 211 1.03418 213 | 212 0.892113 214 | 213 0.921933 215 | 214 1.00108 216 | 215 1.05226 217 | 216 3.53834 218 | 217 1.63179 219 | 218 0.769788 220 | 219 0.813584 221 | 220 0.766263 222 | 221 0.93212 223 | 222 1.02495 224 | 223 0.963029 225 | 224 0.924756 226 | 225 0.826848 227 | 226 0.82725 228 | 227 0.843624 229 | 228 0.935476 230 | 229 0.935027 231 | 230 0.970554 232 | 231 0.988966 233 | 232 1.43834 234 | 233 1.29847 235 | 234 1.36468 236 | 235 0.998906 237 | 236 0.782678 238 | 237 0.796563 239 | 238 0.787145 240 | 239 1.63847 241 | 240 0.83091 242 | 241 0.818258 243 | 242 0.994398 244 | 243 0.986789 245 | 244 0.824492 246 | 245 1.1243 247 | 246 0.966838 248 | 247 0.968774 249 | 248 0.97894 250 | 249 0.987354 251 | 250 2.03945 252 | 251 0.875762 253 | 252 0.87574 254 | 253 0.800336 255 | 254 0.813177 256 | 255 0.821736 257 | 256 0.881332 258 | 257 0.869832 259 | 258 0.78197 260 | 259 0.77475 261 | 260 0.979166 262 | 261 0.793683 263 | 262 0.773741 264 | 263 0.745674 265 | 264 0.680852 266 | 265 0.749718 267 | 266 0.710519 268 | 267 1.27882 269 | 268 0.881297 270 | 269 0.944406 271 | 270 0.961055 272 | 271 0.699485 273 | 272 0.741895 274 | 273 0.745974 275 | 274 0.678235 276 | 275 0.786144 277 | 276 0.748435 278 | 277 1.13969 279 | 278 1.29269 280 | 279 1.44348 281 | 280 1.32224 282 | 281 0.851226 283 | 282 0.704127 284 | 283 0.804273 285 | 284 0.969592 286 | 285 0.857498 287 | 286 1.94498 288 | 287 0.742371 289 | 288 0.732411 290 | 289 1.18399 291 | 290 0.65929 292 | 291 0.659192 293 | 292 0.837239 294 | 293 0.837905 295 | 294 0.83791 296 | 295 0.558566 297 | 296 0.536491 298 | 297 0.737222 299 | 298 0.534733 300 | 299 0.580765 301 | 300 0.472797 302 | 301 0.488416 303 | 302 1.10981 304 | 303 0.511382 305 | 304 0.4848 306 | 305 0.483646 307 | 306 0.786924 308 | 307 0.785364 309 | 308 1.12923 310 | 309 0.788089 311 | 310 1.25938 312 | 311 1.29203 313 | 312 1.32041 314 | 313 1.15164 315 | 314 1.33344 316 | 315 0.842663 317 | 316 1.26018 318 | 317 0.712378 319 | 318 0.672408 320 | 319 1.07203 321 | 320 1.03038 322 | 321 1.16514 323 | 322 1.35304 324 | 323 1.39605 325 | 324 0.805287 326 | 325 0.531332 327 | 326 0.466029 328 | 327 0.631265 329 | 328 0.656021 330 | 329 0.979087 331 | 330 0.747026 332 | 331 0.734645 333 | 332 0.766371 334 | 333 0.455599 335 | 334 0.789165 336 | 335 0.626388 337 | 336 0.740249 338 | 337 0.58252 339 | 338 0.453242 340 | 339 0.751347 341 | 340 0.782708 342 | 341 0.434412 343 | 342 0.559635 344 | 343 0.449034 345 | 344 0.809524 346 | 345 0.811975 347 | 346 0.846535 348 | 347 0.594772 349 | 348 0.758355 350 | 349 0.722814 351 | 350 0.99065 352 | 351 0.964749 353 | 352 1.00803 354 | 353 1.02441 355 | 354 0.523688 356 | 355 0.506505 357 | 356 1.45205 358 | 357 0.839683 359 | 358 0.777161 360 | 359 0.779597 361 | 360 0.826419 362 | 361 0.781606 363 | 362 0.697971 364 | 363 0.794731 365 | 364 0.744828 366 | 365 0.741036 367 | 366 0.745534 368 | 367 0.747078 369 | 368 0.749663 370 | 369 0.448399 371 | 370 0.514359 372 | 371 0.509968 373 | 372 0.503599 374 | 373 0.513469 375 | 374 0.478572 376 | 375 0.777912 377 | 376 1.00375 378 | 377 0.452417 379 | 378 0.465837 380 | 379 0.474886 381 | 380 0.425365 382 | 381 0.45586 383 | 382 0.466756 384 | 383 0.760576 385 | 384 0.591262 386 | 385 0.504431 387 | 386 0.504512 388 | 387 0.470377 389 | 388 0.812223 390 | 389 0.347241 391 | 390 0.415803 392 | 391 0.33059 393 | 392 0.345967 394 | 393 0.368319 395 | 394 0.386949 396 | 395 0.685581 397 | 396 0.375548 398 | 397 0.483975 399 | 398 0.377725 400 | 399 0.378807 401 | 400 0.36505 402 | 401 0.394327 403 | 402 0.430512 404 | 403 0.346266 405 | 404 0.437822 406 | 405 0.459129 407 | 406 0.296615 408 | 407 0.29542 409 | 408 0.237523 410 | 409 0.235244 411 | 410 0.253013 412 | 411 0.279729 413 | 412 0.363223 414 | 413 0.270816 415 | 414 0.266861 416 | 415 0.2492 417 | 416 0.290446 418 | 417 1.12194 419 | 418 0.778115 420 | 419 0.243109 421 | 420 0.241015 422 | 421 0.726446 423 | 422 0.381635 424 | 423 0.959835 425 | 424 0.257941 426 | 425 0.243809 427 | 426 0.301874 428 | 427 0.239742 429 | 428 0.233662 430 | 429 1.83873 431 | 430 0.406 432 | 431 0.504837 433 | 432 0.436131 434 | 433 0.251769 435 | 434 0.25137 436 | 435 0.291598 437 | 436 0.208722 438 | 437 0.240263 439 | 438 0.291329 440 | 439 0.28455 441 | 440 0.271699 442 | 441 0.219677 443 | 442 0.519886 444 | 443 0.237723 445 | 444 0.190359 446 | 445 0.201382 447 | 446 0.218362 448 | 447 0.306775 449 | 448 0.225466 450 | 449 0.229252 451 | 450 0.255441 452 | 451 0.248492 453 | 452 0.218519 454 | 453 0.221766 455 | 454 0.212203 456 | 455 0.217074 457 | 456 0.217112 458 | 457 0.208344 459 | 458 0.208816 460 | 459 2.46075 461 | 460 0.283085 462 | 461 0.333615 463 | 462 0.304526 464 | 463 0.35655 465 | 464 0.342581 466 | 465 0.215799 467 | 466 0.33919 468 | 467 0.335754 469 | 468 0.27865 470 | 469 0.271074 471 | 470 0.465072 472 | 471 0.260991 473 | 472 0.298727 474 | 473 0.320668 475 | 474 0.328251 476 | 475 0.253034 477 | 476 0.266699 478 | 477 0.298981 479 | 478 0.206251 480 | 479 0.256476 481 | 480 0.309506 482 | 481 1.2336 483 | 482 0.229291 484 | 483 0.192059 485 | 484 0.231479 486 | 485 0.189727 487 | 486 0.26272 488 | 487 0.242303 489 | 488 0.242345 490 | 489 0.1905 491 | 490 0.190622 492 | 491 0.190214 493 | 492 0.190698 494 | 493 0.189976 495 | 494 0.190484 496 | 495 0.190875 497 | 496 0.285805 498 | 497 0.190831 499 | 498 0.305331 500 | 499 0.31997 501 | 500 0.218353 502 | -------------------------------------------------------------------------------- /server/src/ADG.cpp: -------------------------------------------------------------------------------- 1 | #include "ADG.h" 2 | 3 | ADG::ADG(const std::vector>& plans) { 4 | std::map lastActionIndexByRobot; 5 | num_robots = static_cast (plans.size()); 6 | finished_node_idx.resize(num_robots, -1); 7 | enqueue_nodes_idx.resize(num_robots); 8 | graph.resize(num_robots); 9 | for (int i = 0; i < num_robots; i++) { 10 | int j = 0; 11 | for (const auto& action : plans[i]) { 12 | ADGNode node {action, j, {}, {}}; 13 | graph[i].push_back(node); 14 | j++; 15 | total_nodes_cnt ++; 16 | } 17 | } 18 | std::vector> conflict_pair_table; 19 | conflict_pair_table.resize(plans.size()); 20 | for (auto& tmp_entry: conflict_pair_table) { 21 | tmp_entry.resize(plans.size(), false); 22 | } 23 | 24 | for (int i = 0; i < plans.size(); i++) { 25 | adg_stats.type1EdgeCount += static_cast(plans[i].size()) - 1; 26 | adg_stats.totalNodes += static_cast(plans[i].size()); 27 | bool consecutive_move = false; 28 | for (int j = 0; j < plans[i].size(); j++) { 29 | if (plans[i][j].type == 'M') { 30 | adg_stats.moveActionCount++; 31 | if (not consecutive_move) { 32 | consecutive_move = true; 33 | adg_stats.consecutiveMoveSequences++; 34 | } 35 | } else if (plans[i][j].type == 'T') { 36 | adg_stats.rotateActionCount++; 37 | consecutive_move = false; 38 | } else { 39 | std::cerr << "Invalid type!\n" << std::endl; 40 | consecutive_move = false; 41 | } 42 | for (int k = i+1; k < plans.size(); k++) { 43 | for (int l = 0; l < plans[k].size(); l++) { 44 | bool found_conflict = false; 45 | if (plans[i][j].start == plans[k][l].goal && plans[i][j].time <= plans[k][l].time) { 46 | std::shared_ptr tmp_edge = std::make_shared(i, k, j, l); 47 | graph[i][j].outEdges.push_back(tmp_edge); 48 | graph[k][l].incomeEdges.push_back(tmp_edge); 49 | found_conflict = true; 50 | } 51 | else if (plans[k][l].start == plans[i][j].goal && plans[k][l].time <= plans[i][j].time) { 52 | std::shared_ptr tmp_edge = std::make_shared(k, i, l, j); 53 | graph[i][j].incomeEdges.push_back(tmp_edge); 54 | graph[k][l].outEdges.push_back(tmp_edge); 55 | found_conflict = true; 56 | } 57 | if (found_conflict) { 58 | if (i==0 and k==2) { 59 | printf("Agents %d and %d collide at action [(%f, %f)->(%f, %f)] and action [(%f, %f)->(%f, %f)]\n", 60 | i, k, 61 | plans[i][j].start.first, plans[i][j].start.second, 62 | plans[i][j].goal.first, plans[i][j].goal.second, 63 | plans[k][l].start.first, plans[k][l].start.second, 64 | plans[k][l].goal.first, plans[k][l].goal.second); 65 | } 66 | adg_stats.type2EdgeCount++; 67 | if (not conflict_pair_table[i][k]) { 68 | conflict_pair_table[i][k] = true; 69 | conflict_pair_table[k][i] = true; 70 | adg_stats.conflict_pairs.emplace(i, k); 71 | } 72 | } 73 | } 74 | } 75 | } 76 | } 77 | } 78 | 79 | void printEdge() 80 | { 81 | } 82 | 83 | std::pair, std::map> ADG::createRobotIDToStartIndexMaps() { 84 | for (int robot_id = 0; robot_id < num_robots; robot_id++) { 85 | auto start = graph[robot_id][0].action.start; 86 | std::ostringstream oss; 87 | oss << static_cast(start.first) << "_" 88 | << static_cast(start.second); 89 | std::string startStr = oss.str(); 90 | 91 | robotIDToStartIndex[robot_id] = startStr; 92 | startIndexToRobotID[startStr] = robot_id; 93 | } 94 | 95 | return {robotIDToStartIndex, startIndexToRobotID}; 96 | } 97 | 98 | void inline updateADGNode(ADGNode& tmp_node) { 99 | bool valid = false; 100 | for (auto& tmp_in_edge: tmp_node.incomeEdges) { 101 | if (tmp_in_edge->valid) { 102 | valid = true; 103 | break; 104 | } 105 | } 106 | tmp_node.has_valid_in_edge = valid; 107 | } 108 | 109 | bool ADG::getAvailableNodes(int robot_id, std::vector& available_nodes) { 110 | auto& curr_agent_plan = graph[robot_id]; 111 | int latest_finished_idx = finished_node_idx[robot_id]; 112 | int next_node_idx = latest_finished_idx + 1; 113 | for (int i = next_node_idx; i < curr_agent_plan.size(); i++) { 114 | if (curr_agent_plan[i].has_valid_in_edge) { 115 | updateADGNode(curr_agent_plan[i]); 116 | } 117 | 118 | if (curr_agent_plan[i].has_valid_in_edge) { 119 | break; 120 | } 121 | 122 | if (enqueue_nodes_idx[robot_id].empty() or enqueue_nodes_idx[robot_id].back() < i) { 123 | available_nodes.push_back(i); 124 | } 125 | } 126 | return true; 127 | } 128 | 129 | bool ADG::updateFinishedNode(int robot_id, int node_id) { 130 | int latest_finished_idx = finished_node_idx[robot_id]; 131 | if (node_id <= latest_finished_idx) { 132 | std::cerr << "Reconfirming nodes!" << std::endl; 133 | return true; 134 | } else { 135 | if (not enqueue_nodes_idx[robot_id].empty() and node_id > enqueue_nodes_idx[robot_id].back()) { 136 | std::cerr << "Confirm for nodes never enqueue!" << std::endl; 137 | return false; 138 | } else { 139 | for (int tmp_idx = latest_finished_idx+1; tmp_idx <= node_id; tmp_idx++) { 140 | for (auto& tmp_edge: graph[robot_id][tmp_idx].incomeEdges) { 141 | assert(not tmp_edge->valid); 142 | } 143 | 144 | for (auto& tmp_edge: graph[robot_id][tmp_idx].outEdges) { 145 | tmp_edge->valid = false; 146 | } 147 | } 148 | finished_node_idx[robot_id] = node_id; 149 | while(not enqueue_nodes_idx[robot_id].empty()) { 150 | if (enqueue_nodes_idx[robot_id].front() <= node_id) { 151 | enqueue_nodes_idx[robot_id].pop_front(); 152 | } else { 153 | break; 154 | } 155 | } 156 | return true; 157 | } 158 | } 159 | } 160 | 161 | void ADG::setEnqueueNodes(int robot_id, std::vector& enqueue_nodes) { 162 | auto& curr_enqueue = enqueue_nodes_idx[robot_id]; 163 | if (curr_enqueue.empty()) { 164 | curr_enqueue.insert(curr_enqueue.end(), enqueue_nodes.begin(), enqueue_nodes.end()); 165 | } else { 166 | int i = 0; 167 | for (i = 0; i < enqueue_nodes.size(); i++) { 168 | if (enqueue_nodes[i] > curr_enqueue.back()) { 169 | break; 170 | } 171 | } 172 | curr_enqueue.insert(curr_enqueue.end(), enqueue_nodes.begin()+i, enqueue_nodes.end()); 173 | } 174 | } 175 | 176 | void ADG::findConstraining(int robot_id) { 177 | auto& curr_agent_plan = graph[robot_id]; 178 | int latest_finished_idx = finished_node_idx[robot_id]; 179 | int next_node_idx = latest_finished_idx + 1; 180 | for (int i = next_node_idx; i < curr_agent_plan.size(); i++) { 181 | if (curr_agent_plan[i].has_valid_in_edge) { 182 | updateADGNode(curr_agent_plan[i]); 183 | } 184 | 185 | if (curr_agent_plan[i].has_valid_in_edge) { 186 | std::cout << "Constraining idx: " << i << ";"; 187 | for (auto tmp_edge: curr_agent_plan[i].incomeEdges) { 188 | if (tmp_edge->valid) { 189 | std::cout << " Constraint Agent " << tmp_edge->from_agent_id << " at node " << tmp_edge->from_node_id << ";"; 190 | } 191 | } 192 | break; 193 | } 194 | } 195 | } 196 | 197 | void ADG::printActions(const std::vector, std::pair>>& actions) { 198 | for (const auto& action : actions) { 199 | std::string robot_id = std::get<0>(action); 200 | int time = std::get<1>(action); 201 | double orientation = std::get<2>(action); 202 | std::string type = std::get<3>(action); 203 | auto start = std::get<4>(action); 204 | auto goal = std::get<5>(action); 205 | 206 | std::cout << "Robot ID: " << robot_id 207 | << ", nodeID: " << time 208 | << ", Orientation: " << orientation 209 | << ", Type: " << type 210 | << ", Start: (" << start.first << ", " << start.second << ")" 211 | << ", Goal: (" << goal.first << ", " << goal.second << ")" 212 | << std::endl; 213 | } 214 | } 215 | 216 | SIM_PLAN ADG::getPlan(int agent_id) { 217 | SIM_PLAN sim_plan; 218 | std::vector enque_acts; 219 | getAvailableNodes(agent_id, enque_acts); 220 | for (int enque_id: enque_acts) { 221 | const Action& action = graph[agent_id][enque_id].action; 222 | sim_plan.emplace_back(robotIDToStartIndex[action.robot_id], enque_id, action.orientation, std::string(1, action.type), action.start, action.goal); 223 | enqueue_nodes_idx[agent_id].push_back(enque_id); 224 | } 225 | return sim_plan; 226 | } 227 | -------------------------------------------------------------------------------- /warehouse-10-20-10-2-1.map: -------------------------------------------------------------------------------- 1 | type octile 2 | height 63 3 | width 161 4 | map 5 | TTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTT 6 | T...............................................................................................................................................................T 7 | T.........................TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.........................T 8 | T.........................TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.........................T 9 | T...............................................................................................................................................................T 10 | T.........................TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.........................T 11 | T.........................TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.........................T 12 | T...............................................................................................................................................................T 13 | T.........................TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.........................T 14 | T.........................TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.........................T 15 | T...............................................................................................................................................................T 16 | T.........................TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.........................T 17 | T.........................TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.........................T 18 | T...............................................................................................................................................................T 19 | T.........................TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.........................T 20 | T.........................TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.........................T 21 | T...............................................................................................................................................................T 22 | T.........................TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.........................T 23 | T.........................TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.........................T 24 | T...............................................................................................................................................................T 25 | T.........................TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.........................T 26 | T.........................TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.........................T 27 | T...............................................................................................................................................................T 28 | T.........................TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.........................T 29 | T.........................TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.........................T 30 | T...............................................................................................................................................................T 31 | T.........................TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.........................T 32 | T.........................TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.........................T 33 | T...............................................................................................................................................................T 34 | T.........................TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.........................T 35 | T.........................TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.........................T 36 | T...............................................................................................................................................................T 37 | T.........................TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.........................T 38 | T.........................TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.........................T 39 | T...............................................................................................................................................................T 40 | T.........................TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.........................T 41 | T.........................TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.........................T 42 | T...............................................................................................................................................................T 43 | T.........................TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.........................T 44 | T.........................TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.........................T 45 | T...............................................................................................................................................................T 46 | T.........................TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.........................T 47 | T.........................TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.........................T 48 | T...............................................................................................................................................................T 49 | T.........................TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.........................T 50 | T.........................TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.........................T 51 | T...............................................................................................................................................................T 52 | T.........................TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.........................T 53 | T.........................TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.........................T 54 | T...............................................................................................................................................................T 55 | T.........................TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.........................T 56 | T.........................TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.........................T 57 | T...............................................................................................................................................................T 58 | T.........................TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.........................T 59 | T.........................TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.........................T 60 | T...............................................................................................................................................................T 61 | T.........................TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.........................T 62 | T.........................TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.........................T 63 | T...............................................................................................................................................................T 64 | T.........................TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.........................T 65 | T.........................TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.TTTTTTTTTT.........................T 66 | T...............................................................................................................................................................T 67 | TTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTT 68 | -------------------------------------------------------------------------------- /server/src/ADG_server.cpp: -------------------------------------------------------------------------------- 1 | #include "ADG_server.h" 2 | 3 | std::vector startTimers; // Start times for each robot 4 | // ======================= 5 | 6 | ADG_Server::ADG_Server(std::string &path_filename, 7 | std::string& target_output_filename, 8 | std::string map_name, 9 | std::string scen_name, 10 | std::string method_name, 11 | bool flip_coord): 12 | path_filename_(path_filename), curr_map_name(map_name), curr_scen_name(scen_name), curr_method_name(method_name) 13 | { 14 | if (path_filename == "none") { 15 | std::cerr << "No path file provided, exiting ..." << std::endl; 16 | exit(-1); 17 | } else { 18 | bool success = parseEntirePlan(path_filename, plans, raw_plan_cost, flip_coord); 19 | if (not success){ 20 | std::cerr << "Incorrect path, no ADG constructed! exiting ..." << std::endl; 21 | exit(-1); 22 | } 23 | } 24 | 25 | adg = std::make_shared (plans); 26 | output_filename = target_output_filename; 27 | numRobots = adg->numRobots(); 28 | agent_finish_time.resize(numRobots, -1); 29 | agents_finish.resize(numRobots, false); 30 | agent_finish_sim_step.resize(numRobots, -1); 31 | auto name_mapping = adg->createRobotIDToStartIndexMaps(); 32 | robotIDTOStartIndex = name_mapping.first; 33 | startIndexToRobotID = name_mapping.second; 34 | 35 | outgoingEdgesByRobot.resize(numRobots); 36 | startTimers.resize(numRobots); 37 | } 38 | 39 | void ADG_Server::saveStats() { 40 | std::ifstream infile(output_filename); 41 | bool exist = infile.good(); 42 | infile.close(); 43 | if (!exist) { 44 | ofstream addHeads(output_filename); 45 | addHeads << "steps finish sim,sum of steps finish sim,time finish sim,sum finish time,original plan cost," << 46 | "#type-2 edges,#type-1 edges,#Nodes," 47 | "#Move,#Rotate,#Consecutive Move,#Agent pair," << 48 | "instance name,number of agent" << endl; 49 | addHeads.close(); 50 | } 51 | ofstream stats(output_filename, std::ios::app); 52 | stats << latest_arr_sim_step << "," << std::accumulate(agent_finish_sim_step.begin(), agent_finish_sim_step.end(), 0) << "," << 53 | *(std::max_element(agent_finish_time.begin(), agent_finish_time.end())) << "," << 54 | std::accumulate(agent_finish_time.begin(), agent_finish_time.end(), 0.0) << "," << 55 | raw_plan_cost << "," << adg->adg_stats.type2EdgeCount << "," << 56 | adg->adg_stats.type1EdgeCount << "," << adg->adg_stats.totalNodes << "," << 57 | adg->adg_stats.moveActionCount << "," << adg->adg_stats.rotateActionCount << "," << 58 | adg->adg_stats.consecutiveMoveSequences << "," << static_cast(adg->adg_stats.conflict_pairs.size()) << "," << 59 | path_filename_ << "," << numRobots << endl; 60 | stats.close(); 61 | // std::cout << "ADG statistics written to " << output_filename << std::endl; 62 | json result = { 63 | {"steps finish sim", latest_arr_sim_step}, 64 | {"sum of steps finish sim", std::accumulate(agent_finish_sim_step.begin(), agent_finish_sim_step.end(), 0)}, 65 | {"time finish sim", *(std::max_element(agent_finish_time.begin(), agent_finish_time.end()))}, 66 | {"sum finish time", std::accumulate(agent_finish_time.begin(), agent_finish_time.end(), 0.0)}, 67 | {"original plan cost", raw_plan_cost}, 68 | {"#type-2 edges", adg->adg_stats.type2EdgeCount}, 69 | {"#type-1 edges", adg->adg_stats.type1EdgeCount}, 70 | {"#Nodes", adg->adg_stats.totalNodes}, 71 | {"#Move", adg->adg_stats.moveActionCount}, 72 | {"#Rotate", adg->adg_stats.rotateActionCount}, 73 | {"#Consecutive Move", adg->adg_stats.consecutiveMoveSequences}, 74 | {"#Agent pair", static_cast(adg->adg_stats.conflict_pairs.size())}, 75 | {"instance name", path_filename_}, 76 | {"number of agent", numRobots} 77 | }; 78 | 79 | // Output the JSON 80 | std::cout << result.dump() << std::endl; 81 | } 82 | 83 | 84 | std::shared_ptr server_ptr = nullptr; 85 | 86 | std::string receive_update(std::string& RobotID, int node_ID) { 87 | std::lock_guard guard(globalMutex); 88 | int Robot_ID = server_ptr->startIndexToRobotID[RobotID]; 89 | bool status_update = server_ptr->adg->updateFinishedNode(Robot_ID, node_ID); 90 | if (server_ptr->adg->isAgentFinished(Robot_ID)) { 91 | auto endTime = std::chrono::steady_clock::now(); 92 | auto diff = endTime - startTimers[Robot_ID]; 93 | double duration = std::chrono::duration_cast>(diff).count(); 94 | if (server_ptr->agent_finish_time[Robot_ID] < 0) { 95 | server_ptr->agent_finish_time[Robot_ID] = duration; 96 | server_ptr->agents_finish[Robot_ID] = true; 97 | } 98 | } 99 | 100 | server_ptr->all_agents_finished = true; 101 | for (auto status: server_ptr->agents_finish) { 102 | if (not status) { 103 | server_ptr->all_agents_finished = false; 104 | } 105 | } 106 | if (server_ptr->all_agents_finished) { 107 | return "exit"; 108 | } 109 | if (server_ptr->agents_finish[Robot_ID]) { 110 | return "end"; 111 | } 112 | return "None"; 113 | } 114 | 115 | std::vector, std::pair>> init(std::string RobotID) { 116 | std::lock_guard guard(globalMutex); 117 | int Robot_ID = server_ptr->startIndexToRobotID[RobotID]; 118 | #ifdef DEBUG 119 | std::cerr << "TMP::Receive init request from agent " << RobotID << " with: " << Robot_ID << std::endl; 120 | if (Robot_ID == DEBUG_AGENT) { 121 | std::cerr << "Receive init request from agent " << Robot_ID << std::endl; 122 | exit(0); 123 | } 124 | #endif 125 | startTimers[Robot_ID] = std::chrono::steady_clock::now(); 126 | return server_ptr->adg->getPlan(Robot_ID); 127 | } 128 | 129 | std::string getScenConfigName() 130 | { 131 | std::string target_path = server_ptr->curr_method_name + "/" + server_ptr->curr_map_name + "/" + std::to_string(server_ptr->numRobots) + "/" + server_ptr->curr_scen_name; 132 | return target_path; 133 | } 134 | 135 | 136 | std::vector, std::pair>> update(std::string RobotID) { 137 | std::lock_guard guard(globalMutex); 138 | 139 | 140 | int Robot_ID = server_ptr->startIndexToRobotID[RobotID]; 141 | // if (server_ptr->step_cnt % 20 == 0 and Robot_ID == 0) { 142 | // server_ptr->adg->printProgress(); 143 | // } 144 | server_ptr->step_cnt++; 145 | #ifdef DEBUG 146 | if (Robot_ID == DEBUG_AGENT) 147 | std::cerr << "Receive update request from agent " << Robot_ID << std::endl; 148 | #endif 149 | return server_ptr->adg->getPlan(Robot_ID); 150 | } 151 | 152 | void updateSimFinishTime(std::string& robot_id_str, int sim_step) 153 | { 154 | int robot_id = server_ptr->startIndexToRobotID[robot_id_str]; 155 | if (server_ptr->agent_finish_sim_step[robot_id] < 0) { 156 | server_ptr->agent_finish_sim_step[robot_id] = sim_step; 157 | server_ptr->latest_arr_sim_step = sim_step; 158 | } 159 | } 160 | 161 | void closeServer(rpc::server& srv) 162 | { 163 | // if (server_ptr->all_agents_finished) { 164 | // std::cout << "Finish all actions, exiting..." << std::endl; 165 | // } else { 166 | // std::cout << "BUG::exiting without finish all actions" << std::endl; 167 | // } 168 | server_ptr->saveStats(); 169 | srv.stop(); 170 | } 171 | 172 | int main(int argc, char **argv) { 173 | namespace po = boost::program_options; 174 | // Declare the supported options. 175 | po::options_description desc("Allowed options"); 176 | desc.add_options() 177 | ("help", "produce help message") 178 | // params for the input instance and experiment settings 179 | ("path_file,p", po::value(), "input file for path") 180 | // ("path_file,p", po::value()->default_value("../data/maze-32-32-4_paths.txt"), "input file for path") 181 | ("port_number,n", po::value()->default_value(8080), "rpc port number") 182 | ("flip_coord", po::value()->default_value(true), "input format of the mapf planner, 0 if xy, 1 if yx") 183 | ("output_file,o", po::value()->default_value("stats.csv"), "output statistic filename") 184 | ("map_file,m", po::value()->default_value("empty-8-8"), "map filename") 185 | ("scen_file,s", po::value()->default_value("empty-8-8-random-1"), "scen filename") 186 | ("method_name", po::value()->default_value("PBS"), "method we used") 187 | ; 188 | 189 | po::variables_map vm; 190 | po::store(po::parse_command_line(argc, argv, desc), vm); 191 | 192 | if (vm.count("help")) { 193 | cout << desc << endl; 194 | return 1; 195 | } 196 | po::notify(vm); 197 | std::string filename = "none"; 198 | 199 | if (vm.count("path_file")) { 200 | filename = vm["path_file"].as(); 201 | } 202 | // std::cout << "Solving for path name: " << filename << std::endl; 203 | std::string out_filename = vm["output_file"].as(); 204 | server_ptr = std::make_shared(filename, out_filename, vm["map_file"].as(), 205 | vm["scen_file"].as(), vm["method_name"].as(), vm["flip_coord"].as()); 206 | 207 | int port_number = vm["port_number"].as(); 208 | try { 209 | rpc::server srv(port_number); // Setup the server to listen on the specified port number 210 | srv.bind("receive_update", &receive_update); // Bind the function to the server 211 | srv.bind("init", &init); 212 | srv.bind("update", &update); 213 | srv.bind("get_config", &getScenConfigName); 214 | srv.bind("update_finish_agent", &updateSimFinishTime); 215 | srv.bind("closeServer", [&srv]() { 216 | closeServer(srv); 217 | }); 218 | srv.run(); // Start the server, blocking call 219 | } catch (...) { 220 | // Catch any other exceptions 221 | std::cerr << "Fail to starting the server for scen "<< vm["scen_file"].as() << " at port number: " << port_number << std::endl; 222 | } 223 | return 0; 224 | } -------------------------------------------------------------------------------- /client/controllers/footbot_nn/nn/ctrnn_multilayer.cpp: -------------------------------------------------------------------------------- 1 | #include "ctrnn_multilayer.h" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | /****************************************/ 8 | /****************************************/ 9 | 10 | CCtrnnMultilayer::CCtrnnMultilayer() : 11 | m_pfInputToHiddenWeights(NULL), 12 | m_pfHiddenBiases(NULL), 13 | m_pfHiddenTaus(NULL), 14 | m_pfHiddenDeltaStates(NULL), 15 | m_pfHiddenStates(NULL), 16 | m_pfHiddenToHiddenWeights(NULL), 17 | m_pfHiddenToOutputWeights(NULL), 18 | m_pfOutputBiases(NULL), 19 | m_pfOutputTaus(NULL), 20 | m_unNumberOfHiddenNodes(0), 21 | m_fTimeStep(0.1f), 22 | m_cWeightsBounds(-4.0f, 4.0f), 23 | m_cBiasesBounds(-4.0f, 4.0f), 24 | m_cTausBounds(-1.0f, 3.0f) {} 25 | 26 | /****************************************/ 27 | /****************************************/ 28 | 29 | CCtrnnMultilayer::~CCtrnnMultilayer() { 30 | if(m_pfInputToHiddenWeights) delete[] m_pfInputToHiddenWeights; 31 | if(m_pfHiddenToHiddenWeights) delete[] m_pfHiddenToHiddenWeights; 32 | if(m_pfHiddenBiases) delete[] m_pfHiddenBiases; 33 | if(m_pfHiddenToOutputWeights) delete[] m_pfHiddenToOutputWeights; 34 | if(m_pfOutputBiases) delete[] m_pfOutputBiases; 35 | if(m_pfHiddenTaus) delete[] m_pfHiddenTaus; 36 | if(m_pfHiddenDeltaStates) delete[] m_pfHiddenDeltaStates; 37 | if(m_pfHiddenStates) delete[] m_pfHiddenStates; 38 | } 39 | 40 | /****************************************/ 41 | /****************************************/ 42 | 43 | void CCtrnnMultilayer::Init( TConfigurationNode& t_node ) { 44 | //////////////////////////////////////////////////////////////////////////////// 45 | // First perform common initialisation from base class 46 | //////////////////////////////////////////////////////////////////////////////// 47 | CNeuralNetwork::Init( t_node ); 48 | 49 | //////////////////////////////////////////////////////////////////////////////// 50 | // Load number of hidden nodes 51 | //////////////////////////////////////////////////////////////////////////////// 52 | try{ 53 | GetNodeAttribute(t_node, "num_hidden", m_unNumberOfHiddenNodes); 54 | } 55 | catch(CARGoSException& ex) { 56 | THROW_ARGOSEXCEPTION_NESTED("Missing required tag for CTRNN multi-layer initialisation", ex); 57 | } 58 | 59 | //////////////////////////////////////////////////////////////////////////////// 60 | // integration step 61 | //////////////////////////////////////////////////////////////////////////////// 62 | GetNodeAttribute(t_node, "integration_step", m_fTimeStep); 63 | 64 | //////////////////////////////////////////////////////////////////////////////// 65 | // Load upper and lower bounds for weigths, biases and taus 66 | //////////////////////////////////////////////////////////////////////////////// 67 | GetNodeAttribute(t_node, "weight_range", m_cWeightsBounds); 68 | GetNodeAttribute(t_node, "bias_range", m_cBiasesBounds); 69 | GetNodeAttribute(t_node, "tau_range", m_cTausBounds); 70 | 71 | //////////////////////////////////////////////////////////////////////////////// 72 | // check and load parameters from file 73 | //////////////////////////////////////////////////////////////////////////////// 74 | if(m_strParameterFile != "") { 75 | try { 76 | LoadNetworkParameters(m_strParameterFile); 77 | } 78 | catch(CARGoSException& ex) { 79 | THROW_ARGOSEXCEPTION_NESTED("cannot load parameters from file.", ex); 80 | } 81 | } 82 | } 83 | 84 | /****************************************/ 85 | /****************************************/ 86 | 87 | void CCtrnnMultilayer::Reset() { 88 | for(size_t i = 0; i < m_unNumberOfHiddenNodes; ++i) { 89 | m_pfHiddenDeltaStates[i] = 0.0f; 90 | m_pfHiddenStates[i] = 0.0f; 91 | } 92 | } 93 | 94 | /****************************************/ 95 | /****************************************/ 96 | 97 | void CCtrnnMultilayer::Destroy() { 98 | m_unNumberOfHiddenNodes = 0; 99 | 100 | if( m_pfInputToHiddenWeights ) delete[] m_pfInputToHiddenWeights; 101 | m_pfInputToHiddenWeights = NULL; 102 | 103 | if( m_pfHiddenToHiddenWeights ) delete[] m_pfHiddenToHiddenWeights; 104 | m_pfHiddenToHiddenWeights = NULL; 105 | 106 | if( m_pfHiddenBiases ) delete[] m_pfHiddenBiases; 107 | m_pfHiddenBiases = NULL; 108 | 109 | if( m_pfHiddenToOutputWeights ) delete[] m_pfHiddenToOutputWeights; 110 | m_pfHiddenToOutputWeights = NULL; 111 | 112 | if( m_pfOutputBiases ) delete[] m_pfOutputBiases; 113 | m_pfOutputBiases = NULL; 114 | 115 | if( m_pfHiddenTaus ) delete[] m_pfHiddenTaus; 116 | m_pfHiddenTaus = NULL; 117 | 118 | if( m_pfHiddenDeltaStates ) delete[] m_pfHiddenDeltaStates; 119 | m_pfHiddenDeltaStates = NULL; 120 | 121 | if( m_pfHiddenStates ) delete[] m_pfHiddenStates; 122 | m_pfHiddenStates = NULL; 123 | } 124 | 125 | 126 | /****************************************/ 127 | /****************************************/ 128 | 129 | void CCtrnnMultilayer::LoadNetworkParameters(const std::string& str_filename ) { 130 | std::ifstream cIn; 131 | if( !cIn ) { 132 | THROW_ARGOSEXCEPTION("Cannot open parameter file '" << str_filename << "' for reading"); 133 | } 134 | 135 | // first parameter is the number of real-valued weights 136 | UInt32 un_length = 0; 137 | if( !(cIn >> un_length) ) { 138 | THROW_ARGOSEXCEPTION("Cannot read data from file '" << str_filename << "'"); 139 | } 140 | 141 | 142 | // create weights vector and load it from file 143 | Real* pfGenes = new Real[un_length]; 144 | for( UInt32 i = 0; i < un_length; i++ ) { 145 | if( !(cIn >> pfGenes[i]) ) { 146 | delete[] pfGenes; 147 | THROW_ARGOSEXCEPTION("Cannot read data from file '" << str_filename << "'"); 148 | } 149 | } 150 | 151 | // load parameters in the appropriate structures 152 | LoadNetworkParameters(un_length, pfGenes); 153 | delete[] pfGenes; 154 | } 155 | 156 | 157 | /****************************************/ 158 | /****************************************/ 159 | 160 | void CCtrnnMultilayer::LoadNetworkParameters( const UInt32 un_num_params, const Real* params ) { 161 | // check consistency between paramter file and xml declaration 162 | UInt32 un_num_parameters = 163 | m_unNumberOfHiddenNodes * (m_unNumberOfInputs + 1) + 164 | m_unNumberOfHiddenNodes * m_unNumberOfHiddenNodes + 165 | m_unNumberOfOutputs * (m_unNumberOfHiddenNodes + 1) + 166 | m_unNumberOfHiddenNodes; 167 | 168 | if(un_num_params != un_num_parameters) { 169 | THROW_ARGOSEXCEPTION("Number of parameter mismatch: '" 170 | << "passed " 171 | << un_num_params 172 | << " parameters, while " 173 | << un_num_parameters 174 | << " were expected from the xml configuration file"); 175 | } 176 | 177 | UInt32 unChromosomePosition = 0; 178 | 179 | if( m_pfInputToHiddenWeights == NULL ) m_pfInputToHiddenWeights = new Real[m_unNumberOfInputs * m_unNumberOfHiddenNodes]; 180 | for( UInt32 i = 0; i < m_unNumberOfInputs * m_unNumberOfHiddenNodes; i++ ) { 181 | m_pfInputToHiddenWeights[i] = params[unChromosomePosition++]*(m_cWeightsBounds.GetMax() - m_cWeightsBounds.GetMin() ) + m_cWeightsBounds.GetMin(); 182 | } 183 | 184 | if( m_pfHiddenToHiddenWeights == NULL ) m_pfHiddenToHiddenWeights = new Real[m_unNumberOfHiddenNodes * m_unNumberOfHiddenNodes]; 185 | for( UInt32 i = 0; i < m_unNumberOfHiddenNodes * m_unNumberOfHiddenNodes; i++ ) { 186 | m_pfHiddenToHiddenWeights[i] = params[unChromosomePosition++]*(m_cWeightsBounds.GetMax() - m_cWeightsBounds.GetMin() ) + m_cWeightsBounds.GetMin(); 187 | } 188 | 189 | if( m_pfHiddenBiases == NULL ) m_pfHiddenBiases = new Real[m_unNumberOfHiddenNodes]; 190 | for( UInt32 i = 0; i < m_unNumberOfHiddenNodes; i++ ) { 191 | m_pfHiddenBiases[i] = params[unChromosomePosition++]*(m_cBiasesBounds.GetMax() - m_cBiasesBounds.GetMin() ) + m_cBiasesBounds.GetMin(); 192 | } 193 | 194 | if( m_pfHiddenTaus == NULL ) m_pfHiddenTaus = new Real[m_unNumberOfHiddenNodes]; 195 | for( UInt32 i = 0; i < m_unNumberOfHiddenNodes; i++ ) { 196 | m_pfHiddenTaus[i] = pow(10, (m_cTausBounds.GetMin() + ((m_cTausBounds.GetMax()-m_cTausBounds.GetMin()) * (params[unChromosomePosition++])))); 197 | } 198 | 199 | if( m_pfHiddenToOutputWeights == NULL ) m_pfHiddenToOutputWeights = new Real[m_unNumberOfHiddenNodes * m_unNumberOfOutputs]; 200 | for( UInt32 i = 0; i < m_unNumberOfHiddenNodes * m_unNumberOfOutputs; i++ ) { 201 | m_pfHiddenToOutputWeights[i] = params[unChromosomePosition++]*(m_cWeightsBounds.GetMax() - m_cWeightsBounds.GetMin() ) + m_cWeightsBounds.GetMin(); 202 | } 203 | 204 | if( m_pfOutputBiases == NULL ) m_pfOutputBiases = new Real[m_unNumberOfOutputs]; 205 | for( UInt32 i = 0; i < m_unNumberOfOutputs; i++ ) { 206 | m_pfOutputBiases[i] = params[unChromosomePosition++]*(m_cBiasesBounds.GetMax() - m_cBiasesBounds.GetMin() ) + m_cBiasesBounds.GetMin(); 207 | } 208 | 209 | if( m_pfHiddenDeltaStates == NULL) m_pfHiddenDeltaStates = new Real[m_unNumberOfHiddenNodes]; 210 | if( m_pfHiddenStates == NULL ) m_pfHiddenStates = new Real[m_unNumberOfHiddenNodes]; 211 | for( UInt32 i = 0; i < m_unNumberOfHiddenNodes; i++ ) { 212 | m_pfHiddenDeltaStates[i] = 0.0f; 213 | m_pfHiddenStates[i] = 0.0f; 214 | } 215 | } 216 | 217 | 218 | 219 | /****************************************/ 220 | /****************************************/ 221 | 222 | void CCtrnnMultilayer::ComputeOutputs( void ) { 223 | // Update delta state of hidden layer from inputs: 224 | for( UInt32 i = 0; i < m_unNumberOfHiddenNodes; i++ ) { 225 | m_pfHiddenDeltaStates[i] = -m_pfHiddenStates[i]; 226 | 227 | for( UInt32 j = 0; j < m_unNumberOfInputs; j++ ) { 228 | // weight * sigmoid(state) 229 | m_pfHiddenDeltaStates[i] += m_pfInputToHiddenWeights[i * m_unNumberOfInputs + j] * m_pfInputs[j] ; 230 | } 231 | 232 | // Update delta state from hidden layer, self-recurrent connections: 233 | for( UInt32 j = 0; j < m_unNumberOfHiddenNodes; j++ ) { 234 | Real z = (Real(1.0)/(exp(-( m_pfHiddenStates[j] + m_pfHiddenBiases[j])) + 1.0)); 235 | m_pfHiddenDeltaStates[i] += m_pfHiddenToHiddenWeights[i * m_unNumberOfHiddenNodes + j] * z; 236 | } 237 | } 238 | 239 | 240 | // once all delta state are computed, get the new activation for the hidden unit 241 | for( UInt32 i = 0; i < m_unNumberOfHiddenNodes; i++ ) { 242 | m_pfHiddenStates[i] += m_pfHiddenDeltaStates[i] * m_fTimeStep/m_pfHiddenTaus[i]; 243 | } 244 | 245 | // Update the outputs layer:: 246 | for( UInt32 i = 0; i < m_unNumberOfOutputs; i++ ) { 247 | 248 | // Initialise to 0 249 | m_pfOutputs[i] = 0.0f; 250 | 251 | // Sum over all the hidden nodes 252 | for( UInt32 j = 0; j < m_unNumberOfHiddenNodes; j++ ) { 253 | Real z = (Real(1.0)/( exp(-( m_pfHiddenStates[j] + m_pfHiddenBiases[j])) + 1.0 )); 254 | m_pfOutputs[i] += m_pfHiddenToOutputWeights[i * m_unNumberOfHiddenNodes + j] * z; 255 | } 256 | 257 | // Compute the activation function immediately, since this is 258 | // what we return and since the output layer is not recurrent: 259 | m_pfOutputs[i] = (Real(1.0)/( exp(-( m_pfOutputs[i] + m_pfOutputBiases[i])) + 1.0 )); 260 | } 261 | } 262 | 263 | -------------------------------------------------------------------------------- /server/src/parser.cpp: -------------------------------------------------------------------------------- 1 | #include "parser.h" 2 | 3 | int getOrientation(int x1, int y1, int x2, int y2) { 4 | if (x1 == x2 and y1 == y2) { 5 | // indicate inplace waiting 6 | return -1; 7 | } 8 | if (x2 == x1) { 9 | return y2 > y1 ? 1 : 3; // North or South 10 | } else if (y2 == y1) { 11 | return x2 > x1 ? 2 : 0; // East or West 12 | } else { 13 | raiseError("Invalid orient!\n"); 14 | exit(-2); 15 | } 16 | } 17 | 18 | void processAgentActions(const vector& points, vector& steps, bool flipped_coord) { 19 | steps.clear(); 20 | int currentOrientation = 0; 21 | double currentTime = 0; 22 | for (size_t i = 0; i < points.size(); ++i) { 23 | if (i == 0) { 24 | steps.push_back({points[i].x, points[i].y, currentOrientation, currentTime}); 25 | } else { 26 | int neededOrientation = getOrientation(points[i-1].x, points[i-1].y, points[i].x, points[i].y); 27 | 28 | if (not flipped_coord) { 29 | neededOrientation = 3 - neededOrientation; 30 | } 31 | 32 | if (neededOrientation == -1 or neededOrientation == 4) { 33 | neededOrientation = currentOrientation; 34 | } 35 | 36 | if (neededOrientation != currentOrientation) { 37 | steps.push_back({points[i-1].x, points[i-1].y, neededOrientation, currentTime}); 38 | currentOrientation = neededOrientation; 39 | } 40 | steps.push_back({points[i].x, points[i].y, currentOrientation, ++currentTime}); 41 | } 42 | } 43 | } 44 | 45 | void processAgentActionsContinuous(const vector& points, vector& steps, bool flipped_coord) { 46 | steps.clear(); 47 | int currentOrientation = 0; 48 | double currentTime = 0.0; 49 | for (size_t i = 0; i < points.size(); ++i) { 50 | if (i == 0) { 51 | currentTime = points[i].time; 52 | steps.push_back({points[i].x, points[i].y, currentOrientation, currentTime}); 53 | } else { 54 | int neededOrientation = getOrientation(points[i-1].x, points[i-1].y, points[i].x, points[i].y); 55 | if (not flipped_coord) { 56 | neededOrientation = 3 - neededOrientation; 57 | } 58 | 59 | if (neededOrientation == -1 or neededOrientation == 4) { 60 | neededOrientation = currentOrientation; 61 | } 62 | 63 | if (neededOrientation != currentOrientation) { 64 | steps.push_back({points[i-1].x, points[i-1].y, neededOrientation, currentTime}); 65 | currentOrientation = neededOrientation; 66 | } 67 | currentTime = points[i].time; 68 | steps.push_back({points[i].x, points[i].y, currentOrientation, currentTime}); 69 | } 70 | } 71 | } 72 | 73 | vector parseLine(const string& line) { 74 | vector points; 75 | stringstream ss(line); 76 | char ignore; 77 | bool first_loc = true; 78 | int prev_x = 0, prev_y = 0; 79 | int x, y; 80 | double time = 1.0; 81 | 82 | ss.ignore(100, ':'); 83 | 84 | while (ss >> ignore >> x >> ignore >> y >> ignore) { 85 | if (first_loc) { 86 | prev_x = x; 87 | prev_y = y; 88 | first_loc = false; 89 | } 90 | if (std::abs(prev_x - x) + std::abs(prev_y - y) >= 2) { 91 | raiseError("Invalid Plan"); 92 | // std::cerr << "Invalid Plan, move from " << prev_x << "," << prev_y << ", to: " << 93 | // x << "," << y << std::endl; 94 | } 95 | points.push_back({x, y, time++}); 96 | prev_x = x; 97 | prev_y = y; 98 | ss >> ignore >> ignore; 99 | } 100 | 101 | return points; 102 | } 103 | 104 | vector parseLineContinuous(const string& line) { 105 | vector points; 106 | stringstream ss(line); 107 | char ignore; 108 | bool first_loc = true; 109 | int prev_x = 0, prev_y = 0; 110 | int x, y; 111 | double t; 112 | 113 | ss.ignore(100, ':'); 114 | while (ss >> ignore >> x >> ignore >> y >> ignore >> t >> ignore) { 115 | if (first_loc) { 116 | prev_x = x; 117 | prev_y = y; 118 | first_loc = false; 119 | } 120 | if (std::abs(prev_x - x) + std::abs(prev_y - y) >= 2) { 121 | raiseError("Invalid Plan"); 122 | } 123 | points.push_back({x, y, t}); 124 | prev_x = x; 125 | prev_y = y; 126 | ss >> ignore >> ignore; 127 | } 128 | 129 | return points; 130 | } 131 | 132 | std::vector> processActions(const std::vector>& raw_steps, bool flipped_coord) { 133 | std::vector> plans; 134 | int node_id=0; 135 | for (size_t i = 0; i < raw_steps.size(); ++i) { 136 | std::vector processedActions; 137 | for (size_t j = 1; j < raw_steps[i].size(); j++) { 138 | double prev_step_x, prev_step_y, curr_step_x, curr_step_y; 139 | if (flipped_coord) { 140 | prev_step_x = raw_steps[i][j-1].y; 141 | prev_step_y = raw_steps[i][j-1].x; 142 | curr_step_x = raw_steps[i][j].y; 143 | curr_step_y = raw_steps[i][j].x; 144 | } else { 145 | prev_step_x = raw_steps[i][j-1].x; 146 | prev_step_y = raw_steps[i][j-1].y; 147 | curr_step_x = raw_steps[i][j].x; 148 | curr_step_y = raw_steps[i][j].y; 149 | } 150 | Action processedAction; 151 | processedAction.robot_id = (int) i; 152 | processedAction.time = raw_steps[i][j-1].time; 153 | processedAction.start.first = prev_step_x; 154 | processedAction.start.second = prev_step_y; 155 | processedAction.goal.first = curr_step_x; 156 | processedAction.goal.second = curr_step_y; 157 | processedAction.orientation = raw_steps[i][j].orientation; 158 | processedAction.nodeID = node_id; 159 | Action accesoray_action = processedAction; 160 | 161 | if (processedAction.start == processedAction.goal && 162 | raw_steps[i][j-1].orientation != raw_steps[i][j].orientation) { 163 | processedAction.type = 'T'; 164 | } else if (processedAction.start != processedAction.goal && 165 | raw_steps[i][j-1].orientation == raw_steps[i][j].orientation) { 166 | double mid_x = (prev_step_x + curr_step_x)/2.0; 167 | double mid_y = (prev_step_y + curr_step_y)/2.0; 168 | processedAction.type = 'M'; 169 | processedAction.start.first = prev_step_x; 170 | processedAction.start.second = prev_step_y; 171 | processedAction.goal.first = mid_x; 172 | processedAction.goal.second = mid_y; 173 | accesoray_action.type = 'M'; 174 | accesoray_action.start.first = mid_x; 175 | accesoray_action.start.second = mid_y; 176 | accesoray_action.goal.first = curr_step_x; 177 | accesoray_action.goal.second = curr_step_y; 178 | accesoray_action.nodeID = node_id+1; 179 | } else if (processedAction.start == processedAction.goal && 180 | raw_steps[i][j-1].orientation == raw_steps[i][j].orientation) { 181 | continue; 182 | } else { 183 | std::cerr << "Invalid case exiting ..." << std::endl; 184 | exit(-1); 185 | } 186 | 187 | if (processedAction.type == 'T' and j == (raw_steps[i].size()-1)) { 188 | continue; 189 | } 190 | processedActions.push_back(processedAction); 191 | if (processedAction.type == 'M') { 192 | processedActions.push_back(accesoray_action); 193 | node_id++; 194 | } 195 | node_id++; 196 | } 197 | plans.push_back(processedActions); 198 | } 199 | return plans; 200 | } 201 | 202 | void showStepPoints(std::vector>& raw_plan){ 203 | for (size_t i = 0; i < raw_plan.size(); i++) { 204 | printf("Path of agent: %lu\n", i); 205 | for (auto& tmp_point: raw_plan[i]) { 206 | std::cout << "(" << tmp_point.x << ", " << tmp_point.y << "), " 207 | << tmp_point.orientation << ", " << tmp_point.time << ", " 208 | << std::endl; 209 | } 210 | printf("\n"); 211 | } 212 | } 213 | 214 | void showActionsPlan(std::vector>& plans) { 215 | for (size_t i = 0; i < plans.size(); i++) { 216 | printf("Path of agent: %lu\n", i); 217 | for (auto &action: plans[i]) { 218 | std::cout << " {" << action.robot_id << ", " << action.time << ", " 219 | << std::fixed << std::setprecision(1) << action.orientation << ", '" 220 | << action.type << "', {" << action.start.first << ", " << action.start.second << "}, {" 221 | << action.goal.first << ", " << action.goal.second << "}, " << action.nodeID << "}," << std::endl; 222 | } 223 | printf("\n"); 224 | } 225 | } 226 | 227 | void raiseError(const string &msg) { 228 | std::cout << "Invalid MAPF plan: " << msg << std::endl; 229 | // std::cerr << msg << std::endl; 230 | exit(-1); 231 | } 232 | 233 | 234 | bool parseEntirePlan(const std::string& input_file, 235 | std::vector>& plans, 236 | double& raw_cost, 237 | bool flipped_coord, 238 | PlanType file_type){ 239 | plans.clear(); 240 | ifstream inFile(input_file); 241 | if (!inFile.is_open()) { 242 | cerr << "Failed to open the file." << endl; 243 | return false; 244 | } 245 | 246 | if (file_type == DISCRETE) { 247 | std::vector> raw_plan; 248 | raw_cost = 0.0; 249 | string line; 250 | int agentId = 0; 251 | while (getline(inFile, line)) { 252 | std::vector tmp_plan; 253 | if (!line.empty()) { 254 | std::vector points = parseLine(line); 255 | raw_cost += static_cast (points.size()); 256 | processAgentActions(points, tmp_plan, flipped_coord); 257 | agentId++; 258 | } 259 | raw_plan.push_back(tmp_plan); 260 | } 261 | inFile.close(); 262 | plans = processActions(raw_plan, flipped_coord); 263 | return true; 264 | } else if (file_type == CONTINUOUS) { 265 | std::vector> raw_plan; 266 | raw_cost = 0.0; 267 | string line; 268 | int agentId = 0; 269 | while (getline(inFile, line)) { 270 | std::vector tmp_plan; 271 | if (!line.empty()) { 272 | vector points = parseLineContinuous(line); 273 | raw_cost += static_cast (points.end()->time); 274 | processAgentActionsContinuous(points, tmp_plan, flipped_coord); 275 | agentId++; 276 | } 277 | raw_plan.push_back(tmp_plan); 278 | } 279 | inFile.close(); 280 | // showStepPoints(raw_plan); 281 | plans = processActions(raw_plan, flipped_coord); 282 | // showActionsPlan(plans); 283 | return true; 284 | } else { 285 | std::cerr << "Unsupported path format!" << std::endl; 286 | } 287 | exit(-1); 288 | } 289 | -------------------------------------------------------------------------------- /ArgosConfig/ToArgos.py: -------------------------------------------------------------------------------- 1 | import xml.etree.ElementTree as ET 2 | from xml.dom import minidom 3 | import argparse 4 | import os 5 | 6 | # def prettify(elem, level=0): 7 | # """Return a pretty-printed XML string for the Element.""" 8 | # indent = " " 9 | # newline = "\n" 10 | # if len(elem): 11 | # if not elem.text or not elem.text.strip(): 12 | # elem.text = newline + indent * (level + 1) 13 | # if not elem.tail or not elem.tail.strip(): 14 | # elem.tail = newline + indent * level 15 | # for child in elem: 16 | # prettify(child, level + 1) 17 | # if not elem.tail or not elem.tail.strip(): 18 | # elem.tail = newline + indent * level 19 | # else: 20 | # if level and (not elem.tail or not elem.tail.strip()): 21 | # elem.tail = newline + indent * level 22 | obstacles = ['@', 'T'] 23 | 24 | def prettify(elem): 25 | """Return a pretty-printed XML string for the Element.""" 26 | rough_string = ET.tostring(elem, 'utf-8') 27 | reparsed = minidom.parseString(rough_string) 28 | return reparsed.toprettyxml(indent=" ") 29 | 30 | def parse_map_file(map_file_path): 31 | map_data = [] 32 | with open(map_file_path, 'r') as file: 33 | lines = file.readlines() 34 | height = int(lines[1].split()[1]) 35 | width = int(lines[2].split()[1]) 36 | print(f"Width: {width}, Height: {height}") 37 | for line in lines[4:]: # Skip the first 4 lines (type, height, width, map) 38 | map_data.append(line.strip()) 39 | return map_data, width, height 40 | 41 | 42 | def read_scen(file_path): 43 | column_data = [] 44 | num_agent = 0 45 | with open(file_path, 'r') as file: 46 | lines = file.readlines() 47 | num_agent = len(lines) - 1 48 | for line in lines[1:]: 49 | columns = line.split('\t') 50 | column_5 = columns[4] 51 | column_6 = columns[5] 52 | column_data.append((column_5, column_6)) 53 | return column_data, num_agent 54 | 55 | def create_xml(map_data, output_file_path, width, height, robot_init_pos): 56 | arena = ET.Element("arena", size=f"{width},{height},1", center=f"{width/2},{height/2},0") 57 | arena.text = "\n " 58 | # Adding new line 59 | 60 | for y, row in enumerate(map_data): 61 | for x, cell in enumerate(row): 62 | if cell in obstacles: 63 | # Creating four walls for each box 64 | box = ET.SubElement(arena, "box", id=f"box_{x}_{y}", size="0.9,0.9,0.1", movable="false") 65 | body = ET.SubElement(box, "body", position=f"{-y},{-x},0", orientation="0,0,0") 66 | 67 | tree = ET.ElementTree(arena) 68 | 69 | for x, y in robot_init_pos: 70 | foot_bot = ET.SubElement(arena, "foot-bot", id=f"fb_{x}_{y}") 71 | x, y = -int(y), -int(x) 72 | body = ET.SubElement(foot_bot, "body", position=f"{x},{y},0", orientation="0,0,0") 73 | controller = ET.SubElement(foot_bot, "controller", config="fdc") 74 | 75 | 76 | prettify(arena) 77 | 78 | tree.write(output_file_path, encoding='utf-8', xml_declaration=True) 79 | 80 | def create_Argos(map_data, output_file_path, width, height, robot_init_pos, curr_num_agent, port_num, visualization=False): 81 | # Create the root element 82 | argos_config = ET.Element("argos-configuration") 83 | 84 | # General configuration section 85 | framework = ET.SubElement(argos_config, "framework") 86 | 87 | # System configuration 88 | system = ET.SubElement(framework, "system", threads="32") 89 | 90 | # Experiment configuration 91 | if visualization: 92 | experiment = ET.SubElement(framework, "experiment", 93 | length="0", 94 | ticks_per_second="10", 95 | random_seed="124") 96 | else: 97 | experiment = ET.SubElement(framework, "experiment", 98 | length="0", 99 | ticks_per_second="10", 100 | random_seed="124", 101 | visualization="none") 102 | 103 | # Controllers section 104 | controllers = ET.SubElement(argos_config, "controllers") 105 | 106 | # Footbot controller 107 | footbot_controller = ET.SubElement( 108 | controllers, 109 | "footbot_diffusion_controller", 110 | id="fdc", 111 | library="build/client/controllers/footbot_diffusion/libfootbot_diffusion") 112 | 113 | # Actuators 114 | actuators = ET.SubElement(footbot_controller, "actuators") 115 | differential_steering = ET.SubElement(actuators, "differential_steering", implementation="default") 116 | 117 | # Sensors 118 | sensors = ET.SubElement(footbot_controller, "sensors") 119 | footbot_proximity = ET.SubElement(sensors, "footbot_proximity", implementation="default", show_rays="true") 120 | positioning = ET.SubElement(sensors, "positioning", implementation="default") 121 | 122 | # Parameters 123 | params = ET.SubElement(footbot_controller, "params", alpha="7.5", omega="1.57", velocity="2.0", acceleration="0.5", portNumber=f"{port_num}", outputDir=f"metaData{port_num}/") 124 | 125 | map_center_x = -height / 2+0.5 126 | map_center_y = -width / 2+0.5 127 | arena = ET.SubElement(argos_config, "arena", size=f"{height},{width},1", center=f"{map_center_x},{map_center_y},0") 128 | 129 | # arena.text = "\n " 130 | # Adding new line 131 | 132 | for y, row in enumerate(map_data): 133 | for x, cell in enumerate(row): 134 | if cell in obstacles: 135 | # Creating four walls for each box 136 | box = ET.SubElement(arena, "box", id=f"box_{x}_{y}", size="0.9,0.9,0.1", movable="false") 137 | body = ET.SubElement(box, "body", position=f"{-y},{-x},0", orientation="0,0,0") 138 | 139 | # tree = ET.ElementTree(arena) 140 | 141 | agent_count = 0 142 | for x, y in robot_init_pos: 143 | foot_bot = ET.SubElement(arena, "foot-bot", id=f"fb_{x}_{y}") 144 | x, y = -int(y), -int(x) 145 | body = ET.SubElement(foot_bot, "body", position=f"{x},{y},0", orientation="0,0,0") 146 | controller = ET.SubElement(foot_bot, "controller", config="fdc") 147 | agent_count += 1 148 | if agent_count >= curr_num_agent: 149 | break 150 | 151 | ############################################################################################# 152 | # # Arena configuration 153 | # arena = ET.SubElement(argos_config, "arena", size="8,8,1", center="-3.5,-3.5,0") 154 | # 155 | # # Foot-bots within the arena 156 | # footbot_positions = [ 157 | # {"id": "fb_1_4", "position": "-4,-1,0"}, 158 | # {"id": "fb_1_0", "position": "0,-1,0"}, 159 | # {"id": "fb_1_6", "position": "-6,-1,0"}, 160 | # {"id": "fb_4_6", "position": "-6,-4,0"}, 161 | # {"id": "fb_7_2", "position": "-2,-7,0"}, 162 | # {"id": "fb_0_1", "position": "-1,0,0"}, 163 | # {"id": "fb_7_6", "position": "-6,-7,0"}, 164 | # {"id": "fb_7_7", "position": "-7,-7,0"}, 165 | # {"id": "fb_0_4", "position": "-4,0,0"}, 166 | # {"id": "fb_6_0", "position": "0,-6,0"} 167 | # ] 168 | # 169 | # for fb in footbot_positions: 170 | # footbot = ET.SubElement(arena, "foot-bot", id=fb["id"]) 171 | # body = ET.SubElement(footbot, "body", position=fb["position"], orientation="0,0,0") 172 | # controller = ET.SubElement(footbot, "controller", config="fdc") 173 | ############################################################################################# 174 | 175 | # Physics engines 176 | physics_engines = ET.SubElement(argos_config, "physics_engines") 177 | dynamics2d = ET.SubElement(physics_engines, "dynamics2d", id="dyn2d") 178 | 179 | # Media 180 | media = ET.SubElement(argos_config, "media") 181 | if visualization: 182 | # Visualization 183 | visualization = ET.SubElement(argos_config, "visualization") 184 | # qt_opengl = ET.SubElement(visualization, "qt-opengl", autoplay="true") 185 | qt_opengl = ET.SubElement(visualization, "qt-opengl") 186 | 187 | # autoplay = ET.SubElement(qt_opengl, "autoplay", 188 | # autoplay="true") 189 | # Camera 190 | camera = ET.SubElement(qt_opengl, "camera") 191 | placements = ET.SubElement(camera, "placements") 192 | placement = ET.SubElement(placements, "placement", 193 | index="0", 194 | position=f"{map_center_x},{map_center_y},{max(width,height)}", 195 | look_at=f"{map_center_x},{map_center_y},0", 196 | up="1,0,0", 197 | lens_focal_length="20") 198 | xml_str = prettify(argos_config) 199 | with open(output_file_path, "w") as f: 200 | f.write(xml_str) 201 | 202 | 203 | def create_folder(folder: str): 204 | if not os.path.exists(folder): 205 | os.makedirs(folder) 206 | 207 | if __name__ == "__main__": 208 | # parser = argparse.ArgumentParser(description="Parser for input map and scen arguments.") 209 | # # parser.add_argument("-parent", '-p', type=str, default="./data/empty-8-8", help="parent folder") 210 | # # parser.add_argument('-scen_filename', '-s', type=str, default="empty-8-8-random-1.scen", help='name of scen file') 211 | # # parser.add_argument('-map_filename', '-m', type=str, default="empty-8-8.map", help='name of map file') 212 | # # parser.add_argument('-output_filename', '-o', type=str, default="empty-8-8-1.argos", help='name of output file') 213 | # # parser.add_argument("-parent", '-p', type=str, default="./data/maze-32-32-4", help="parent folder") 214 | # # parser.add_argument('-scen_filename', '-s', type=str, default="maze-32-32-4-random-1.scen", help='name of scen file') 215 | # # parser.add_argument('-map_filename', '-m', type=str, default="maze-32-32-4.map", help='name of map file') 216 | # # parser.add_argument('-output_filename', '-o', type=str, default="maze-32-32-4-random-1.argos", help='name of output file') 217 | # parser.add_argument("-parent", '-p', type=str, default="./data/warehouse-10-20-10-2-1", help="parent folder") 218 | # parser.add_argument('-scen_filename', '-s', type=str, default="scen-random/warehouse-10-20-10-2-1-random-1.scen", help='name of scen file') 219 | # parser.add_argument('-map_filename', '-m', type=str, default="warehouse-10-20-10-2-1.map", help='name of map file') 220 | # parser.add_argument('-output_filename', '-o', type=str, default="warehouse-10-20-10-2-1-random-1.argos", help='name of output file') 221 | # args = parser.parse_args() 222 | # scen_file_path = os.path.join(args.parent, args.scen_filename) 223 | # map_file_path = os.path.join(args.parent, args.map_filename) 224 | # output_file_path = os.path.join(args.parent, args.output_filename) 225 | # robot_init_pos = read_scen(scen_file_path) 226 | # map_data, width, height = parse_map_file(map_file_path) 227 | # # create_xml(map_data, output_file_path, width, height, robot_init_pos) 228 | # create_Argos(map_data, output_file_path, width, height, robot_init_pos) 229 | 230 | map_list = ["empty-32-32", "Boston_0_256", "room-64-64-16", "maze-32-32-4", "random-64-64-10", 231 | "warehouse-20-40-10-2-2", "den520d"] 232 | NUM_AGENT_STEP = 20 233 | for map in map_list: 234 | map_filename = os.path.join("./data", map, map + ".map") 235 | argos_folder = os.path.join("output", map) 236 | map_data, width, height = parse_map_file(map_filename) 237 | create_folder(argos_folder) 238 | all_agents_solved = True 239 | curr_num_agent = 0 240 | while all_agents_solved: 241 | curr_num_agent += NUM_AGENT_STEP 242 | output_num_agent_path = os.path.join(argos_folder, str(curr_num_agent)) 243 | create_folder(output_num_agent_path) 244 | for scen_idx in range(1, 26): 245 | scen_filename = os.path.join("./data", map, "scen-random", map + f"-random-{scen_idx}.scen") 246 | output_config_filename = os.path.join(output_num_agent_path, map + f"-random-{scen_idx}.argos") 247 | robot_init_pos, scen_num_agent = read_scen(scen_filename) 248 | # create_xml(map_data, output_file_path, width, height, robot_init_pos) 249 | if scen_num_agent < curr_num_agent: 250 | all_agents_solved = False 251 | break 252 | port_num = 12000+scen_idx 253 | create_Argos(map_data, output_config_filename, width, height, robot_init_pos, curr_num_agent, port_num) -------------------------------------------------------------------------------- /client/controllers/footbot_diffusion/footbot_diffusion.cpp: -------------------------------------------------------------------------------- 1 | #include "footbot_diffusion.h" 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | 8 | CFootBotDiffusion::CFootBotDiffusion() : 9 | m_pcWheels(NULL), 10 | m_pcProximity(NULL), 11 | m_pcPosSens(NULL), 12 | m_cAlpha(10.0f), 13 | m_angularVelocity(0.5f), 14 | m_fWheelVelocity(2.5f), 15 | m_cGoStraightAngleRange(-ToRadians(m_cAlpha), 16 | ToRadians(m_cAlpha)) {} 17 | 18 | void CFootBotDiffusion::insertActions(std::vector actions) 19 | { 20 | for (const auto &action: actions) { 21 | std::string action1 = std::get<3>(action); 22 | int nodeID = std::get<1>(action); 23 | std::tuple end_pos = std::get<5>(action); 24 | double x = ChangeCoordinateFromMapToArgos(std::get<1>(end_pos)); 25 | double y = ChangeCoordinateFromMapToArgos(std::get<0>(end_pos)); 26 | double angle; 27 | if (std::get<2>(action) == 0) { 28 | angle = 0.0; 29 | } else if (std::get<2>(action) == 1) { 30 | angle = 270.0; 31 | } else if (std::get<2>(action) == 2) { 32 | angle = 180.0; 33 | } else { 34 | angle = 90.0; 35 | } 36 | if (robot_id == debug_id) { 37 | std::cout << "Action: " << action1 << " NodeID: " << nodeID << " End Position: " << x << " " << y << " Angle: " << angle << std::endl; 38 | } 39 | if (action1 == "M") { 40 | std::deque prev_ids; 41 | if (not q.empty() and q.back().type == Action::MOVE) { 42 | prev_ids = q.back().nodeIDS; 43 | q.pop_back(); 44 | } 45 | prev_ids.push_back(nodeID); 46 | q.push_back({x, y, angle, prev_ids, Action::MOVE}); 47 | } else if (action1 == "T") { 48 | q.push_back({x, y, angle, std::deque{nodeID}, Action::TURN}); 49 | } else { 50 | q.push_back({x, y, angle, std::deque{nodeID}, Action::STOP}); 51 | } 52 | } 53 | } 54 | 55 | void CFootBotDiffusion::Init(TConfigurationNode &t_node) { 56 | m_pcWheels = GetActuator("differential_steering"); 57 | m_pcProximity = GetSensor("footbot_proximity"); 58 | m_pcPosSens = GetSensor("positioning"); 59 | 60 | angular_pid_ptr = std::make_shared(1.0, 0.0, 0.1); 61 | linear_pid_ptr = std::make_shared(2.0, 0.0, 0.1); 62 | GetNodeAttributeOrDefault(t_node, "alpha", m_cAlpha, m_cAlpha); 63 | m_cGoStraightAngleRange.Set(-ToRadians(m_cAlpha), ToRadians(m_cAlpha)); 64 | GetNodeAttributeOrDefault(t_node, "omega", m_angularVelocity, m_angularVelocity); 65 | GetNodeAttributeOrDefault(t_node, "velocity", m_fWheelVelocity, m_fWheelVelocity); 66 | GetNodeAttributeOrDefault(t_node, "acceleration", m_linearAcceleration, m_linearAcceleration); 67 | GetNodeAttributeOrDefault(t_node, "portNumber", port_number, 8080); 68 | GetNodeAttributeOrDefault(t_node, "outputDir", m_outputDir,std::string("metaData/")); 69 | m_rotateWheelVelocity = 0.144 * m_angularVelocity / 2; // Convert from rad/s to m/s 70 | m_currVelocity = 0.0; 71 | CVector3 currPos = m_pcPosSens->GetReading().Position; 72 | robot_id = std::to_string((int) ChangeCoordinateFromArgosToMap(currPos.GetY())) + "_" + 73 | std::to_string((int) ChangeCoordinateFromArgosToMap(currPos.GetX())); 74 | } 75 | 76 | 77 | Real normalizeAngle(Real angle) { 78 | while (angle > 180) angle -= 360; 79 | while (angle < -180) angle += 360; 80 | return angle; 81 | } 82 | 83 | double angleDifference(double curr_angle, double target_angle) { 84 | double diff = normalizeAngle(target_angle) - normalizeAngle(curr_angle); 85 | if (diff > 180.0) { 86 | diff -= 360.0; 87 | } else if (diff < -180.0) { 88 | diff += 360.0; 89 | } 90 | 91 | return abs(diff); 92 | } 93 | 94 | bool is_port_open(const std::string& ip, int port) { 95 | boost::asio::io_context io_context; 96 | boost::asio::ip::tcp::socket socket(io_context); 97 | 98 | try { 99 | boost::asio::ip::tcp::endpoint endpoint(boost::asio::ip::address::from_string(ip), port); 100 | socket.connect(endpoint); 101 | return true; 102 | } catch (const boost::system::system_error& e) { 103 | return false; 104 | } 105 | } 106 | 107 | void CFootBotDiffusion::updateQueue() { 108 | if (q.empty()) { 109 | return; 110 | } 111 | auto first_act = q.front(); 112 | if (first_act.type == Action::MOVE) { 113 | q.pop_front(); 114 | while (not q.empty() and q.front().type == Action::MOVE) { 115 | auto next_act = q.front(); 116 | q.pop_front(); 117 | first_act.x = next_act.x; 118 | first_act.y = next_act.y; 119 | first_act.nodeIDS.insert(first_act.nodeIDS.end(), next_act.nodeIDS.begin(), next_act.nodeIDS.end()); 120 | } 121 | q.push_front(first_act); 122 | } 123 | } 124 | 125 | void CFootBotDiffusion::ControlStep() { 126 | if (not is_initialized) { 127 | if (is_port_open("127.0.0.1", port_number)) { 128 | client = std::make_shared("127.0.0.1", port_number); 129 | } else { 130 | return; 131 | } 132 | is_initialized = true; 133 | 134 | std::vector actions = client->call("init", robot_id).as>(); 135 | insertActions(actions); 136 | return; 137 | } 138 | Action a; 139 | a.type = Action::STOP; 140 | CQuaternion currOrient = m_pcPosSens->GetReading().Orientation; 141 | CRadians cZAngle, cYAngle, cXAngle; 142 | currOrient.ToEulerAngles(cZAngle, cYAngle, cXAngle); 143 | Real currAngle = ToDegrees(cZAngle).GetValue(); 144 | if (currAngle < 0.0) { 145 | currAngle = currAngle + 360.0; 146 | } 147 | Real left_v, right_v; 148 | CVector3 currPos = m_pcPosSens->GetReading().Position; 149 | if (count % 10 == 0) { 150 | std::vector updateActions = client->call("update", robot_id).as>(); 151 | if (updateActions.size() != 0) { 152 | insertActions(updateActions); 153 | } 154 | } 155 | count++; 156 | std::string receive_msg = ""; 157 | updateQueue(); 158 | while (!q.empty()) { 159 | a = q.front(); 160 | CVector3 currPos = m_pcPosSens->GetReading().Position; 161 | CVector3 targetPos = CVector3(a.x, a.y, 0.0f); 162 | 163 | if (a.type == Action::MOVE && ((currPos - targetPos).Length() < EPS) and (abs(prevVelocity_)) <= dt*m_fWheelVelocity) { 164 | a.type = Action::STOP; 165 | q.pop_front(); 166 | for (auto tmp_nodeId: a.nodeIDS) { 167 | receive_msg = client->call("receive_update", robot_id, tmp_nodeId).as(); 168 | } 169 | continue; 170 | } 171 | else if (a.type == Action::MOVE && ((currPos - targetPos).Length() + MOVE_DIS*(-static_cast(a.nodeIDS.size()) + 1)) < EPS) { 172 | if (robot_id == debug_id) { 173 | std::cout << "Action: " << a.type << ", Target Position: (" << a.x << ", " << a.y << ")" << 174 | ", Current Position: (" << currPos.GetX() << ", " << currPos.GetY() << "). Previous speed is: " 175 | << prevVelocity_ << std::endl; 176 | std::cout << "size of node id: " << a.nodeIDS.size() << std::endl; 177 | for (auto nodeId : a.nodeIDS) { 178 | std::cout << "Node ID: " << nodeId << std::endl; 179 | } 180 | } 181 | if (a.nodeIDS.size() > 1) { 182 | receive_msg = client->call("receive_update", robot_id, a.nodeIDS.front()).as(); 183 | q.front().nodeIDS.pop_front(); 184 | } 185 | if ((currPos - targetPos).Length() < EPS) { 186 | if ((abs(prevVelocity_)) <= dt*m_fWheelVelocity) { 187 | a.type = Action::STOP; 188 | q.pop_front(); 189 | for (auto tmp_nodeId: a.nodeIDS) { 190 | receive_msg = client->call("receive_update", robot_id, tmp_nodeId).as(); 191 | } 192 | continue; 193 | } 194 | } 195 | } 196 | else if (a.type == Action::TURN && angleDifference(currAngle, a.angle) < 0.1f) { 197 | a.type = Action::STOP; 198 | receive_msg = client->call("receive_update", robot_id, a.nodeIDS.front()).as(); 199 | q.pop_front(); 200 | continue; 201 | } 202 | break; 203 | } 204 | 205 | if (a.type == Action::MOVE) { 206 | CVector3 targetPos = CVector3(a.x, a.y, 0.0f); 207 | CVector3 currPos = m_pcPosSens->GetReading().Position; 208 | std::pair velocities = Move(targetPos, currPos, currAngle, 1.0f); 209 | left_v = velocities.first; 210 | right_v = velocities.second; 211 | } else if (a.type == Action::TURN) { 212 | std::pair turn_velocities = Turn(a.angle, currAngle, 1.0f); 213 | left_v = turn_velocities.first; 214 | right_v = turn_velocities.second; 215 | 216 | } else { 217 | setWheels(0.0f, 0.0f); 218 | left_v = 0.0f; 219 | right_v = 0.0f; 220 | } 221 | 222 | if (receive_msg == "end" || receive_msg == "exit") { 223 | client->call("update_finish_agent", robot_id, count); 224 | } 225 | if (receive_msg == "exit") { 226 | client->async_call("closeServer"); 227 | sleep(1); 228 | exit(0); 229 | } 230 | } 231 | 232 | 233 | std::pair CFootBotDiffusion::pidAngular(Real error) 234 | { 235 | integral_turn_error += error * dt; 236 | Real derivative = (error - prev_turn_error) / dt; 237 | prev_turn_error = error; 238 | 239 | Real output = kp_turn_ * error + ki_turn_ * integral_turn_error + kd_turn_ * derivative; 240 | output = std::clamp(output, -m_rotateWheelVelocity, m_rotateWheelVelocity); 241 | Real left_v = -output, right_v = output; 242 | return std::make_pair(left_v, right_v); 243 | } 244 | 245 | Real CFootBotDiffusion::pidLinear(Real error) 246 | { 247 | integral_move_error += error * dt; 248 | Real derivative = (error - prev_move_error) / dt; 249 | prev_move_error = error; 250 | Real desiredVelocity = kp_move_ * error + ki_move_ * integral_move_error + kd_move_ * derivative; 251 | return desiredVelocity; 252 | } 253 | 254 | std::pair CFootBotDiffusion::Turn(Real targetAngle, Real currAngle, Real tolerance = 1.0f) 255 | { 256 | Real error = normalizeAngle(targetAngle - currAngle); 257 | auto turn_v = pidAngular(error); 258 | Real left_v = turn_v.first; 259 | Real right_v = turn_v.second; 260 | 261 | setWheels(left_v, right_v); 262 | return std::make_pair(left_v, right_v); 263 | } 264 | 265 | inline Real toAngle(Real deltaX, Real deltaY) { 266 | Real targetAngle = std::atan2(deltaY, deltaX); 267 | Real tmp_angle = targetAngle/M_PI * 180.0 + 360; 268 | if (tmp_angle > 360) { 269 | tmp_angle -= 360; 270 | } 271 | assert(tmp_angle >= 0.0); 272 | return tmp_angle; 273 | } 274 | 275 | double CFootBotDiffusion::getReferenceSpeed(double dist) 276 | { 277 | int dist_flag = 0; 278 | if (dist < 0) { 279 | dist_flag = -1; 280 | } else if (dist > 0) { 281 | dist_flag = 1; 282 | } 283 | return dist_flag * std::min(0.9*sqrt(2*(m_linearAcceleration)*std::abs(dist)), m_fWheelVelocity); 284 | } 285 | 286 | std::pair CFootBotDiffusion::Move(CVector3& targetPos, CVector3& currPos, Real currAngle, Real tolerance = 1.0f) 287 | { 288 | Real deltaX = targetPos.GetX() - currPos.GetX(); 289 | Real deltaY = targetPos.GetY() - currPos.GetY(); 290 | Real distance = std::sqrt(deltaX * deltaX + deltaY * deltaY); 291 | Real targetAngle = toAngle(deltaX, deltaY); 292 | Real targetAngle2 = toAngle(-deltaX, -deltaY); 293 | 294 | Real angleError1 = normalizeAngle(targetAngle - currAngle); 295 | Real angleError2 = normalizeAngle(targetAngle2 - currAngle); 296 | Real angleError = 0.0; 297 | Real flag = 0.0; 298 | if (std::abs(angleError1) < std::abs(angleError2)) { 299 | angleError = angleError1; 300 | flag = 1.0; 301 | } else { 302 | angleError = angleError2; 303 | flag = -1.0; 304 | } 305 | 306 | Real refer_velocity = flag*getReferenceSpeed(distance); 307 | Real control_acc = pidLinear(refer_velocity - prevVelocity_); 308 | auto angularVelocity = pidAngular(angleError); 309 | 310 | Real maxDeltaV = m_linearAcceleration*dt; 311 | Real linearVelocity = prevVelocity_ + std::clamp(control_acc, - maxDeltaV, maxDeltaV); 312 | 313 | Real left_v_total = linearVelocity; 314 | Real right_v_total = linearVelocity; 315 | 316 | // Add angular velocity components 317 | left_v_total += (1/m_fWheelVelocity)*std::abs(linearVelocity)*angularVelocity.first; 318 | right_v_total += (1/m_fWheelVelocity)*std::abs(linearVelocity)*angularVelocity.second; 319 | 320 | // Check if either wheel exceeds the limit and scale down proportionally to preserve turning 321 | Real max_wheel_speed = std::max(std::abs(left_v_total), std::abs(right_v_total)); 322 | if (max_wheel_speed > m_fWheelVelocity) { 323 | Real scale_factor = m_fWheelVelocity / max_wheel_speed; 324 | left_v_total *= scale_factor; 325 | right_v_total *= scale_factor; 326 | } 327 | 328 | prevLeftVelocity_ = left_v_total; 329 | prevRightVelocity_ = right_v_total; 330 | prevVelocity_ = linearVelocity; 331 | 332 | setWheels(left_v_total, right_v_total); 333 | return std::make_pair(left_v_total, right_v_total); 334 | } 335 | 336 | Real CFootBotDiffusion::ChangeCoordinateFromMapToArgos(Real x) { 337 | if (x == 0) { 338 | return 0; 339 | } 340 | return -x; 341 | } 342 | 343 | Real CFootBotDiffusion::ChangeCoordinateFromArgosToMap(Real x) { 344 | if (x == 0) { 345 | return 0; 346 | } 347 | return -x; 348 | } 349 | 350 | REGISTER_CONTROLLER(CFootBotDiffusion, "footbot_diffusion_controller") 351 | -------------------------------------------------------------------------------- /example_paths_xy.txt: -------------------------------------------------------------------------------- 1 | Agent 0:(5,16,0)->(5,17,1)->(5,18,2)->(6,18,3)->(7,18,4)->(7,19,5)->(7,20,6)->(8,20,7)->(9,20,8)->(10,20,9)->(11,20,10)->(11,21,11)->(11,22,12)->(11,23,13)->(11,24,14)->(11,25,15)->(11,26,16)->(11,27,17)->(12,27,18)->(13,27,19)->(14,27,20)->(15,27,21)->(16,27,22)->(17,27,23)->(18,27,24)->(19,27,25)->(20,27,26)->(21,27,27)->(22,27,28)->(23,27,29)->(23,26,30)->(24,26,31)->(25,26,32)->(26,26,33)->(27,26,34)->(28,26,35)->(29,26,36)->(29,25,37)->(29,24,38)->(30,24,39)->(31,24,40)-> 2 | Agent 1:(21,29,0)->(22,29,1)->(22,28,2)->(22,27,3)->(22,26,4)->(22,25,5)->(23,25,6)->(24,25,7)->(24,24,8)->(25,24,9)->(25,23,10)->(25,22,11)->(24,22,12)-> 3 | Agent 2:(27,1,0)->(27,2,1)->(27,3,2)->(28,3,3)->(28,4,4)->(28,5,5)->(28,6,6)->(28,7,7)->(28,8,8)->(29,8,9)->(29,9,10)->(29,10,11)->(28,10,12)->(28,11,13)->(28,12,14)->(28,13,15)->(28,14,16)->(28,15,17)->(28,16,18)->(27,16,19)->(27,17,20)->(27,18,21)->(27,19,22)->(28,19,23)->(29,19,24)->(30,19,25)->(30,20,26)->(30,21,27)->(29,21,28)->(29,22,29)->(29,23,30)->(28,23,31)-> 4 | Agent 3:(20,14,0)->(19,14,1)->(18,14,2)->(18,15,3)->(18,16,4)->(17,16,5)->(17,17,6)->(16,17,7)->(16,18,8)->(15,18,9)->(15,19,10)->(15,20,11)->(15,21,12)->(14,21,13)->(13,21,14)->(12,21,15)->(11,21,16)->(11,22,17)->(11,23,18)->(11,24,19)->(11,25,20)->(11,26,21)->(11,27,22)->(12,27,23)->(13,27,24)->(14,27,25)->(15,27,26)->(16,27,27)->(16,28,28)-> 5 | Agent 4:(29,25,0)->(29,24,1)->(29,23,2)->(28,23,3)->(28,22,4)->(27,22,5)->(26,22,6)->(25,22,7)->(24,22,8)->(23,22,9)->(22,22,10)->(21,22,11)->(20,22,12)->(20,21,13)->(19,21,14)->(19,20,15)->(18,20,16)->(17,20,17)->(17,21,18)->(16,21,19)->(15,21,20)->(14,21,21)->(13,21,22)->(12,21,23)->(11,21,24)->(11,20,25)->(10,20,26)->(9,20,27)->(9,19,28)->(8,19,29)->(7,19,30)->(7,18,31)-> 6 | Agent 5:(25,8,0)->(25,9,1)->(24,9,2)->(23,9,3)->(22,9,4)->(21,9,5)->(21,10,6)->(20,10,7)->(19,10,8)->(18,10,9)->(17,10,10)->(16,10,11)->(16,9,12)->(15,9,13)->(14,9,14)->(13,9,15)->(12,9,16)->(11,9,17)->(10,9,18)->(9,9,19)->(8,9,20)->(7,9,21)->(6,9,22)->(5,9,23)->(5,8,24)-> 7 | Agent 6:(23,30,0)->(22,30,1)->(21,30,2)->(20,30,3)->(20,29,4)->(20,28,5)->(20,27,6)->(19,27,7)->(18,27,8)->(17,27,9)->(16,27,10)->(15,27,11)->(14,27,12)->(14,28,13)->(13,28,14)->(12,28,15)-> 8 | Agent 7:(20,23,0)->(20,24,1)->(20,25,2)->(21,25,3)->(22,25,4)->(23,25,5)->(23,26,6)->(23,27,7)->(23,28,8)->(24,28,9)->(25,28,10)-> 9 | Agent 8:(15,9,0)->(15,9,1)->(15,9,2)->(15,9,3)->(15,9,4)->(15,9,5)->(15,9,6)->(15,10,7)->(15,10,8)->(15,10,9)->(16,10,10)->(16,11,11)->(16,12,12)->(16,12,13)->(17,12,14)->(17,11,15)-> 10 | Agent 9:(11,7,0)->(10,7,1)->(9,7,2)->(8,7,3)->(8,6,4)->(7,6,5)->(7,5,6)->(7,4,7)->(6,4,8)->(5,4,9)->(5,5,10)->(4,5,11)->(3,5,12)->(3,4,13)->(3,3,14)->(2,3,15)->(1,3,16)->(0,3,17)-> 11 | Agent 10:(12,18,0)->(13,18,1)->(14,18,2)->(15,18,3)->(16,18,4)->(16,17,5)->(16,16,6)->(16,15,7)->(16,14,8)->(17,14,9)->(18,14,10)->(19,14,11)->(20,14,12)->(21,14,13)->(22,14,14)->(22,15,15)->(23,15,16)->(24,15,17)->(25,15,18)->(26,15,19)->(26,14,20)->(27,14,21)->(28,14,22)-> 12 | Agent 11:(30,30,0)->(29,30,1)->(28,30,2)->(28,29,3)->(28,28,4)->(27,28,5)->(27,27,6)->(27,26,7)->(26,26,8)->(25,26,9)->(24,26,10)->(23,26,11)->(23,25,12)->(22,25,13)->(21,25,14)->(20,25,15)->(20,24,16)->(20,23,17)->(20,22,18)->(20,21,19)->(19,21,20)->(19,20,21)->(18,20,22)->(17,20,23)-> 13 | Agent 12:(22,22,0)->(22,22,1)->(22,22,2)->(22,22,3)->(23,22,4)->(24,22,5)->(25,22,6)->(25,23,7)->(25,22,8)->(26,22,9)->(27,22,10)->(28,22,11)->(28,23,12)->(29,23,13)->(30,23,14)->(31,23,15)-> 14 | Agent 13:(3,27,0)->(3,26,1)->(4,26,2)->(5,26,3)->(5,25,4)->(5,24,5)->(6,24,6)->(6,23,7)->(6,22,8)->(6,21,9)->(6,20,10)->(7,20,11)->(8,20,12)->(9,20,13)->(10,20,14)->(11,20,15)->(11,19,16)->(12,19,17)->(12,18,18)->(12,17,19)->(12,16,20)->(12,15,21)->(12,14,22)->(12,13,23)->(13,13,24)->(14,13,25)->(14,12,26)->(14,11,27)->(15,11,28)->(15,10,29)->(15,9,30)->(15,8,31)->(15,7,32)->(15,6,33)->(16,6,34)->(16,5,35)->(17,5,36)->(18,5,37)->(18,4,38)->(19,4,39)->(19,3,40)->(19,2,41)->(20,2,42)->(21,2,43)->(22,2,44)->(22,1,45)->(23,1,46)->(24,1,47)->(24,0,48)-> 15 | Agent 14:(27,26,0)->(26,26,1)->(26,27,2)->(25,27,3)->(24,27,4)->(23,27,5)->(22,27,6)->(21,27,7)->(20,27,8)->(19,27,9)->(18,27,10)->(17,27,11)->(16,27,12)->(15,27,13)->(14,27,14)->(14,26,15)->(13,26,16)->(13,25,17)->(12,25,18)->(11,25,19)->(10,25,20)->(9,25,21)->(8,25,22)->(7,25,23)-> 16 | Agent 15:(0,9,0)->(0,8,1)->(0,7,2)->(0,6,3)->(0,5,4)->(1,5,5)->(2,5,6)->(3,5,7)->(3,4,8)->(4,4,9)->(5,4,10)->(6,4,11)->(7,4,12)->(8,4,13)->(8,3,14)->(9,3,15)->(10,3,16)->(11,3,17)->(12,3,18)->(13,3,19)->(14,3,20)->(15,3,21)->(15,2,22)->(16,2,23)->(17,2,24)->(18,2,25)->(19,2,26)->(19,3,27)->(20,3,28)->(21,3,29)->(22,3,30)->(23,3,31)->(24,3,32)->(25,3,33)->(26,3,34)->(27,3,35)->(28,3,36)->(28,4,37)->(29,4,38)-> 17 | Agent 16:(6,14,0)->(6,13,1)->(7,13,2)->(8,13,3)->(9,13,4)->(10,13,5)->(11,13,6)->(12,13,7)->(13,13,8)->(14,13,9)->(15,13,10)->(16,13,11)->(17,13,12)->(17,12,13)->(17,11,14)->(17,10,15)->(18,10,16)->(18,9,17)->(18,8,18)-> 18 | Agent 17:(24,20,0)->(24,19,1)->(25,19,2)->(25,18,3)->(26,18,4)->(27,18,5)->(27,17,6)->(28,17,7)-> 19 | Agent 18:(6,15,0)->(6,14,1)->(7,14,2)->(7,13,3)->(7,12,4)->(6,12,5)->(5,12,6)->(5,11,7)->(5,10,8)->(4,10,9)->(3,10,10)->(3,11,11)->(2,11,12)->(1,11,13)->(0,11,14)->(0,12,15)->(0,13,16)->(0,14,17)->(1,14,18)->(1,15,19)->(2,15,20)->(3,15,21)->(3,16,22)->(3,17,23)->(2,17,24)->(2,18,25)->(2,19,26)->(2,20,27)->(2,21,28)->(1,21,29)->(0,21,30)->(0,22,31)->(0,23,32)->(1,23,33)->(2,23,34)->(3,23,35)->(3,23,36)->(3,24,37)-> 20 | Agent 19:(17,19,0)->(17,19,1)->(17,19,2)->(17,19,3)->(17,19,4)->(17,19,5)->(17,19,6)->(17,19,7)->(17,19,8)->(17,19,9)->(17,19,10)->(17,19,11)->(17,19,12)->(17,19,13)->(17,19,14)->(17,19,15)->(17,19,16)->(17,19,17)->(17,20,18)->(16,20,19)->(16,21,20)->(15,21,21)->(14,21,22)->(13,21,23)->(12,21,24)->(11,21,25)-> 21 | Agent 20:(18,7,0)->(18,8,1)->(18,9,2)->(18,10,3)->(17,10,4)->(16,10,5)->(15,10,6)->(15,11,7)->(14,11,8)->(14,12,9)->(14,13,10)->(14,14,11)->(14,15,12)->(14,16,13)->(14,17,14)->(14,18,15)->(13,18,16)->(12,18,17)->(11,18,18)->(11,19,19)->(11,20,20)->(10,20,21)->(10,21,22)->(10,22,23)->(10,23,24)->(9,23,25)->(9,24,26)->(9,25,27)->(9,26,28)->(9,27,29)->(9,28,30)->(8,28,31)-> 22 | Agent 21:(4,15,0)->(5,15,1)->(6,15,2)->(7,15,3)->(7,14,4)->(7,13,5)->(7,12,6)->(7,11,7)->(7,10,8)-> 23 | Agent 22:(9,10,0)->(10,10,1)->(11,10,2)->(11,9,3)->(12,9,4)->(13,9,5)->(14,9,6)->(15,9,7)->(16,9,8)->(16,10,9)->(16,11,10)->(17,11,11)->(17,10,12)->(18,10,13)->(19,10,14)-> 24 | Agent 23:(0,27,0)->(0,26,1)->(0,25,2)->(1,25,3)->(2,25,4)->(2,24,5)->(2,23,6)->(3,23,7)->(3,22,8)->(3,21,9)->(4,21,10)->(5,21,11)->(5,20,12)->(6,20,13)->(7,20,14)->(7,19,15)->(7,18,16)->(7,17,17)->(7,16,18)->(8,16,19)->(9,16,20)->(9,15,21)->(9,14,22)->(9,13,23)->(9,12,24)->(9,11,25)->(9,10,26)->(9,9,27)->(10,9,28)->(10,8,29)->(11,8,30)->(11,7,31)->(12,7,32)->(13,7,33)->(14,7,34)->(15,7,35)->(16,7,36)->(17,7,37)->(18,7,38)->(19,7,39)->(20,7,40)->(20,6,41)->(21,6,42)->(22,6,43)->(23,6,44)->(24,6,45)->(25,6,46)->(26,6,47)-> 25 | Agent 24:(27,27,0)->(27,26,1)->(26,26,2)->(25,26,3)->(24,26,4)->(24,25,5)->(24,24,6)->(25,24,7)->(25,23,8)->(25,22,9)->(25,21,10)->(24,21,11)->(24,20,12)->(24,19,13)->(25,19,14)-> 26 | Agent 25:(31,27,0)->(30,27,1)->(30,26,2)->(30,25,3)->(29,25,4)->(29,24,5)->(29,23,6)->(29,22,7)->(29,21,8)->(30,21,9)->(30,20,10)->(30,19,11)->(29,19,12)->(28,19,13)->(28,18,14)->(27,18,15)->(27,17,16)->(27,16,17)->(27,15,18)->(27,14,19)->(27,13,20)->(27,12,21)->(28,12,22)->(29,12,23)->(29,11,24)->(29,10,25)->(30,10,26)->(30,9,27)->(30,8,28)->(30,7,29)->(30,6,30)->(29,6,31)->(29,5,32)->(29,4,33)->(30,4,34)->(31,4,35)-> 27 | Agent 26:(13,0,0)->(13,1,1)->(13,2,2)->(13,3,3)->(13,4,4)->(13,5,5)->(13,6,6)->(14,6,7)->(15,6,8)->(15,7,9)->(15,8,10)->(15,9,11)->(15,10,12)->(15,11,13)->(16,11,14)->(16,12,15)->(16,13,16)->(16,14,17)->(17,14,18)->(18,14,19)->(19,14,20)->(20,14,21)->(21,14,22)->(21,15,23)->(21,16,24)->(21,17,25)->(21,18,26)->(20,18,27)->(20,19,28)->(20,20,29)->(20,21,30)->(20,22,31)->(20,23,32)->(19,23,33)-> 28 | Agent 27:(13,20,0)->(13,21,1)->(13,22,2)->(14,22,3)->(14,23,4)->(15,23,5)->(15,24,6)->(15,24,7)->(15,24,8)->(15,24,9)->(15,24,10)->(15,24,11)->(15,24,12)->(15,24,13)->(15,24,14)->(15,24,15)->(15,24,16)->(15,24,17)->(15,24,18)->(15,24,19)->(15,24,20)->(15,24,21)->(15,24,22)->(15,24,23)->(15,25,24)->(14,25,25)-> 29 | Agent 28:(21,20,0)->(21,20,1)->(21,20,2)->(21,20,3)->(21,20,4)->(21,20,5)->(21,20,6)->(21,20,7)->(21,20,8)->(21,20,9)->(21,20,10)->(21,20,11)->(21,20,12)->(21,20,13)->(21,20,14)->(21,20,15)->(21,20,16)->(21,20,17)->(21,20,18)->(21,20,19)->(21,20,20)->(21,20,21)->(21,20,22)->(21,20,23)->(21,20,24)->(21,20,25)->(21,20,26)->(21,20,27)->(21,20,28)->(21,20,29)->(21,20,30)->(21,20,31)->(21,20,32)->(21,20,33)->(21,20,34)->(21,20,35)->(21,20,36)->(21,20,37)->(20,20,38)->(20,21,39)->(20,22,40)->(21,22,41)->(22,22,42)->(23,22,43)-> 30 | Agent 29:(3,18,0)->(3,17,1)->(4,17,2)->(5,17,3)->(6,17,4)->(7,17,5)->(7,16,6)->(8,16,7)->(9,16,8)->(10,16,9)->(11,16,10)->(12,16,11)->(13,16,12)->(13,16,13)->(14,16,14)->(14,15,15)->(14,14,16)->(15,14,17)->(16,14,18)->(16,13,19)->(17,13,20)->(17,12,21)->(18,12,22)->(19,12,23)->(20,12,24)->(21,12,25)->(22,12,26)->(23,12,27)->(23,11,28)->(23,10,29)->(23,9,30)-> 31 | Agent 30:(26,28,0)->(25,28,1)->(25,28,2)->(24,28,3)->(24,28,4)->(24,27,5)->(24,26,6)->(23,26,7)->(22,26,8)->(22,25,9)->(21,25,10)->(20,25,11)->(20,24,12)->(20,23,13)->(20,22,14)->(20,21,15)->(20,20,16)->(20,19,17)->(20,18,18)->(21,18,19)->(21,17,20)->(22,17,21)->(22,16,22)->(22,15,23)->(23,15,24)-> 32 | Agent 31:(16,13,0)->(17,13,1)->(17,12,2)->(18,12,3)->(19,12,4)->(20,12,5)->(21,12,6)->(21,11,7)->(21,10,8)->(21,9,9)->(21,8,10)->(22,8,11)->(23,8,12)->(23,7,13)->(24,7,14)->(25,7,15)->(25,6,16)->(25,5,17)->(25,4,18)->(25,3,19)->(25,2,20)-> 33 | Agent 32:(4,27,0)->(4,26,1)->(4,25,2)->(4,24,3)->(5,24,4)->(6,24,5)->(7,24,6)->(8,24,7)->(8,23,8)-> 34 | Agent 33:(8,11,0)->(9,11,1)->(9,12,2)->(9,13,3)->(10,13,4)->(11,13,5)->(12,13,6)->(13,13,7)->(14,13,8)->(14,14,9)->(15,14,10)->(16,14,11)->(17,14,12)->(18,14,13)->(19,14,14)->(20,14,15)->(21,14,16)->(21,15,17)->(22,15,18)->(23,15,19)->(24,15,20)->(25,15,21)->(26,15,22)->(27,15,23)->(28,15,24)->(29,15,25)->(30,15,26)-> 35 | Agent 34:(31,1,0)->(30,1,1)->(29,1,2)->(28,1,3)->(27,1,4)->(26,1,5)->(26,2,6)->(25,2,7)->(25,3,8)->(24,3,9)->(23,3,10)->(22,3,11)->(22,4,12)->(21,4,13)->(20,4,14)->(19,4,15)->(19,5,16)->(18,5,17)->(17,5,18)->(16,5,19)->(16,6,20)->(15,6,21)->(15,7,22)->(14,7,23)->(14,8,24)->(13,8,25)->(13,9,26)-> 36 | Agent 35:(31,22,0)->(31,23,1)->(31,24,2)->(30,24,3)->(30,25,4)->(30,26,5)->(29,26,6)->(28,26,7)->(27,26,8)->(27,27,9)->(26,27,10)->(25,27,11)->(24,27,12)->(23,27,13)->(22,27,14)->(21,27,15)->(20,27,16)->(19,27,17)->(18,27,18)->(18,26,19)->(17,26,20)->(16,26,21)->(15,26,22)->(15,25,23)->(14,25,24)->(13,25,25)->(12,25,26)->(12,24,27)->(11,24,28)->(10,24,29)->(9,24,30)->(8,24,31)->(7,24,32)->(6,24,33)->(5,24,34)->(4,24,35)->(3,24,36)->(2,24,37)->(1,24,38)->(0,24,39)->(0,23,40)-> 37 | Agent 36:(15,13,0)->(16,13,1)->(17,13,2)->(17,12,3)->(18,12,4)->(19,12,5)->(19,11,6)-> 38 | Agent 37:(7,26,0)->(7,25,1)->(8,25,2)->(8,24,3)->(9,24,4)->(10,24,5)->(11,24,6)->(11,24,7)->(12,24,8)->(13,24,9)->(14,24,10)-> 39 | Agent 38:(25,24,0)->(24,24,1)->(24,25,2)->(23,25,3)->(23,26,4)->(23,26,5)->(23,27,6)->(22,27,7)->(21,27,8)->(20,27,9)->(19,27,10)->(18,27,11)->(17,27,12)->(16,27,13)->(16,26,14)->(15,26,15)->(14,26,16)->(13,26,17)-> 40 | Agent 39:(8,21,0)->(8,22,1)->(9,22,2)->(9,23,3)->(10,23,4)->(11,23,5)->(12,23,6)->(12,24,7)->(13,24,8)->(14,24,9)->(14,25,10)->(14,26,11)->(15,26,12)->(16,26,13)->(17,26,14)->(18,26,15)->(19,26,16)->(19,26,17)->(19,27,18)->(20,27,19)->(21,27,20)->(22,27,21)->(23,27,22)->(24,27,23)->(25,27,24)->(26,27,25)->(27,27,26)->(28,27,27)-> 41 | Agent 40:(21,3,0)->(20,3,1)->(20,2,2)->(19,2,3)->(18,2,4)->(18,1,5)->(17,1,6)->(16,1,7)->(15,1,8)->(14,1,9)->(13,1,10)->(12,1,11)->(11,1,12)->(10,1,13)->(9,1,14)->(9,2,15)->(8,2,16)->(7,2,17)->(6,2,18)-> 42 | Agent 41:(10,30,0)->(10,31,1)->(11,31,2)->(12,31,3)->(13,31,4)->(13,30,5)->(13,29,6)->(12,29,7)->(12,28,8)->(12,27,9)->(12,26,10)-> 43 | Agent 42:(0,7,0)->(0,6,1)->(1,6,2)->(2,6,3)->(3,6,4)->(4,6,5)->(4,7,6)->(4,8,7)->(5,8,8)->(5,9,9)->(5,10,10)->(5,11,11)->(5,12,12)->(5,13,13)->(6,13,14)->(7,13,15)->(8,13,16)->(9,13,17)->(10,13,18)->(11,13,19)->(12,13,20)->(13,13,21)->(14,13,22)->(15,13,23)->(16,13,24)->(16,14,25)->(17,14,26)->(18,14,27)->(19,14,28)->(20,14,29)->(21,14,30)->(21,15,31)->(21,16,32)->(21,17,33)->(21,18,34)->(21,19,35)->(20,19,36)->(20,20,37)->(20,21,38)->(20,22,39)->(21,22,40)->(22,22,41)->(23,22,42)->(23,23,43)-> 44 | Agent 43:(30,12,0)->(29,12,1)->(29,11,2)->(28,11,3)->(27,11,4)->(26,11,5)->(25,11,6)->(25,12,7)->(24,12,8)->(23,12,9)->(22,12,10)->(21,12,11)->(20,12,12)->(19,12,13)->(18,12,14)->(17,12,15)->(17,13,16)->(16,13,17)->(15,13,18)->(15,14,19)->(14,14,20)->(14,15,21)->(14,16,22)->(14,17,23)->(14,18,24)->(13,18,25)->(12,18,26)->(11,18,27)->(11,19,28)->(11,20,29)->(10,20,30)->(9,20,31)->(8,20,32)->(7,20,33)->(6,20,34)->(6,21,35)->(5,21,36)->(4,21,37)->(3,21,38)->(3,22,39)-> 45 | Agent 44:(28,4,0)->(28,5,1)->(28,6,2)->(28,7,3)->(28,8,4)->(29,8,5)->(29,9,6)->(29,10,7)->(29,11,8)->(28,11,9)->(27,11,10)->(26,11,11)->(25,11,12)->(24,11,13)->(23,11,14)->(23,12,15)->(23,13,16)->(22,13,17)->(22,14,18)->(21,14,19)->(21,15,20)->(21,16,21)->(21,17,22)->(21,18,23)->(21,19,24)->(20,19,25)->(20,20,26)->(20,21,27)->(20,22,28)->(20,23,29)->(20,24,30)->(20,25,31)->(19,25,32)->(18,25,33)->(18,26,34)->(18,27,35)->(17,27,36)->(16,27,37)->(16,26,38)-> 46 | Agent 45:(25,4,0)->(25,5,1)->(24,5,2)->(23,5,3)->(23,6,4)->(23,7,5)->(23,8,6)->(23,9,7)->(23,10,8)->(23,11,9)->(23,12,10)->(23,13,11)->(22,13,12)->(21,13,13)->(21,14,14)->(21,15,15)->(21,16,16)->(21,17,17)->(21,18,18)->(21,19,19)->(20,19,20)->(20,20,21)->(20,21,22)->(20,22,23)->(20,23,24)->(19,23,25)->(19,24,26)->(18,24,27)->(18,25,28)->(17,25,29)->(17,26,30)-> 47 | Agent 46:(18,24,0)->(19,24,1)->(20,24,2)->(20,23,3)->(20,22,4)->(20,21,5)->(20,20,6)->(20,19,7)->(20,18,8)->(21,18,9)->(21,17,10)->(21,16,11)->(21,15,12)->(22,15,13)->(23,15,14)->(24,15,15)->(24,14,16)->(25,14,17)->(25,13,18)->(25,12,19)->(25,11,20)->(26,11,21)->(27,11,22)->(28,11,23)->(28,10,24)->(28,10,25)->(29,10,26)->(29,9,27)->(29,8,28)->(28,8,29)->(28,7,30)-> 48 | Agent 47:(26,22,0)->(25,22,1)->(24,22,2)->(23,22,3)->(23,23,4)->(23,22,5)->(24,22,6)->(23,22,7)->(22,22,8)->(21,22,9)->(20,22,10)->(20,21,11)->(20,20,12)->(19,20,13)->(18,20,14)->(17,20,15)->(16,20,16)->(16,21,17)->(15,21,18)->(14,21,19)->(13,21,20)->(12,21,21)->(11,21,22)->(11,22,23)->(10,22,24)->(10,23,25)->(10,24,26)->(10,25,27)-> 49 | Agent 48:(23,7,0)->(23,6,1)->(24,6,2)->(24,5,3)->(25,5,4)->(26,5,5)->(26,4,6)->(26,3,7)->(27,3,8)->(28,3,9)->(28,4,10)->(29,4,11)->(30,4,12)->(30,3,13)->(30,2,14)-> 50 | Agent 49:(24,30,0)->(24,29,1)->(24,28,2)->(24,27,3)->(23,27,4)->(22,27,5)->(22,26,6)->(22,25,7)->(21,25,8)->(20,25,9)->(20,24,10)->(20,23,11)->(20,23,12)->(20,22,13)->(20,21,14)->(20,20,15)->(19,20,16)->(18,20,17)->(18,20,18)->(17,20,19)->(17,19,20)->(17,18,21)->(16,18,22)->(16,17,23)->(16,16,24)->(16,15,25)->(16,14,26)->(16,13,27)->(16,12,28)->(16,11,29)-> 51 | -------------------------------------------------------------------------------- /example_paths_yx.txt: -------------------------------------------------------------------------------- 1 | Agent 0:(16,5,0)->(17,5,1)->(18,5,2)->(18,6,3)->(18,7,4)->(19,7,5)->(20,7,6)->(20,8,7)->(20,9,8)->(20,10,9)->(20,11,10)->(21,11,11)->(22,11,12)->(23,11,13)->(24,11,14)->(25,11,15)->(26,11,16)->(27,11,17)->(27,12,18)->(27,13,19)->(27,14,20)->(27,15,21)->(27,16,22)->(27,17,23)->(27,18,24)->(27,19,25)->(27,20,26)->(27,21,27)->(27,22,28)->(27,23,29)->(26,23,30)->(26,24,31)->(26,25,32)->(26,26,33)->(26,27,34)->(26,28,35)->(26,29,36)->(25,29,37)->(24,29,38)->(24,30,39)->(24,31,40)-> 2 | Agent 1:(29,21,0)->(29,22,1)->(28,22,2)->(27,22,3)->(26,22,4)->(25,22,5)->(25,23,6)->(25,24,7)->(24,24,8)->(24,25,9)->(23,25,10)->(22,25,11)->(22,24,12)-> 3 | Agent 2:(1,27,0)->(2,27,1)->(3,27,2)->(3,28,3)->(4,28,4)->(5,28,5)->(6,28,6)->(7,28,7)->(8,28,8)->(8,29,9)->(9,29,10)->(10,29,11)->(10,28,12)->(11,28,13)->(12,28,14)->(13,28,15)->(14,28,16)->(15,28,17)->(16,28,18)->(16,27,19)->(17,27,20)->(18,27,21)->(19,27,22)->(19,28,23)->(19,29,24)->(19,30,25)->(20,30,26)->(21,30,27)->(21,29,28)->(22,29,29)->(23,29,30)->(23,28,31)-> 4 | Agent 3:(14,20,0)->(14,19,1)->(14,18,2)->(15,18,3)->(16,18,4)->(16,17,5)->(17,17,6)->(17,16,7)->(18,16,8)->(18,15,9)->(19,15,10)->(20,15,11)->(21,15,12)->(21,14,13)->(21,13,14)->(21,12,15)->(21,11,16)->(22,11,17)->(23,11,18)->(24,11,19)->(25,11,20)->(26,11,21)->(27,11,22)->(27,12,23)->(27,13,24)->(27,14,25)->(27,15,26)->(27,16,27)->(28,16,28)-> 5 | Agent 4:(25,29,0)->(24,29,1)->(23,29,2)->(23,28,3)->(22,28,4)->(22,27,5)->(22,26,6)->(22,25,7)->(22,24,8)->(22,23,9)->(22,22,10)->(22,21,11)->(22,20,12)->(21,20,13)->(21,19,14)->(20,19,15)->(20,18,16)->(20,17,17)->(21,17,18)->(21,16,19)->(21,15,20)->(21,14,21)->(21,13,22)->(21,12,23)->(21,11,24)->(20,11,25)->(20,10,26)->(20,9,27)->(19,9,28)->(19,8,29)->(19,7,30)->(18,7,31)-> 6 | Agent 5:(8,25,0)->(9,25,1)->(9,24,2)->(9,23,3)->(9,22,4)->(9,21,5)->(10,21,6)->(10,20,7)->(10,19,8)->(10,18,9)->(10,17,10)->(10,16,11)->(9,16,12)->(9,15,13)->(9,14,14)->(9,13,15)->(9,12,16)->(9,11,17)->(9,10,18)->(9,9,19)->(9,8,20)->(9,7,21)->(9,6,22)->(9,5,23)->(8,5,24)-> 7 | Agent 6:(30,23,0)->(30,22,1)->(30,21,2)->(30,20,3)->(29,20,4)->(28,20,5)->(27,20,6)->(27,19,7)->(27,18,8)->(27,17,9)->(27,16,10)->(27,15,11)->(27,14,12)->(28,14,13)->(28,13,14)->(28,12,15)-> 8 | Agent 7:(23,20,0)->(24,20,1)->(25,20,2)->(25,21,3)->(25,22,4)->(25,23,5)->(26,23,6)->(27,23,7)->(28,23,8)->(28,24,9)->(28,25,10)-> 9 | Agent 8:(9,15,0)->(9,15,1)->(9,15,2)->(9,15,3)->(9,15,4)->(9,15,5)->(9,15,6)->(10,15,7)->(10,15,8)->(10,15,9)->(10,16,10)->(11,16,11)->(12,16,12)->(12,16,13)->(12,17,14)->(11,17,15)-> 10 | Agent 9:(7,11,0)->(7,10,1)->(7,9,2)->(7,8,3)->(6,8,4)->(6,7,5)->(5,7,6)->(4,7,7)->(4,6,8)->(4,5,9)->(5,5,10)->(5,4,11)->(5,3,12)->(4,3,13)->(3,3,14)->(3,2,15)->(3,1,16)->(3,0,17)-> 11 | Agent 10:(18,12,0)->(18,13,1)->(18,14,2)->(18,15,3)->(18,16,4)->(17,16,5)->(16,16,6)->(15,16,7)->(14,16,8)->(14,17,9)->(14,18,10)->(14,19,11)->(14,20,12)->(14,21,13)->(14,22,14)->(15,22,15)->(15,23,16)->(15,24,17)->(15,25,18)->(15,26,19)->(14,26,20)->(14,27,21)->(14,28,22)-> 12 | Agent 11:(30,30,0)->(30,29,1)->(30,28,2)->(29,28,3)->(28,28,4)->(28,27,5)->(27,27,6)->(26,27,7)->(26,26,8)->(26,25,9)->(26,24,10)->(26,23,11)->(25,23,12)->(25,22,13)->(25,21,14)->(25,20,15)->(24,20,16)->(23,20,17)->(22,20,18)->(21,20,19)->(21,19,20)->(20,19,21)->(20,18,22)->(20,17,23)-> 13 | Agent 12:(22,22,0)->(22,22,1)->(22,22,2)->(22,22,3)->(22,23,4)->(22,24,5)->(22,25,6)->(23,25,7)->(22,25,8)->(22,26,9)->(22,27,10)->(22,28,11)->(23,28,12)->(23,29,13)->(23,30,14)->(23,31,15)-> 14 | Agent 13:(27,3,0)->(26,3,1)->(26,4,2)->(26,5,3)->(25,5,4)->(24,5,5)->(24,6,6)->(23,6,7)->(22,6,8)->(21,6,9)->(20,6,10)->(20,7,11)->(20,8,12)->(20,9,13)->(20,10,14)->(20,11,15)->(19,11,16)->(19,12,17)->(18,12,18)->(17,12,19)->(16,12,20)->(15,12,21)->(14,12,22)->(13,12,23)->(13,13,24)->(13,14,25)->(12,14,26)->(11,14,27)->(11,15,28)->(10,15,29)->(9,15,30)->(8,15,31)->(7,15,32)->(6,15,33)->(6,16,34)->(5,16,35)->(5,17,36)->(5,18,37)->(4,18,38)->(4,19,39)->(3,19,40)->(2,19,41)->(2,20,42)->(2,21,43)->(2,22,44)->(1,22,45)->(1,23,46)->(1,24,47)->(0,24,48)-> 15 | Agent 14:(26,27,0)->(26,26,1)->(27,26,2)->(27,25,3)->(27,24,4)->(27,23,5)->(27,22,6)->(27,21,7)->(27,20,8)->(27,19,9)->(27,18,10)->(27,17,11)->(27,16,12)->(27,15,13)->(27,14,14)->(26,14,15)->(26,13,16)->(25,13,17)->(25,12,18)->(25,11,19)->(25,10,20)->(25,9,21)->(25,8,22)->(25,7,23)-> 16 | Agent 15:(9,0,0)->(8,0,1)->(7,0,2)->(6,0,3)->(5,0,4)->(5,1,5)->(5,2,6)->(5,3,7)->(4,3,8)->(4,4,9)->(4,5,10)->(4,6,11)->(4,7,12)->(4,8,13)->(3,8,14)->(3,9,15)->(3,10,16)->(3,11,17)->(3,12,18)->(3,13,19)->(3,14,20)->(3,15,21)->(2,15,22)->(2,16,23)->(2,17,24)->(2,18,25)->(2,19,26)->(3,19,27)->(3,20,28)->(3,21,29)->(3,22,30)->(3,23,31)->(3,24,32)->(3,25,33)->(3,26,34)->(3,27,35)->(3,28,36)->(4,28,37)->(4,29,38)-> 17 | Agent 16:(14,6,0)->(13,6,1)->(13,7,2)->(13,8,3)->(13,9,4)->(13,10,5)->(13,11,6)->(13,12,7)->(13,13,8)->(13,14,9)->(13,15,10)->(13,16,11)->(13,17,12)->(12,17,13)->(11,17,14)->(10,17,15)->(10,18,16)->(9,18,17)->(8,18,18)-> 18 | Agent 17:(20,24,0)->(19,24,1)->(19,25,2)->(18,25,3)->(18,26,4)->(18,27,5)->(17,27,6)->(17,28,7)-> 19 | Agent 18:(15,6,0)->(14,6,1)->(14,7,2)->(13,7,3)->(12,7,4)->(12,6,5)->(12,5,6)->(11,5,7)->(10,5,8)->(10,4,9)->(10,3,10)->(11,3,11)->(11,2,12)->(11,1,13)->(11,0,14)->(12,0,15)->(13,0,16)->(14,0,17)->(14,1,18)->(15,1,19)->(15,2,20)->(15,3,21)->(16,3,22)->(17,3,23)->(17,2,24)->(18,2,25)->(19,2,26)->(20,2,27)->(21,2,28)->(21,1,29)->(21,0,30)->(22,0,31)->(23,0,32)->(23,1,33)->(23,2,34)->(23,3,35)->(23,3,36)->(24,3,37)-> 20 | Agent 19:(19,17,0)->(19,17,1)->(19,17,2)->(19,17,3)->(19,17,4)->(19,17,5)->(19,17,6)->(19,17,7)->(19,17,8)->(19,17,9)->(19,17,10)->(19,17,11)->(19,17,12)->(19,17,13)->(19,17,14)->(19,17,15)->(19,17,16)->(19,17,17)->(20,17,18)->(20,16,19)->(21,16,20)->(21,15,21)->(21,14,22)->(21,13,23)->(21,12,24)->(21,11,25)-> 21 | Agent 20:(7,18,0)->(8,18,1)->(9,18,2)->(10,18,3)->(10,17,4)->(10,16,5)->(10,15,6)->(11,15,7)->(11,14,8)->(12,14,9)->(13,14,10)->(14,14,11)->(15,14,12)->(16,14,13)->(17,14,14)->(18,14,15)->(18,13,16)->(18,12,17)->(18,11,18)->(19,11,19)->(20,11,20)->(20,10,21)->(21,10,22)->(22,10,23)->(23,10,24)->(23,9,25)->(24,9,26)->(25,9,27)->(26,9,28)->(27,9,29)->(28,9,30)->(28,8,31)-> 22 | Agent 21:(15,4,0)->(15,5,1)->(15,6,2)->(15,7,3)->(14,7,4)->(13,7,5)->(12,7,6)->(11,7,7)->(10,7,8)-> 23 | Agent 22:(10,9,0)->(10,10,1)->(10,11,2)->(9,11,3)->(9,12,4)->(9,13,5)->(9,14,6)->(9,15,7)->(9,16,8)->(10,16,9)->(11,16,10)->(11,17,11)->(10,17,12)->(10,18,13)->(10,19,14)-> 24 | Agent 23:(27,0,0)->(26,0,1)->(25,0,2)->(25,1,3)->(25,2,4)->(24,2,5)->(23,2,6)->(23,3,7)->(22,3,8)->(21,3,9)->(21,4,10)->(21,5,11)->(20,5,12)->(20,6,13)->(20,7,14)->(19,7,15)->(18,7,16)->(17,7,17)->(16,7,18)->(16,8,19)->(16,9,20)->(15,9,21)->(14,9,22)->(13,9,23)->(12,9,24)->(11,9,25)->(10,9,26)->(9,9,27)->(9,10,28)->(8,10,29)->(8,11,30)->(7,11,31)->(7,12,32)->(7,13,33)->(7,14,34)->(7,15,35)->(7,16,36)->(7,17,37)->(7,18,38)->(7,19,39)->(7,20,40)->(6,20,41)->(6,21,42)->(6,22,43)->(6,23,44)->(6,24,45)->(6,25,46)->(6,26,47)-> 25 | Agent 24:(27,27,0)->(26,27,1)->(26,26,2)->(26,25,3)->(26,24,4)->(25,24,5)->(24,24,6)->(24,25,7)->(23,25,8)->(22,25,9)->(21,25,10)->(21,24,11)->(20,24,12)->(19,24,13)->(19,25,14)-> 26 | Agent 25:(27,31,0)->(27,30,1)->(26,30,2)->(25,30,3)->(25,29,4)->(24,29,5)->(23,29,6)->(22,29,7)->(21,29,8)->(21,30,9)->(20,30,10)->(19,30,11)->(19,29,12)->(19,28,13)->(18,28,14)->(18,27,15)->(17,27,16)->(16,27,17)->(15,27,18)->(14,27,19)->(13,27,20)->(12,27,21)->(12,28,22)->(12,29,23)->(11,29,24)->(10,29,25)->(10,30,26)->(9,30,27)->(8,30,28)->(7,30,29)->(6,30,30)->(6,29,31)->(5,29,32)->(4,29,33)->(4,30,34)->(4,31,35)-> 27 | Agent 26:(0,13,0)->(1,13,1)->(2,13,2)->(3,13,3)->(4,13,4)->(5,13,5)->(6,13,6)->(6,14,7)->(6,15,8)->(7,15,9)->(8,15,10)->(9,15,11)->(10,15,12)->(11,15,13)->(11,16,14)->(12,16,15)->(13,16,16)->(14,16,17)->(14,17,18)->(14,18,19)->(14,19,20)->(14,20,21)->(14,21,22)->(15,21,23)->(16,21,24)->(17,21,25)->(18,21,26)->(18,20,27)->(19,20,28)->(20,20,29)->(21,20,30)->(22,20,31)->(23,20,32)->(23,19,33)-> 28 | Agent 27:(20,13,0)->(21,13,1)->(22,13,2)->(22,14,3)->(23,14,4)->(23,15,5)->(24,15,6)->(24,15,7)->(24,15,8)->(24,15,9)->(24,15,10)->(24,15,11)->(24,15,12)->(24,15,13)->(24,15,14)->(24,15,15)->(24,15,16)->(24,15,17)->(24,15,18)->(24,15,19)->(24,15,20)->(24,15,21)->(24,15,22)->(24,15,23)->(25,15,24)->(25,14,25)-> 29 | Agent 28:(20,21,0)->(20,21,1)->(20,21,2)->(20,21,3)->(20,21,4)->(20,21,5)->(20,21,6)->(20,21,7)->(20,21,8)->(20,21,9)->(20,21,10)->(20,21,11)->(20,21,12)->(20,21,13)->(20,21,14)->(20,21,15)->(20,21,16)->(20,21,17)->(20,21,18)->(20,21,19)->(20,21,20)->(20,21,21)->(20,21,22)->(20,21,23)->(20,21,24)->(20,21,25)->(20,21,26)->(20,21,27)->(20,21,28)->(20,21,29)->(20,21,30)->(20,21,31)->(20,21,32)->(20,21,33)->(20,21,34)->(20,21,35)->(20,21,36)->(20,21,37)->(20,20,38)->(21,20,39)->(22,20,40)->(22,21,41)->(22,22,42)->(22,23,43)-> 30 | Agent 29:(18,3,0)->(17,3,1)->(17,4,2)->(17,5,3)->(17,6,4)->(17,7,5)->(16,7,6)->(16,8,7)->(16,9,8)->(16,10,9)->(16,11,10)->(16,12,11)->(16,13,12)->(16,13,13)->(16,14,14)->(15,14,15)->(14,14,16)->(14,15,17)->(14,16,18)->(13,16,19)->(13,17,20)->(12,17,21)->(12,18,22)->(12,19,23)->(12,20,24)->(12,21,25)->(12,22,26)->(12,23,27)->(11,23,28)->(10,23,29)->(9,23,30)-> 31 | Agent 30:(28,26,0)->(28,25,1)->(28,25,2)->(28,24,3)->(28,24,4)->(27,24,5)->(26,24,6)->(26,23,7)->(26,22,8)->(25,22,9)->(25,21,10)->(25,20,11)->(24,20,12)->(23,20,13)->(22,20,14)->(21,20,15)->(20,20,16)->(19,20,17)->(18,20,18)->(18,21,19)->(17,21,20)->(17,22,21)->(16,22,22)->(15,22,23)->(15,23,24)-> 32 | Agent 31:(13,16,0)->(13,17,1)->(12,17,2)->(12,18,3)->(12,19,4)->(12,20,5)->(12,21,6)->(11,21,7)->(10,21,8)->(9,21,9)->(8,21,10)->(8,22,11)->(8,23,12)->(7,23,13)->(7,24,14)->(7,25,15)->(6,25,16)->(5,25,17)->(4,25,18)->(3,25,19)->(2,25,20)-> 33 | Agent 32:(27,4,0)->(26,4,1)->(25,4,2)->(24,4,3)->(24,5,4)->(24,6,5)->(24,7,6)->(24,8,7)->(23,8,8)-> 34 | Agent 33:(11,8,0)->(11,9,1)->(12,9,2)->(13,9,3)->(13,10,4)->(13,11,5)->(13,12,6)->(13,13,7)->(13,14,8)->(14,14,9)->(14,15,10)->(14,16,11)->(14,17,12)->(14,18,13)->(14,19,14)->(14,20,15)->(14,21,16)->(15,21,17)->(15,22,18)->(15,23,19)->(15,24,20)->(15,25,21)->(15,26,22)->(15,27,23)->(15,28,24)->(15,29,25)->(15,30,26)-> 35 | Agent 34:(1,31,0)->(1,30,1)->(1,29,2)->(1,28,3)->(1,27,4)->(1,26,5)->(2,26,6)->(2,25,7)->(3,25,8)->(3,24,9)->(3,23,10)->(3,22,11)->(4,22,12)->(4,21,13)->(4,20,14)->(4,19,15)->(5,19,16)->(5,18,17)->(5,17,18)->(5,16,19)->(6,16,20)->(6,15,21)->(7,15,22)->(7,14,23)->(8,14,24)->(8,13,25)->(9,13,26)-> 36 | Agent 35:(22,31,0)->(23,31,1)->(24,31,2)->(24,30,3)->(25,30,4)->(26,30,5)->(26,29,6)->(26,28,7)->(26,27,8)->(27,27,9)->(27,26,10)->(27,25,11)->(27,24,12)->(27,23,13)->(27,22,14)->(27,21,15)->(27,20,16)->(27,19,17)->(27,18,18)->(26,18,19)->(26,17,20)->(26,16,21)->(26,15,22)->(25,15,23)->(25,14,24)->(25,13,25)->(25,12,26)->(24,12,27)->(24,11,28)->(24,10,29)->(24,9,30)->(24,8,31)->(24,7,32)->(24,6,33)->(24,5,34)->(24,4,35)->(24,3,36)->(24,2,37)->(24,1,38)->(24,0,39)->(23,0,40)-> 37 | Agent 36:(13,15,0)->(13,16,1)->(13,17,2)->(12,17,3)->(12,18,4)->(12,19,5)->(11,19,6)-> 38 | Agent 37:(26,7,0)->(25,7,1)->(25,8,2)->(24,8,3)->(24,9,4)->(24,10,5)->(24,11,6)->(24,11,7)->(24,12,8)->(24,13,9)->(24,14,10)-> 39 | Agent 38:(24,25,0)->(24,24,1)->(25,24,2)->(25,23,3)->(26,23,4)->(26,23,5)->(27,23,6)->(27,22,7)->(27,21,8)->(27,20,9)->(27,19,10)->(27,18,11)->(27,17,12)->(27,16,13)->(26,16,14)->(26,15,15)->(26,14,16)->(26,13,17)-> 40 | Agent 39:(21,8,0)->(22,8,1)->(22,9,2)->(23,9,3)->(23,10,4)->(23,11,5)->(23,12,6)->(24,12,7)->(24,13,8)->(24,14,9)->(25,14,10)->(26,14,11)->(26,15,12)->(26,16,13)->(26,17,14)->(26,18,15)->(26,19,16)->(26,19,17)->(27,19,18)->(27,20,19)->(27,21,20)->(27,22,21)->(27,23,22)->(27,24,23)->(27,25,24)->(27,26,25)->(27,27,26)->(27,28,27)-> 41 | Agent 40:(3,21,0)->(3,20,1)->(2,20,2)->(2,19,3)->(2,18,4)->(1,18,5)->(1,17,6)->(1,16,7)->(1,15,8)->(1,14,9)->(1,13,10)->(1,12,11)->(1,11,12)->(1,10,13)->(1,9,14)->(2,9,15)->(2,8,16)->(2,7,17)->(2,6,18)-> 42 | Agent 41:(30,10,0)->(31,10,1)->(31,11,2)->(31,12,3)->(31,13,4)->(30,13,5)->(29,13,6)->(29,12,7)->(28,12,8)->(27,12,9)->(26,12,10)-> 43 | Agent 42:(7,0,0)->(6,0,1)->(6,1,2)->(6,2,3)->(6,3,4)->(6,4,5)->(7,4,6)->(8,4,7)->(8,5,8)->(9,5,9)->(10,5,10)->(11,5,11)->(12,5,12)->(13,5,13)->(13,6,14)->(13,7,15)->(13,8,16)->(13,9,17)->(13,10,18)->(13,11,19)->(13,12,20)->(13,13,21)->(13,14,22)->(13,15,23)->(13,16,24)->(14,16,25)->(14,17,26)->(14,18,27)->(14,19,28)->(14,20,29)->(14,21,30)->(15,21,31)->(16,21,32)->(17,21,33)->(18,21,34)->(19,21,35)->(19,20,36)->(20,20,37)->(21,20,38)->(22,20,39)->(22,21,40)->(22,22,41)->(22,23,42)->(23,23,43)-> 44 | Agent 43:(12,30,0)->(12,29,1)->(11,29,2)->(11,28,3)->(11,27,4)->(11,26,5)->(11,25,6)->(12,25,7)->(12,24,8)->(12,23,9)->(12,22,10)->(12,21,11)->(12,20,12)->(12,19,13)->(12,18,14)->(12,17,15)->(13,17,16)->(13,16,17)->(13,15,18)->(14,15,19)->(14,14,20)->(15,14,21)->(16,14,22)->(17,14,23)->(18,14,24)->(18,13,25)->(18,12,26)->(18,11,27)->(19,11,28)->(20,11,29)->(20,10,30)->(20,9,31)->(20,8,32)->(20,7,33)->(20,6,34)->(21,6,35)->(21,5,36)->(21,4,37)->(21,3,38)->(22,3,39)-> 45 | Agent 44:(4,28,0)->(5,28,1)->(6,28,2)->(7,28,3)->(8,28,4)->(8,29,5)->(9,29,6)->(10,29,7)->(11,29,8)->(11,28,9)->(11,27,10)->(11,26,11)->(11,25,12)->(11,24,13)->(11,23,14)->(12,23,15)->(13,23,16)->(13,22,17)->(14,22,18)->(14,21,19)->(15,21,20)->(16,21,21)->(17,21,22)->(18,21,23)->(19,21,24)->(19,20,25)->(20,20,26)->(21,20,27)->(22,20,28)->(23,20,29)->(24,20,30)->(25,20,31)->(25,19,32)->(25,18,33)->(26,18,34)->(27,18,35)->(27,17,36)->(27,16,37)->(26,16,38)-> 46 | Agent 45:(4,25,0)->(5,25,1)->(5,24,2)->(5,23,3)->(6,23,4)->(7,23,5)->(8,23,6)->(9,23,7)->(10,23,8)->(11,23,9)->(12,23,10)->(13,23,11)->(13,22,12)->(13,21,13)->(14,21,14)->(15,21,15)->(16,21,16)->(17,21,17)->(18,21,18)->(19,21,19)->(19,20,20)->(20,20,21)->(21,20,22)->(22,20,23)->(23,20,24)->(23,19,25)->(24,19,26)->(24,18,27)->(25,18,28)->(25,17,29)->(26,17,30)-> 47 | Agent 46:(24,18,0)->(24,19,1)->(24,20,2)->(23,20,3)->(22,20,4)->(21,20,5)->(20,20,6)->(19,20,7)->(18,20,8)->(18,21,9)->(17,21,10)->(16,21,11)->(15,21,12)->(15,22,13)->(15,23,14)->(15,24,15)->(14,24,16)->(14,25,17)->(13,25,18)->(12,25,19)->(11,25,20)->(11,26,21)->(11,27,22)->(11,28,23)->(10,28,24)->(10,28,25)->(10,29,26)->(9,29,27)->(8,29,28)->(8,28,29)->(7,28,30)-> 48 | Agent 47:(22,26,0)->(22,25,1)->(22,24,2)->(22,23,3)->(23,23,4)->(22,23,5)->(22,24,6)->(22,23,7)->(22,22,8)->(22,21,9)->(22,20,10)->(21,20,11)->(20,20,12)->(20,19,13)->(20,18,14)->(20,17,15)->(20,16,16)->(21,16,17)->(21,15,18)->(21,14,19)->(21,13,20)->(21,12,21)->(21,11,22)->(22,11,23)->(22,10,24)->(23,10,25)->(24,10,26)->(25,10,27)-> 49 | Agent 48:(7,23,0)->(6,23,1)->(6,24,2)->(5,24,3)->(5,25,4)->(5,26,5)->(4,26,6)->(3,26,7)->(3,27,8)->(3,28,9)->(4,28,10)->(4,29,11)->(4,30,12)->(3,30,13)->(2,30,14)-> 50 | Agent 49:(30,24,0)->(29,24,1)->(28,24,2)->(27,24,3)->(27,23,4)->(27,22,5)->(26,22,6)->(25,22,7)->(25,21,8)->(25,20,9)->(24,20,10)->(23,20,11)->(23,20,12)->(22,20,13)->(21,20,14)->(20,20,15)->(20,19,16)->(20,18,17)->(20,18,18)->(20,17,19)->(19,17,20)->(18,17,21)->(18,16,22)->(17,16,23)->(16,16,24)->(15,16,25)->(14,16,26)->(13,16,27)->(12,16,28)->(11,16,29)-> 51 | -------------------------------------------------------------------------------- /random-32-32-20-random-1.scen: -------------------------------------------------------------------------------- 1 | version 1 2 | 7 random-32-32-20.map 32 32 5 16 31 24 31.31370850 3 | 2 random-32-32-20.map 32 32 21 29 24 22 10.24264069 4 | 6 random-32-32-20.map 32 32 27 1 28 23 27.48528137 5 | 4 random-32-32-20.map 32 32 20 14 16 28 17.07106781 6 | 6 random-32-32-20.map 32 32 29 25 7 18 27.48528137 7 | 5 random-32-32-20.map 32 32 25 8 5 8 22.82842712 8 | 3 random-32-32-20.map 32 32 23 30 12 28 13.24264069 9 | 2 random-32-32-20.map 32 32 20 23 25 28 8.24264069 10 | 0 random-32-32-20.map 32 32 15 9 17 11 2.82842712 11 | 3 random-32-32-20.map 32 32 11 7 0 3 13.82842712 12 | 4 random-32-32-20.map 32 32 12 18 28 14 19.65685425 13 | 4 random-32-32-20.map 32 32 30 30 17 20 19.48528137 14 | 2 random-32-32-20.map 32 32 22 22 31 23 9.41421356 15 | 10 random-32-32-20.map 32 32 3 27 24 0 40.38477631 16 | 5 random-32-32-20.map 32 32 27 26 7 25 21.24264069 17 | 8 random-32-32-20.map 32 32 0 9 29 4 33.89949493 18 | 4 random-32-32-20.map 32 32 6 14 18 8 16.48528137 19 | 1 random-32-32-20.map 32 32 24 20 28 17 5.82842712 20 | 2 random-32-32-20.map 32 32 6 15 3 24 11.41421356 21 | 1 random-32-32-20.map 32 32 17 19 11 21 7.41421356 22 | 6 random-32-32-20.map 32 32 18 7 8 28 27.48528137 23 | 1 random-32-32-20.map 32 32 4 15 7 10 7.41421356 24 | 2 random-32-32-20.map 32 32 9 10 19 10 10.82842712 25 | 9 random-32-32-20.map 32 32 0 27 26 6 39.97056274 26 | 3 random-32-32-20.map 32 32 27 27 25 19 12.82842712 27 | 7 random-32-32-20.map 32 32 31 27 31 4 30.65685425 28 | 7 random-32-32-20.map 32 32 13 0 19 23 28.07106781 29 | 1 random-32-32-20.map 32 32 13 20 14 25 5.41421356 30 | 1 random-32-32-20.map 32 32 21 20 23 22 6.00000000 31 | 6 random-32-32-20.map 32 32 3 18 23 9 25.48528137 32 | 4 random-32-32-20.map 32 32 26 28 23 15 19.07106781 33 | 4 random-32-32-20.map 32 32 16 13 25 2 17.07106781 34 | 1 random-32-32-20.map 32 32 4 27 8 23 6.82842712 35 | 6 random-32-32-20.map 32 32 8 11 30 15 24.82842712 36 | 5 random-32-32-20.map 32 32 31 1 13 9 21.31370850 37 | 8 random-32-32-20.map 32 32 31 22 0 23 35.31370850 38 | 1 random-32-32-20.map 32 32 15 13 19 11 5.41421356 39 | 2 random-32-32-20.map 32 32 7 26 14 24 8.41421356 40 | 3 random-32-32-20.map 32 32 25 24 13 26 13.41421356 41 | 5 random-32-32-20.map 32 32 8 21 28 27 23.07106781 42 | 4 random-32-32-20.map 32 32 21 3 6 2 16.24264069 43 | 2 random-32-32-20.map 32 32 10 30 12 26 9.41421356 44 | 8 random-32-32-20.map 32 32 0 7 23 23 33.72792206 45 | 8 random-32-32-20.map 32 32 30 12 3 22 33.72792206 46 | 7 random-32-32-20.map 32 32 28 4 16 26 30.72792206 47 | 6 random-32-32-20.map 32 32 25 4 17 26 25.31370850 48 | 6 random-32-32-20.map 32 32 18 24 28 7 24.89949493 49 | 4 random-32-32-20.map 32 32 26 22 10 25 18.07106781 50 | 2 random-32-32-20.map 32 32 23 7 30 2 11.07106781 51 | 6 random-32-32-20.map 32 32 24 30 16 11 24.65685425 52 | 7 random-32-32-20.map 32 32 24 27 13 4 29.89949493 53 | 3 random-32-32-20.map 32 32 1 8 11 14 13.65685425 54 | 3 random-32-32-20.map 32 32 28 0 25 13 14.24264069 55 | 6 random-32-32-20.map 32 32 20 2 21 25 24.82842712 56 | 3 random-32-32-20.map 32 32 16 14 27 17 12.82842712 57 | 8 random-32-32-20.map 32 32 2 1 20 21 32.14213562 58 | 8 random-32-32-20.map 32 32 5 25 31 7 35.79898987 59 | 7 random-32-32-20.map 32 32 25 5 17 30 29.72792206 60 | 5 random-32-32-20.map 32 32 26 25 27 10 21.65685425 61 | 8 random-32-32-20.map 32 32 0 8 21 28 33.97056274 62 | 2 random-32-32-20.map 32 32 11 19 1 18 11.82842712 63 | 5 random-32-32-20.map 32 32 13 25 25 9 22.14213562 64 | 3 random-32-32-20.map 32 32 17 23 22 12 14.24264069 65 | 3 random-32-32-20.map 32 32 17 21 14 9 13.82842712 66 | 5 random-32-32-20.map 32 32 31 19 20 5 21.48528137 67 | 6 random-32-32-20.map 32 32 2 2 11 26 27.72792206 68 | 4 random-32-32-20.map 32 32 6 4 23 8 19.24264069 69 | 7 random-32-32-20.map 32 32 5 24 29 10 31.55634918 70 | 2 random-32-32-20.map 32 32 8 30 14 28 8.24264069 71 | 8 random-32-32-20.map 32 32 4 0 20 25 35.14213562 72 | 4 random-32-32-20.map 32 32 24 19 14 30 18.07106781 73 | 6 random-32-32-20.map 32 32 2 15 23 28 27.55634918 74 | 3 random-32-32-20.map 32 32 17 2 3 2 14.82842712 75 | 2 random-32-32-20.map 32 32 1 19 8 14 9.07106781 76 | 3 random-32-32-20.map 32 32 16 23 24 15 13.65685425 77 | 3 random-32-32-20.map 32 32 20 10 30 8 13.07106781 78 | 2 random-32-32-20.map 32 32 12 15 14 7 10.24264069 79 | 4 random-32-32-20.map 32 32 31 9 27 22 18.89949493 80 | 2 random-32-32-20.map 32 32 3 11 4 19 10.41421356 81 | 9 random-32-32-20.map 32 32 12 31 27 3 36.55634918 82 | 4 random-32-32-20.map 32 32 15 3 2 13 18.31370850 83 | 4 random-32-32-20.map 32 32 3 13 19 17 18.82842712 84 | 2 random-32-32-20.map 32 32 11 2 14 12 11.82842712 85 | 8 random-32-32-20.map 32 32 18 0 17 31 34.48528137 86 | 8 random-32-32-20.map 32 32 14 27 25 0 34.14213562 87 | 5 random-32-32-20.map 32 32 5 22 22 24 20.65685425 88 | 5 random-32-32-20.map 32 32 13 24 29 16 21.89949493 89 | 4 random-32-32-20.map 32 32 7 7 10 22 16.82842712 90 | 5 random-32-32-20.map 32 32 15 24 17 6 20.82842712 91 | 5 random-32-32-20.map 32 32 21 13 23 31 20.82842712 92 | 3 random-32-32-20.map 32 32 2 30 2 20 12.82842712 93 | 4 random-32-32-20.map 32 32 19 24 29 11 18.89949493 94 | 9 random-32-32-20.map 32 32 27 20 2 3 36.14213562 95 | 3 random-32-32-20.map 32 32 7 13 16 17 12.41421356 96 | 4 random-32-32-20.map 32 32 29 21 24 6 19.65685425 97 | 1 random-32-32-20.map 32 32 6 21 1 22 6.00000000 98 | 6 random-32-32-20.map 32 32 7 20 27 25 24.89949493 99 | 3 random-32-32-20.map 32 32 10 20 5 31 14.82842712 100 | 5 random-32-32-20.map 32 32 1 28 14 17 23.07106781 101 | 1 random-32-32-20.map 32 32 7 19 2 21 6.41421356 102 | 8 random-32-32-20.map 32 32 31 8 11 31 35.38477631 103 | 9 random-32-32-20.map 32 32 25 30 8 5 36.72792206 104 | 1 random-32-32-20.map 32 32 31 30 26 27 6.24264069 105 | 7 random-32-32-20.map 32 32 8 1 9 28 28.24264069 106 | 2 random-32-32-20.map 32 32 22 17 20 8 9.82842712 107 | 8 random-32-32-20.map 32 32 10 31 4 4 32.07106781 108 | 5 random-32-32-20.map 32 32 20 7 11 22 21.65685425 109 | 8 random-32-32-20.map 32 32 16 0 4 26 32.72792206 110 | 4 random-32-32-20.map 32 32 20 19 29 31 18.07106781 111 | 5 random-32-32-20.map 32 32 18 27 31 12 23.31370850 112 | 5 random-32-32-20.map 32 32 1 6 2 24 23.24264069 113 | 8 random-32-32-20.map 32 32 4 30 28 16 33.55634918 114 | 5 random-32-32-20.map 32 32 17 10 3 21 21.48528137 115 | 4 random-32-32-20.map 32 32 17 16 30 6 19.48528137 116 | 7 random-32-32-20.map 32 32 11 27 26 4 30.38477631 117 | 5 random-32-32-20.map 32 32 3 3 18 12 20.48528137 118 | 3 random-32-32-20.map 32 32 5 17 9 4 15.82842712 119 | 6 random-32-32-20.map 32 32 22 6 0 0 25.65685425 120 | 8 random-32-32-20.map 32 32 23 27 3 7 33.55634918 121 | 8 random-32-32-20.map 32 32 29 26 12 4 34.89949493 122 | 1 random-32-32-20.map 32 32 11 6 8 6 5.00000000 123 | 2 random-32-32-20.map 32 32 17 12 11 17 9.24264069 124 | 6 random-32-32-20.map 32 32 13 28 5 6 27.31370850 125 | 6 random-32-32-20.map 32 32 29 19 17 3 24.72792206 126 | 5 random-32-32-20.map 32 32 22 15 31 3 20.07106781 127 | 3 random-32-32-20.map 32 32 14 13 7 24 15.65685425 128 | 4 random-32-32-20.map 32 32 15 0 29 0 16.48528137 129 | 3 random-32-32-20.map 32 32 12 13 9 2 13.65685425 130 | 7 random-32-32-20.map 32 32 28 18 3 17 29.31370850 131 | 2 random-32-32-20.map 32 32 25 15 27 8 11.24264069 132 | 2 random-32-32-20.map 32 32 16 29 11 20 11.65685425 133 | 5 random-32-32-20.map 32 32 21 10 8 22 22.07106781 134 | 4 random-32-32-20.map 32 32 12 27 11 8 19.41421356 135 | 5 random-32-32-20.map 32 32 28 29 24 11 23.07106781 136 | 3 random-32-32-20.map 32 32 5 4 1 15 13.24264069 137 | 7 random-32-32-20.map 32 32 24 21 0 12 31.72792206 138 | 2 random-32-32-20.map 32 32 24 13 24 5 8.82842712 139 | 8 random-32-32-20.map 32 32 26 16 4 29 33.14213562 140 | 1 random-32-32-20.map 32 32 5 9 7 5 6.00000000 141 | 8 random-32-32-20.map 32 32 29 8 3 0 32.24264069 142 | 9 random-32-32-20.map 32 32 0 26 31 10 39.97056274 143 | 0 random-32-32-20.map 32 32 14 1 12 1 2.00000000 144 | 4 random-32-32-20.map 32 32 27 24 27 15 18.07106781 145 | 2 random-32-32-20.map 32 32 15 30 12 21 11.41421356 146 | 4 random-32-32-20.map 32 32 25 3 23 19 18.24264069 147 | 4 random-32-32-20.map 32 32 15 10 6 20 17.24264069 148 | 6 random-32-32-20.map 32 32 0 16 19 0 27.97056274 149 | 7 random-32-32-20.map 32 32 28 8 6 10 28.48528137 150 | 3 random-32-32-20.map 32 32 8 24 20 20 13.65685425 151 | 5 random-32-32-20.map 32 32 27 18 15 1 23.72792206 152 | 3 random-32-32-20.map 32 32 4 5 16 3 12.82842712 153 | 4 random-32-32-20.map 32 32 0 25 12 14 19.48528137 154 | 5 random-32-32-20.map 32 32 30 25 21 9 22.89949493 155 | 6 random-32-32-20.map 32 32 10 9 16 30 24.07106781 156 | 2 random-32-32-20.map 32 32 23 13 16 15 8.41421356 157 | 7 random-32-32-20.map 32 32 0 2 16 22 29.55634918 158 | 4 random-32-32-20.map 32 32 19 21 6 23 16.65685425 159 | 0 random-32-32-20.map 32 32 17 7 16 4 3.41421356 160 | 4 random-32-32-20.map 32 32 8 0 1 14 18.07106781 161 | 0 random-32-32-20.map 32 32 3 4 2 6 2.41421356 162 | 3 random-32-32-20.map 32 32 29 2 18 2 13.24264069 163 | 2 random-32-32-20.map 32 32 27 11 27 19 8.00000000 164 | 2 random-32-32-20.map 32 32 19 15 26 11 9.24264069 165 | 4 random-32-32-20.map 32 32 4 21 17 25 17.24264069 166 | 7 random-32-32-20.map 32 32 26 31 0 21 31.31370850 167 | 2 random-32-32-20.map 32 32 16 9 9 15 11.24264069 168 | 2 random-32-32-20.map 32 32 24 29 15 26 10.82842712 169 | 2 random-32-32-20.map 32 32 23 18 31 15 9.24264069 170 | 6 random-32-32-20.map 32 32 26 30 7 16 27.14213562 171 | 2 random-32-32-20.map 32 32 31 20 25 27 11.24264069 172 | 6 random-32-32-20.map 32 32 29 15 10 1 27.14213562 173 | 2 random-32-32-20.map 32 32 7 27 14 21 10.65685425 174 | 2 random-32-32-20.map 32 32 25 22 30 19 8.00000000 175 | 1 random-32-32-20.map 32 32 14 11 21 12 7.41421356 176 | 2 random-32-32-20.map 32 32 14 6 6 3 10.41421356 177 | 1 random-32-32-20.map 32 32 11 15 15 11 7.41421356 178 | 5 random-32-32-20.map 32 32 30 0 12 8 21.31370850 179 | 1 random-32-32-20.map 32 32 7 14 10 8 7.24264069 180 | 4 random-32-32-20.map 32 32 21 5 16 21 19.24264069 181 | 2 random-32-32-20.map 32 32 11 11 9 3 9.65685425 182 | 4 random-32-32-20.map 32 32 22 25 7 28 16.24264069 183 | 5 random-32-32-20.map 32 32 22 13 9 0 20.72792206 184 | 2 random-32-32-20.map 32 32 12 29 14 18 11.82842712 185 | 1 random-32-32-20.map 32 32 7 31 6 25 7.82842712 186 | 3 random-32-32-20.map 32 32 9 26 17 18 13.65685425 187 | 2 random-32-32-20.map 32 32 8 8 1 16 11.48528137 188 | 1 random-32-32-20.map 32 32 10 11 12 7 4.82842712 189 | 3 random-32-32-20.map 32 32 20 29 26 18 15.48528137 190 | 4 random-32-32-20.map 32 32 6 11 21 8 17.07106781 191 | 8 random-32-32-20.map 32 32 22 2 6 28 34.97056274 192 | 3 random-32-32-20.map 32 32 22 31 14 23 13.65685425 193 | 4 random-32-32-20.map 32 32 28 1 12 6 18.07106781 194 | 5 random-32-32-20.map 32 32 3 25 20 31 21.82842712 195 | 7 random-32-32-20.map 32 32 22 26 23 1 29.65685425 196 | 6 random-32-32-20.map 32 32 31 11 9 9 25.07106781 197 | 4 random-32-32-20.map 32 32 14 15 6 29 18.48528137 198 | 6 random-32-32-20.map 32 32 8 20 23 6 26.07106781 199 | 9 random-32-32-20.map 32 32 30 7 3 23 37.38477631 200 | 2 random-32-32-20.map 32 32 14 29 6 26 10.65685425 201 | 6 random-32-32-20.map 32 32 2 19 23 10 26.48528137 202 | 6 random-32-32-20.map 32 32 15 22 26 1 26.72792206 203 | 7 random-32-32-20.map 32 32 29 24 19 3 28.89949493 204 | 1 random-32-32-20.map 32 32 28 15 21 14 7.41421356 205 | 3 random-32-32-20.map 32 32 8 2 21 6 15.82842712 206 | 5 random-32-32-20.map 32 32 19 26 3 16 21.31370850 207 | 2 random-32-32-20.map 32 32 19 13 10 15 11.24264069 208 | 3 random-32-32-20.map 32 32 25 6 28 19 14.24264069 209 | 4 random-32-32-20.map 32 32 21 19 11 9 18.82842712 210 | 5 random-32-32-20.map 32 32 19 14 5 1 22.89949493 211 | 4 random-32-32-20.map 32 32 18 26 30 14 19.31370850 212 | 8 random-32-32-20.map 32 32 4 7 31 13 32.07106781 213 | 2 random-32-32-20.map 32 32 12 16 16 10 9.41421356 214 | 5 random-32-32-20.map 32 32 15 25 4 10 21.31370850 215 | 4 random-32-32-20.map 32 32 4 3 18 6 16.41421356 216 | 2 random-32-32-20.map 32 32 1 5 0 13 9.24264069 217 | 3 random-32-32-20.map 32 32 11 18 1 11 14.07106781 218 | 5 random-32-32-20.map 32 32 4 25 16 12 21.48528137 219 | 8 random-32-32-20.map 32 32 3 29 5 2 34.65685425 220 | 7 random-32-32-20.map 32 32 29 13 5 28 31.97056274 221 | 1 random-32-32-20.map 32 32 11 13 4 11 7.82842712 222 | 1 random-32-32-20.map 32 32 26 3 25 7 4.41421356 223 | 5 random-32-32-20.map 32 32 1 10 9 25 20.07106781 224 | 3 random-32-32-20.map 32 32 13 16 20 24 13.82842712 225 | 6 random-32-32-20.map 32 32 30 27 4 24 27.24264069 226 | 5 random-32-32-20.map 32 32 6 31 19 19 20.31370850 227 | 8 random-32-32-20.map 32 32 28 28 20 0 35.07106781 228 | 5 random-32-32-20.map 32 32 29 30 15 18 22.48528137 229 | 5 random-32-32-20.map 32 32 8 9 7 29 21.24264069 230 | 11 random-32-32-20.map 32 32 0 24 30 3 44.79898987 231 | 3 random-32-32-20.map 32 32 19 8 28 5 14.24264069 232 | 5 random-32-32-20.map 32 32 11 24 31 21 23.48528137 233 | 3 random-32-32-20.map 32 32 30 9 21 16 13.07106781 234 | 8 random-32-32-20.map 32 32 23 12 1 27 33.14213562 235 | 2 random-32-32-20.map 32 32 4 16 7 8 9.82842712 236 | 5 random-32-32-20.map 32 32 6 12 26 9 22.07106781 237 | 2 random-32-32-20.map 32 32 2 17 4 23 8.00000000 238 | 1 random-32-32-20.map 32 32 5 15 3 12 6.41421356 239 | 2 random-32-32-20.map 32 32 1 12 5 5 9.24264069 240 | 0 random-32-32-20.map 32 32 9 8 7 11 3.82842712 241 | 2 random-32-32-20.map 32 32 13 18 6 24 10.65685425 242 | 6 random-32-32-20.map 32 32 10 28 2 5 26.31370850 243 | 6 random-32-32-20.map 32 32 18 18 5 0 25.72792206 244 | 5 random-32-32-20.map 32 32 13 1 5 20 22.89949493 245 | 3 random-32-32-20.map 32 32 14 26 1 24 13.82842712 246 | 3 random-32-32-20.map 32 32 2 26 15 19 15.89949493 247 | 4 random-32-32-20.map 32 32 8 25 24 31 19.07106781 248 | 5 random-32-32-20.map 32 32 24 9 7 17 22.07106781 249 | 7 random-32-32-20.map 32 32 3 1 6 27 28.65685425 250 | 7 random-32-32-20.map 32 32 16 24 28 3 28.31370850 251 | 10 random-32-32-20.map 32 32 31 0 0 20 41.04163055 252 | 4 random-32-32-20.map 32 32 9 12 25 10 18.24264069 253 | 6 random-32-32-20.map 32 32 4 8 2 28 26.82842712 254 | 4 random-32-32-20.map 32 32 12 0 24 7 16.07106781 255 | 2 random-32-32-20.map 32 32 23 11 23 3 8.00000000 256 | 6 random-32-32-20.map 32 32 10 24 31 14 26.89949493 257 | 2 random-32-32-20.map 32 32 25 18 20 18 11.82842712 258 | 3 random-32-32-20.map 32 32 8 7 17 1 12.65685425 259 | 2 random-32-32-20.map 32 32 8 3 9 14 11.41421356 260 | 3 random-32-32-20.map 32 32 16 6 27 12 14.07106781 261 | 7 random-32-32-20.map 32 32 19 30 10 10 28.07106781 262 | 4 random-32-32-20.map 32 32 24 26 12 19 19.24264069 263 | 6 random-32-32-20.map 32 32 5 19 28 22 26.24264069 264 | 2 random-32-32-20.map 32 32 4 17 11 12 10.24264069 265 | 7 random-32-32-20.map 32 32 19 7 27 31 29.89949493 266 | 4 random-32-32-20.map 32 32 24 28 29 14 17.48528137 267 | 9 random-32-32-20.map 32 32 2 29 11 0 36.14213562 268 | 3 random-32-32-20.map 32 32 12 2 24 3 13.24264069 269 | 4 random-32-32-20.map 32 32 15 23 12 9 16.41421356 270 | 4 random-32-32-20.map 32 32 13 31 7 15 18.48528137 271 | 8 random-32-32-20.map 32 32 29 29 22 3 32.89949493 272 | 7 random-32-32-20.map 32 32 20 4 25 31 31.07106781 273 | 6 random-32-32-20.map 32 32 25 17 5 21 27.07106781 274 | 8 random-32-32-20.map 32 32 15 7 2 31 34.07106781 275 | 3 random-32-32-20.map 32 32 2 8 13 11 14.00000000 276 | 3 random-32-32-20.map 32 32 21 2 7 4 14.82842712 277 | 8 random-32-32-20.map 32 32 26 29 5 10 32.97056274 278 | 2 random-32-32-20.map 32 32 30 23 28 20 8.41421356 279 | 5 random-32-32-20.map 32 32 26 26 29 9 22.48528137 280 | 8 random-32-32-20.map 32 32 29 1 4 18 34.38477631 281 | 4 random-32-32-20.map 32 32 25 29 26 20 17.65685425 282 | 5 random-32-32-20.map 32 32 20 12 6 22 21.65685425 283 | 4 random-32-32-20.map 32 32 0 22 15 20 16.65685425 284 | 7 random-32-32-20.map 32 32 19 2 25 26 29.65685425 285 | 3 random-32-32-20.map 32 32 25 23 17 22 12.41421356 286 | 2 random-32-32-20.map 32 32 5 11 10 5 11.24264069 287 | 3 random-32-32-20.map 32 32 27 16 23 4 13.65685425 288 | 7 random-32-32-20.map 32 32 24 24 3 15 28.14213562 289 | 5 random-32-32-20.map 32 32 25 14 10 27 22.14213562 290 | 4 random-32-32-20.map 32 32 9 13 16 1 16.07106781 291 | 7 random-32-32-20.map 32 32 29 23 18 1 30.89949493 292 | 5 random-32-32-20.map 32 32 10 4 13 22 21.82842712 293 | 6 random-32-32-20.map 32 32 24 18 7 12 25.24264069 294 | 5 random-32-32-20.map 32 32 20 28 5 18 20.89949493 295 | 1 random-32-32-20.map 32 32 0 4 1 9 5.41421356 296 | 2 random-32-32-20.map 32 32 31 2 22 0 11.82842712 297 | 3 random-32-32-20.map 32 32 11 3 8 16 15.65685425 298 | 9 random-32-32-20.map 32 32 13 3 28 30 36.72792206 299 | 6 random-32-32-20.map 32 32 3 9 18 29 27.97056274 300 | 3 random-32-32-20.map 32 32 12 25 3 19 12.65685425 301 | 9 random-32-32-20.map 32 32 2 0 19 25 36.97056274 302 | 1 random-32-32-20.map 32 32 12 24 18 25 7.24264069 303 | 9 random-32-32-20.map 32 32 28 6 2 25 39.97056274 304 | 3 random-32-32-20.map 32 32 19 5 7 2 14.41421356 305 | 4 random-32-32-20.map 32 32 10 7 26 5 17.41421356 306 | 2 random-32-32-20.map 32 32 12 5 3 6 10.82842712 307 | 5 random-32-32-20.map 32 32 29 27 9 23 21.65685425 308 | 3 random-32-32-20.map 32 32 9 7 22 8 13.41421356 309 | 4 random-32-32-20.map 32 32 9 16 9 31 19.24264069 310 | 5 random-32-32-20.map 32 32 3 26 7 6 22.24264069 311 | 6 random-32-32-20.map 32 32 25 12 9 29 26.55634918 312 | 7 random-32-32-20.map 32 32 27 29 21 4 30.89949493 313 | 8 random-32-32-20.map 32 32 31 31 30 4 35.07106781 314 | 4 random-32-32-20.map 32 32 24 1 21 17 19.24264069 315 | 3 random-32-32-20.map 32 32 13 2 21 11 14.65685425 316 | 4 random-32-32-20.map 32 32 12 3 17 17 16.07106781 317 | 3 random-32-32-20.map 32 32 14 19 26 15 15.65685425 318 | 2 random-32-32-20.map 32 32 17 8 13 13 10.41421356 319 | 6 random-32-32-20.map 32 32 20 30 1 21 25.65685425 320 | 2 random-32-32-20.map 32 32 11 4 2 7 10.24264069 321 | 4 random-32-32-20.map 32 32 8 10 22 4 16.48528137 322 | 3 random-32-32-20.map 32 32 7 9 10 23 15.82842712 323 | 3 random-32-32-20.map 32 32 6 18 17 27 15.89949493 324 | 4 random-32-32-20.map 32 32 14 22 13 6 17.00000000 325 | 4 random-32-32-20.map 32 32 18 15 18 28 16.65685425 326 | 4 random-32-32-20.map 32 32 1 29 9 18 18.07106781 327 | 2 random-32-32-20.map 32 32 20 9 22 1 8.82842712 328 | 5 random-32-32-20.map 32 32 9 11 8 29 20.07106781 329 | 3 random-32-32-20.map 32 32 23 25 9 27 14.82842712 330 | 3 random-32-32-20.map 32 32 18 22 19 12 12.41421356 331 | 0 random-32-32-20.map 32 32 12 23 9 24 3.41421356 332 | 4 random-32-32-20.map 32 32 30 10 24 25 19.48528137 333 | 2 random-32-32-20.map 32 32 27 14 21 18 8.82842712 334 | 0 random-32-32-20.map 32 32 17 5 15 6 2.41421356 335 | 3 random-32-32-20.map 32 32 1 0 6 9 12.24264069 336 | 8 random-32-32-20.map 32 32 22 30 0 11 33.38477631 337 | 0 random-32-32-20.map 32 32 16 7 18 9 3.41421356 338 | 8 random-32-32-20.map 32 32 31 16 6 0 33.38477631 339 | 1 random-32-32-20.map 32 32 13 10 18 5 7.65685425 340 | 5 random-32-32-20.map 32 32 10 16 23 5 21.07106781 341 | 0 random-32-32-20.map 32 32 28 11 28 12 1.00000000 342 | 7 random-32-32-20.map 32 32 2 11 26 0 29.72792206 343 | 4 random-32-32-20.map 32 32 24 14 22 27 17.48528137 344 | 5 random-32-32-20.map 32 32 1 25 14 14 21.07106781 345 | 3 random-32-32-20.map 32 32 0 19 4 31 15.41421356 346 | 7 random-32-32-20.map 32 32 4 20 28 13 28.89949493 347 | 6 random-32-32-20.map 32 32 0 17 20 22 25.72792206 348 | 0 random-32-32-20.map 32 32 19 16 16 16 3.00000000 349 | 6 random-32-32-20.map 32 32 4 6 11 29 27.89949493 350 | 5 random-32-32-20.map 32 32 19 4 9 20 23.07106781 351 | 1 random-32-32-20.map 32 32 16 27 15 21 6.41421356 352 | 3 random-32-32-20.map 32 32 5 13 10 2 13.07106781 353 | 8 random-32-32-20.map 32 32 13 29 1 3 32.14213562 354 | 3 random-32-32-20.map 32 32 18 4 27 13 15.65685425 355 | 5 random-32-32-20.map 32 32 16 2 18 20 20.24264069 356 | 1 random-32-32-20.map 32 32 11 23 8 19 5.82842712 357 | 0 random-32-32-20.map 32 32 20 27 22 29 2.82842712 358 | 6 random-32-32-20.map 32 32 29 22 6 17 26.48528137 359 | 9 random-32-32-20.map 32 32 21 31 0 5 38.79898987 360 | 4 random-32-32-20.map 32 32 3 31 16 20 19.89949493 361 | 2 random-32-32-20.map 32 32 15 27 5 26 11.24264069 362 | 4 random-32-32-20.map 32 32 20 3 3 10 19.89949493 363 | 4 random-32-32-20.map 32 32 26 17 30 31 19.07106781 364 | 5 random-32-32-20.map 32 32 13 21 11 1 22.24264069 365 | 3 random-32-32-20.map 32 32 15 2 25 11 14.89949493 366 | 3 random-32-32-20.map 32 32 4 12 16 5 15.48528137 367 | 5 random-32-32-20.map 32 32 29 5 18 16 20.48528137 368 | 10 random-32-32-20.map 32 32 30 26 4 2 43.79898987 369 | 1 random-32-32-20.map 32 32 17 13 13 7 7.65685425 370 | 8 random-32-32-20.map 32 32 11 25 30 1 35.97056274 371 | 5 random-32-32-20.map 32 32 1 23 16 31 20.07106781 372 | 7 random-32-32-20.map 32 32 24 10 8 31 29.38477631 373 | 3 random-32-32-20.map 32 32 9 22 15 14 12.82842712 374 | 3 random-32-32-20.map 32 32 12 17 15 8 12.00000000 375 | 5 random-32-32-20.map 32 32 28 10 28 26 20.00000000 376 | 1 random-32-32-20.map 32 32 20 6 14 8 7.41421356 377 | 4 random-32-32-20.map 32 32 18 14 19 27 16.82842712 378 | 4 random-32-32-20.map 32 32 13 27 8 13 16.07106781 379 | 3 random-32-32-20.map 32 32 22 16 17 28 14.07106781 380 | 1 random-32-32-20.map 32 32 17 14 24 12 7.82842712 381 | 2 random-32-32-20.map 32 32 18 10 14 16 8.82842712 382 | 8 random-32-32-20.map 32 32 27 28 20 1 33.07106781 383 | 2 random-32-32-20.map 32 32 30 21 29 12 11.41421356 384 | 5 random-32-32-20.map 32 32 26 14 8 4 23.89949493 385 | 7 random-32-32-20.map 32 32 21 27 27 2 30.31370850 386 | 6 random-32-32-20.map 32 32 21 22 9 1 27.72792206 387 | 5 random-32-32-20.map 32 32 21 23 4 13 22.89949493 388 | 6 random-32-32-20.map 32 32 27 0 6 7 24.48528137 389 | 6 random-32-32-20.map 32 32 11 10 5 30 26.24264069 390 | 6 random-32-32-20.map 32 32 2 18 19 31 27.31370850 391 | 5 random-32-32-20.map 32 32 5 23 13 8 20.07106781 392 | 7 random-32-32-20.map 32 32 22 28 3 8 31.97056274 393 | 9 random-32-32-20.map 32 32 29 6 0 15 36.48528137 394 | 2 random-32-32-20.map 32 32 14 20 10 13 9.82842712 395 | 5 random-32-32-20.map 32 32 27 7 19 20 20.89949493 396 | 7 random-32-32-20.map 32 32 13 30 14 0 31.82842712 397 | 5 random-32-32-20.map 32 32 22 14 7 0 23.14213562 398 | 2 random-32-32-20.map 32 32 9 19 5 12 9.24264069 399 | 3 random-32-32-20.map 32 32 13 5 1 2 13.24264069 400 | 6 random-32-32-20.map 32 32 22 9 1 1 24.89949493 401 | 8 random-32-32-20.map 32 32 21 30 10 3 34.72792206 402 | 2 random-32-32-20.map 32 32 2 14 10 14 9.41421356 403 | 2 random-32-32-20.map 32 32 8 18 0 14 9.65685425 404 | 2 random-32-32-20.map 32 32 30 20 25 21 8.00000000 405 | 6 random-32-32-20.map 32 32 11 16 30 24 24.07106781 406 | 6 random-32-32-20.map 32 32 0 6 21 15 27.07106781 407 | 2 random-32-32-20.map 32 32 14 31 10 21 11.65685425 408 | 5 random-32-32-20.map 32 32 2 23 23 26 23.07106781 409 | 2 random-32-32-20.map 32 32 6 13 3 5 9.24264069 410 | 4 random-32-32-20.map 32 32 14 3 16 18 17.24264069 411 | --------------------------------------------------------------------------------