├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── examples ├── domain_template │ ├── ActionsTemplate.cpp │ ├── ActionsTemplate.hpp │ └── domain_template.cpp └── robot_nav_2d │ ├── RobotNav2dActions.cpp │ ├── RobotNav2dActions.hpp │ ├── resources │ ├── brc203d │ │ ├── brc203d.map │ │ ├── brc203d_cost_factor.map │ │ ├── nav2d_goals.txt │ │ └── nav2d_starts.txt │ ├── den501d │ │ ├── den501d.map │ │ ├── den501d.png │ │ ├── den501d.svg │ │ ├── den501d_cost_factor.map │ │ ├── nav2d_goals.txt │ │ └── nav2d_starts.txt │ ├── den520d │ │ ├── den520d.map │ │ ├── den520d.png │ │ ├── den520d.svg │ │ ├── den520d_cost_factor.map │ │ ├── nav2d_goals.txt │ │ └── nav2d_starts.txt │ ├── gen_cost_factor.py │ ├── hrt201n │ │ ├── hrt201n.map │ │ ├── hrt201n.png │ │ ├── hrt201n.svg │ │ ├── hrt201n_cost_factor.map │ │ ├── nav2d_goals.txt │ │ └── nav2d_starts.txt │ └── ht_chantry │ │ ├── ht_chantry.map │ │ ├── ht_chantry_cost_factor.map │ │ ├── nav2d_goals.txt │ │ └── nav2d_starts.txt │ └── run_robot_nav_2d.cpp ├── include ├── common │ ├── Action.hpp │ ├── Edge.hpp │ ├── State.hpp │ ├── Types.hpp │ ├── intrusive_heap.h │ └── intrusive_heap.hpp └── planners │ ├── AgepasePlanner.hpp │ ├── ArastarPlanner.hpp │ ├── EpasePlanner.hpp │ ├── GepasePlanner.hpp │ ├── MplpPlanner.hpp │ ├── PasePlanner.hpp │ ├── Planner.hpp │ ├── PwastarPlanner.hpp │ └── WastarPlanner.hpp └── src ├── common ├── Edge.cpp └── State.cpp └── planners ├── AgepasePlanner.cpp ├── ArastarPlanner.cpp ├── EpasePlanner.cpp ├── GepasePlanner.cpp ├── MplpPlanner.cpp ├── PasePlanner.cpp ├── Planner.cpp ├── PwastarPlanner.cpp └── WastarPlanner.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | # Prerequisites 2 | *.d 3 | 4 | # Compiled Object files 5 | *.slo 6 | *.lo 7 | *.o 8 | *.obj 9 | 10 | # Precompiled Headers 11 | *.gch 12 | *.pch 13 | 14 | # Compiled Dynamic libraries 15 | *.so 16 | *.dylib 17 | *.dll 18 | 19 | # Fortran module files 20 | *.mod 21 | *.smod 22 | 23 | # Compiled Static libraries 24 | *.lai 25 | *.la 26 | *.a 27 | *.lib 28 | 29 | # Executables 30 | *.exe 31 | *.out 32 | *.app 33 | 34 | # Folders 35 | build/ 36 | build* 37 | devel/ 38 | logs/ 39 | 40 | # IDE 41 | tags 42 | .vscode -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | 3 | project (epase) 4 | add_compile_options(-std=c++17) 5 | 6 | find_package(OpenCV REQUIRED) 7 | find_package(SBPL REQUIRED) 8 | 9 | if(NOT CMAKE_BUILD_TYPE) 10 | set(CMAKE_BUILD_TYPE Release) 11 | endif() 12 | 13 | add_compile_options(-std=c++17) 14 | 15 | set(CMAKE_CXX_FLAGS "-g") 16 | SET(CMAKE_CXX_FLAGS_RELEASE "-O3") 17 | SET(CMAKE_C_FLAGS_RELEASE "-O3") 18 | 19 | include_directories(include 20 | ${SBPL_INCLUDE_DIRS}) 21 | 22 | link_directories(${SBPL_LIBRARY_DIRS}) 23 | 24 | set(SOURCES 25 | src/common/State.cpp 26 | src/common/Edge.cpp 27 | src/planners/Planner.cpp 28 | src/planners/WastarPlanner.cpp 29 | src/planners/ArastarPlanner.cpp 30 | src/planners/PwastarPlanner.cpp 31 | src/planners/PasePlanner.cpp 32 | src/planners/EpasePlanner.cpp 33 | src/planners/GepasePlanner.cpp 34 | src/planners/AgepasePlanner.cpp 35 | src/planners/MplpPlanner.cpp 36 | ) 37 | 38 | add_executable(run_robot_nav_2d 39 | examples/robot_nav_2d/run_robot_nav_2d.cpp 40 | examples/robot_nav_2d/RobotNav2dActions.cpp 41 | ${SOURCES}) 42 | 43 | target_link_libraries(run_robot_nav_2d 44 | ${OpenCV_LIBRARIES} 45 | ${SBPL_LIBRARIES} 46 | pthread) 47 | 48 | add_executable(domain_template 49 | examples/domain_template/domain_template.cpp 50 | examples/domain_template/ActionsTemplate.cpp 51 | ${SOURCES}) 52 | 53 | target_link_libraries(domain_template 54 | pthread) -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 Shohin Mukherjee 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Parallel Search-Based Planning Algorithms 2 | 3 | This repository maintains open-source implementations of several parallel search-based planning algorithms. Currently, the following algorithms are available: 4 | * [wA*: Weighted A*](https://www.sciencedirect.com/science/article/pii/000437027090007X) [Pohl 1970] 5 | * [ARA*: Anytime Repairing A*](https://proceedings.neurips.cc/paper_files/paper/2003/file/ee8fe9093fbbb687bef15a38facc44d2-Paper.pdf) [Likhachev et al. 1970] 6 | * PwA*: Parallel Weighted A* (As explained in the [ePA\*SE](https://arxiv.org/pdf/2203.01369.pdf) and [MPLP](https://ieeexplore.ieee.org/abstract/document/9730025) papers) 7 | * [PA\*SE: Parallel A* For Slow Expansions](https://www.aaai.org/ocs/index.php/ICAPS/ICAPS14/paper/view/7952/8029) [Phillips et al. 2014] 8 | * [MPLP: Massively Parallelized Lazy Planning](https://arxiv.org/abs/2107.02826) [Mukherjee et al. 2022] 9 | * [ePA\*SE: Edge-based Parallel A* For Slow Evaluations](https://arxiv.org/abs/2203.01369) [Mukherjee et al. 2022] 10 | * [GePA\*SE: Generalized Edge-Based Parallel A* for Slow Evaluations](https://arxiv.org/abs/2301.10347) [Mukherjee et al. 2023] 11 | * [A-ePA\*SE: Anytime Edge-Based Parallel A* for Slow Evaluations](https://arxiv.org/abs/2305.04408) [Yang et al. 2023] 12 | 13 | If you use MPLP, please use the following citation: 14 | ``` 15 | @article{mukherjee2022mplp, 16 | title = {{MPLP}: Massively Parallelized Lazy Planning}, 17 | author = {Mukherjee, Shohin and Aine, Sandip and Likhachev, Maxim}, 18 | year = 2022, 19 | journal = {IEEE Robotics and Automation Letters}, 20 | publisher = {IEEE}, 21 | volume = 7, 22 | number = 3, 23 | pages = {6067--6074} 24 | } 25 | ``` 26 | If you use ePA*SE, please use the following citation: 27 | ``` 28 | @inproceedings{mukherjee2022epase, 29 | author = {Mukherjee, Shohin and Aine, Sandip and Likhachev, Maxim}, 30 | title = {{ePA*SE}: Edge-Based Parallel {A*} for Slow Evaluations}, 31 | booktitle = {International Symposium on Combinatorial Search}, 32 | volume={15}, 33 | number={1}, 34 | pages={136--144}, 35 | publisher={{AAAI} Press}, 36 | year={2022} 37 | } 38 | ``` 39 | If you use GePA*SE, please use the following citation: 40 | ``` 41 | @inproceedings{mukherjee2023gepase, 42 | title = {{GePA*SE}: Generalized Edge-based Parallel {A*} for Slow Evaluations}, 43 | author = {Mukherjee, Shohin and Likhachev, Maxim}, 44 | year = 2023, 45 | booktitle = {International Symposium on Combinatorial Search}, 46 | publisher = {{AAAI} Press} 47 | } 48 | ``` 49 | If you use A-ePA*SE, please use the following citation: 50 | ``` 51 | @inproceedings{yang2023aepase, 52 | title = {{A-ePA*SE}: Anytime Edge-based Parallel {A*} for Slow Evaluations}, 53 | author = {Yang, Hanlan and Mukherjee, Shohin and Likhachev, Maxim}, 54 | year = 2023, 55 | booktitle = {International Symposium on Combinatorial Search}, 56 | publisher = {{AAAI} Press} 57 | } 58 | ``` 59 | 60 | ## Usage 61 | 62 | ### Dependencies 63 | The planning code only uses C++ standard libraries and has no external dependencies. However, the robot_nav_2d example has the following dependencies: 64 | 1. [Boost C++](https://www.boost.org/) 65 | 2. OpenCV C++ for visualization 66 | 3. [sbpl package](https://github.com/sbpl/sbpl) 67 | 68 | ### Building 69 | 1. Clone the repository. 70 | ``` 71 | git clone https://github.com/shohinm/parallel_search.git 72 | ``` 73 | 2. Create build directory and compile. 74 | ``` 75 | cd parallel_search && mkdir build && cd build && cmake ../ -DCMAKE_BUILD_TYPE=Release && make 76 | ``` 77 | 78 | ### Running the example 79 | Run the following commands from within the build directory: 80 | * wA* 81 | ``` 82 | ./run_robot_nav_2d wastar 83 | ``` 84 | * ARA* 85 | ``` 86 | ./run_robot_nav_2d arastar [time_budget] [heuristic_weight] [heuristic_reduction] 87 | ``` 88 | * PwA*, MPLP, PA\*SE, ePA\*SE, GePA\*SE 89 | ``` 90 | ./run_robot_nav_2d [algorithm] [num_threads] 91 | ``` 92 | * A-ePA*SE: 93 | ``` 94 | ./run_robot_nav_2d agepase [num_threads] [time_budget] [heuristic_weight] [heuristic_reduction] 95 | ``` 96 | 97 | **Maintained by** : [Shohin Mukherjee](https://www.ri.cmu.edu/ri-people/shohin-mukherjee/) 98 | -------------------------------------------------------------------------------- /examples/domain_template/ActionsTemplate.cpp: -------------------------------------------------------------------------------- 1 | #include "ActionsTemplate.hpp" 2 | 3 | using namespace ps; 4 | 5 | ActionTemplate::ActionTemplate(const std::string& type, ParamsType params): 6 | Action(type, params) 7 | { 8 | 9 | } 10 | 11 | ActionSuccessor ActionTemplate::GetSuccessor(const StateVarsType& state_vars, int thread_id) 12 | { 13 | 14 | } 15 | 16 | bool ActionTemplate::CheckPreconditions(const StateVarsType& state) 17 | { 18 | 19 | } 20 | -------------------------------------------------------------------------------- /examples/domain_template/ActionsTemplate.hpp: -------------------------------------------------------------------------------- 1 | #ifndef ACTIONS_TEMPLATE_HPP 2 | #define ACTIONS_TEMPLATE_HPP 3 | 4 | #include 5 | 6 | namespace ps 7 | { 8 | 9 | class ActionTemplate : public Action 10 | { 11 | 12 | public: 13 | ActionTemplate(const std::string& type, ParamsType params); 14 | ActionSuccessor GetSuccessor(const StateVarsType& state_vars, int thread_id); 15 | bool CheckPreconditions(const StateVarsType& state); 16 | }; 17 | 18 | } 19 | 20 | #endif 21 | -------------------------------------------------------------------------------- /examples/domain_template/domain_template.cpp: -------------------------------------------------------------------------------- 1 | #include "ActionsTemplate.hpp" 2 | #include 3 | 4 | using namespace std; 5 | using namespace ps; 6 | 7 | /** 8 | * @brief Calculates the heuristic. 9 | * 10 | * @param[in] state_ptr The state variables 11 | * 12 | * @return The heuristic. 13 | */ 14 | double computeHeuristic(const StateVarsType& state_vars) 15 | { 16 | 17 | } 18 | 19 | /** 20 | * @brief Calculates the heuristic state to state. 21 | * 22 | * @param[in] state_vars_1 The state variables 1 23 | * @param[in] state_vars_2 The state variables 2 24 | * 25 | * @return The heuristic state to state. 26 | */ 27 | double computeHeuristicStateToState(const StateVarsType& state_vars_1, const StateVarsType& state_vars_2) 28 | { 29 | 30 | } 31 | 32 | /** 33 | * @brief Determines whether the specified state pointer is goal state. 34 | * 35 | * @param[in] state_ptr The state variables 36 | * 37 | * @return True if the specified state pointer is goal state, False otherwise. 38 | */ 39 | bool isGoalState(const StateVarsType& state_vars) 40 | { 41 | 42 | } 43 | 44 | /** 45 | * @brief { function_description } 46 | * 47 | * @param[in] state_vars The state variables 48 | * 49 | * @return { description_of_the_return_value } 50 | */ 51 | size_t StateKeyGenerator(const StateVarsType& state_vars) 52 | { 53 | 54 | } 55 | 56 | /** 57 | * @brief { function_description } 58 | * 59 | * @param[in] edge_ptr The edge pointer 60 | * 61 | * @return { description_of_the_return_value } 62 | */ 63 | size_t EdgeKeyGenerator(const EdgePtrType& edge_ptr) 64 | { 65 | 66 | } 67 | 68 | int main(int argc, char* argv[]) 69 | { 70 | // Define planner parameters 71 | ParamsType planner_params; 72 | // planner_params["num_threads"] = 5; 73 | // planner_params["heuristic_weight"] = 50; 74 | 75 | // Define action parameters. Actions can share parameters or have different parameters. 76 | ParamsType action_params; 77 | 78 | // Construct actions 79 | vector> action_ptrs; 80 | 81 | // Construct and set start state 82 | StateVarsType start_state_vars; 83 | 84 | // Construct planner 85 | shared_ptr planner_ptr = make_shared(planner_params); 86 | 87 | // Set actions 88 | planner_ptr->SetActions(action_ptrs); 89 | 90 | // Set state key generator 91 | planner_ptr->SetStateMapKeyGenerator(bind(StateKeyGenerator, placeholders::_1)); 92 | 93 | // Set edge key generator 94 | planner_ptr->SetEdgeKeyGenerator(bind(EdgeKeyGenerator, placeholders::_1)); 95 | 96 | // Set unary heuristic 97 | planner_ptr->SetHeuristicGenerator(bind(computeHeuristic, placeholders::_1)); 98 | 99 | // Set binary heuristic 100 | planner_ptr->SetStateToStateHeuristicGenerator(bind(computeHeuristicStateToState, placeholders::_1, placeholders::_2)); 101 | 102 | // Set goal condition 103 | planner_ptr->SetGoalChecker(bind(isGoalState, placeholders::_1)); 104 | 105 | // Set start state 106 | planner_ptr->SetStartState(start_state_vars); 107 | 108 | // Call planner 109 | bool plan_found = planner_ptr->Plan(); 110 | 111 | // Get planner Stats 112 | auto planner_stats = planner_ptr->GetStats(); 113 | 114 | } -------------------------------------------------------------------------------- /examples/robot_nav_2d/RobotNav2dActions.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include "RobotNav2dActions.hpp" 5 | 6 | using namespace std; 7 | using namespace ps; 8 | 9 | 10 | bool RobotNav2dAction::CheckPreconditions(const StateVarsType& state) 11 | { 12 | return true; 13 | } 14 | 15 | ActionSuccessor RobotNav2dAction::GetSuccessor(const StateVarsType& state_vars, int thread_id) 16 | { 17 | vector next_state_vars(3, 0); 18 | for (int i = 0; i < params_["length"]; ++i) 19 | { 20 | next_state_vars[0] = state_vars[0] + i*move_dir_[0]; 21 | next_state_vars[1] = state_vars[1] + i*move_dir_[1]; 22 | next_state_vars[2] = state_vars[2]; 23 | 24 | int x_limit = map_.size(); 25 | int y_limit = map_[0].size(); 26 | bool in_range = inRange(next_state_vars[0], next_state_vars[1]); 27 | 28 | if (!in_range) 29 | return ActionSuccessor(false, {make_pair(StateVarsType(), -DINF)}); 30 | 31 | bool is_collision = false; 32 | 33 | auto footprint = getFootPrintRectangular(next_state_vars[0], next_state_vars[1], params_["footprint_size"]); 34 | 35 | for (auto& cell : footprint) 36 | { 37 | if (!isValidCell(cell.first, cell.second)) 38 | return ActionSuccessor(false, {make_pair(StateVarsType(), -DINF)}); 39 | } 40 | 41 | } 42 | 43 | double cost = pow(pow((next_state_vars[0] - state_vars[0]), 2) + pow((next_state_vars[1] - state_vars[1]), 2), 0.5);; 44 | 45 | if (!cost_factor_map_.empty()) 46 | { 47 | cost *= cost_factor_map_[state_vars[0]][state_vars[1]]; 48 | } 49 | 50 | return ActionSuccessor(true, {make_pair(next_state_vars, cost)}); 51 | 52 | } 53 | 54 | ActionSuccessor RobotNav2dAction::GetSuccessorLazy(const StateVarsType& state_vars, int thread_id) 55 | { 56 | vector final_state_vars(3, 0); 57 | final_state_vars[0] = state_vars[0] + (params_["length"] - 1)*move_dir_[0]; 58 | final_state_vars[1] = state_vars[1] + (params_["length"] - 1)*move_dir_[1]; 59 | final_state_vars[2] = state_vars[2]; 60 | 61 | int x_limit = map_.size(); 62 | int y_limit = map_[0].size(); 63 | bool in_range = inRange(final_state_vars[0], final_state_vars[1]); 64 | 65 | if (!in_range) 66 | return ActionSuccessor(false, {make_pair(StateVarsType(), -DINF)}); 67 | 68 | bool is_collision = false; 69 | 70 | auto footprint = getFootPrintRectangular(final_state_vars[0], final_state_vars[1], params_["footprint_size"]); 71 | 72 | for (auto& cell : footprint) 73 | { 74 | if (!isValidCell(cell.first, cell.second)) 75 | return ActionSuccessor(false, {make_pair(StateVarsType(), -DINF)}); 76 | } 77 | 78 | double cost = pow(pow((final_state_vars[0] - state_vars[0]), 2) + pow((final_state_vars[1] - state_vars[1]), 2), 0.5);; 79 | 80 | if (!cost_factor_map_.empty()) 81 | { 82 | cost *= cost_factor_map_[state_vars[0]][state_vars[1]]; 83 | } 84 | 85 | return ActionSuccessor(true, {make_pair(final_state_vars, cost)}); 86 | 87 | } 88 | 89 | ActionSuccessor RobotNav2dAction::Evaluate(const StateVarsType& parent_state_vars, const StateVarsType& child_state_vars, int thread_id) 90 | { 91 | return GetSuccessor(parent_state_vars, thread_id); 92 | } 93 | 94 | bool RobotNav2dAction::isValidCell(int x, int y) 95 | { 96 | int x_limit = map_.size(); 97 | int y_limit = map_[0].size(); 98 | 99 | return ((x >= 0) && (x < x_limit) && 100 | (y >= 0) && (y < y_limit) && 101 | (map_[round(x)][round(y)] == 0)); 102 | } 103 | 104 | bool RobotNav2dAction::inRange(int x, int y) 105 | { 106 | int x_limit = map_.size(); 107 | int y_limit = map_[0].size(); 108 | 109 | return ((x >= 0) && (x < x_limit) && 110 | (y >= 0) && (y < y_limit)); 111 | } 112 | 113 | vector> RobotNav2dAction::getFootPrintRectangular(int x, int y, int footprint_size) 114 | { 115 | vector> footprint; 116 | 117 | if (params_["cache_footprint"]) 118 | { 119 | 120 | lock_.lock(); 121 | footprint = footprint_; 122 | lock_.unlock(); 123 | 124 | 125 | if (footprint.size() == 0) 126 | { 127 | // sbpl's footprint calculation method 128 | vector polygon; 129 | set cells; 130 | 131 | // sbpl's footprint function takes in (x,y) world coordinates not cells 132 | polygon.push_back(sbpl_2Dpt_t(footprint_size, footprint_size)); 133 | polygon.push_back(sbpl_2Dpt_t(footprint_size, -footprint_size)); 134 | polygon.push_back(sbpl_2Dpt_t(-footprint_size, -footprint_size)); 135 | polygon.push_back(sbpl_2Dpt_t(-footprint_size, footprint_size)); 136 | 137 | sbpl_xy_theta_pt_t pose(0, 0, 0); 138 | get_2d_footprint_cells(polygon, &cells, pose, 1); 139 | 140 | for (auto cell : cells) 141 | { 142 | footprint.push_back(pair(cell.x, cell.y)); 143 | } 144 | 145 | lock_.lock(); 146 | footprint_ = footprint; 147 | lock_.unlock(); 148 | 149 | } 150 | 151 | for (auto& coord : footprint) 152 | { 153 | coord.first += x; 154 | coord.second += y; 155 | } 156 | } 157 | else 158 | { 159 | // sbpl's footprint calculation method 160 | vector polygon; 161 | set cells; 162 | 163 | // sbpl's footprint function takes in (x,y) world coordinates not cells 164 | polygon.push_back(sbpl_2Dpt_t(footprint_size, footprint_size)); 165 | polygon.push_back(sbpl_2Dpt_t(footprint_size, -footprint_size)); 166 | polygon.push_back(sbpl_2Dpt_t(-footprint_size, -footprint_size)); 167 | polygon.push_back(sbpl_2Dpt_t(-footprint_size, footprint_size)); 168 | 169 | sbpl_xy_theta_pt_t pose(x, y, 0); 170 | get_2d_footprint_cells(polygon, &cells, pose, 1); 171 | 172 | for (auto cell : cells) 173 | { 174 | footprint.push_back(pair(cell.x, cell.y)); 175 | } 176 | } 177 | 178 | return footprint; 179 | } 180 | -------------------------------------------------------------------------------- /examples/robot_nav_2d/RobotNav2dActions.hpp: -------------------------------------------------------------------------------- 1 | #ifndef POINT_ROBOT_ACTION_HPP 2 | #define POINT_ROBOT_ACTION_HPP 3 | 4 | #include 5 | 6 | namespace ps 7 | { 8 | 9 | class RobotNav2dAction : public Action 10 | { 11 | 12 | public: 13 | RobotNav2dAction(const std::string& type, ParamsType params, 14 | std::vector> map, std::vector> cost_factor_map = std::vector>(), 15 | bool is_expensive = true) 16 | : Action(type, params, is_expensive), map_(map), cost_factor_map_(cost_factor_map) {}; 17 | bool CheckPreconditions(const StateVarsType& state); 18 | ActionSuccessor GetSuccessor(const StateVarsType& state_vars, int thread_id); 19 | ActionSuccessor GetSuccessorLazy(const StateVarsType& state_vars, int thread_id); 20 | ActionSuccessor Evaluate(const StateVarsType& parent_state_vars, const StateVarsType& child_state_vars, int thread_id=0); 21 | 22 | protected: 23 | bool isValidCell(int x, int y); 24 | bool inRange(int x, int y); 25 | std::vector> getFootPrintRectangular(int x, int y, int footprint_size); 26 | std::vector move_dir_; 27 | std::vector> map_; 28 | std::vector> cost_factor_map_; 29 | std::vector> footprint_; 30 | LockType lock_; 31 | }; 32 | 33 | class MoveUpAction : public RobotNav2dAction 34 | { 35 | 36 | public: 37 | MoveUpAction(const std::string& type, ParamsType params, 38 | std::vector> map, std::vector> cost_factor_map = std::vector>(), 39 | bool is_expensive = true): 40 | RobotNav2dAction(type, params, map, cost_factor_map, is_expensive) 41 | { 42 | move_dir_ = {0, 1}; 43 | }; 44 | }; 45 | 46 | class MoveUpRightAction : public RobotNav2dAction 47 | { 48 | 49 | public: 50 | MoveUpRightAction(const std::string& type, ParamsType params, 51 | std::vector> map, std::vector> cost_factor_map = std::vector>(), 52 | bool is_expensive = true): 53 | RobotNav2dAction(type, params, map, cost_factor_map, is_expensive) 54 | { 55 | move_dir_ = {1, 1}; 56 | }; 57 | }; 58 | 59 | class MoveRightAction : public RobotNav2dAction 60 | { 61 | 62 | public: 63 | MoveRightAction(const std::string& type, ParamsType params, 64 | std::vector> map, std::vector> cost_factor_map = std::vector>(), 65 | bool is_expensive = true): 66 | RobotNav2dAction(type, params, map, cost_factor_map, is_expensive) 67 | { 68 | move_dir_ = {1, 0}; 69 | }; 70 | }; 71 | 72 | class MoveRightDownAction : public RobotNav2dAction 73 | { 74 | 75 | public: 76 | MoveRightDownAction(const std::string& type, ParamsType params, 77 | std::vector> map, std::vector> cost_factor_map = std::vector>(), 78 | bool is_expensive = true): 79 | RobotNav2dAction(type, params, map, cost_factor_map, is_expensive) 80 | { 81 | move_dir_ = {1, -1}; 82 | }; 83 | }; 84 | 85 | 86 | class MoveDownAction : public RobotNav2dAction 87 | { 88 | 89 | public: 90 | MoveDownAction(const std::string& type, ParamsType params, 91 | std::vector> map, std::vector> cost_factor_map = std::vector>(), 92 | bool is_expensive = true): 93 | RobotNav2dAction(type, params, map, cost_factor_map, is_expensive) 94 | { 95 | move_dir_ = {0, -1}; 96 | }; 97 | }; 98 | 99 | class MoveDownLeftAction : public RobotNav2dAction 100 | { 101 | 102 | public: 103 | MoveDownLeftAction(const std::string& type, ParamsType params, 104 | std::vector> map, std::vector> cost_factor_map = std::vector>(), 105 | bool is_expensive = true): 106 | RobotNav2dAction(type, params, map, cost_factor_map, is_expensive) 107 | { 108 | move_dir_ = {-1, -1}; 109 | }; 110 | }; 111 | 112 | 113 | class MoveLeftAction : public RobotNav2dAction 114 | { 115 | 116 | public: 117 | MoveLeftAction(const std::string& type, ParamsType params, 118 | std::vector> map, std::vector> cost_factor_map = std::vector>(), 119 | bool is_expensive = true): 120 | RobotNav2dAction(type, params, map, cost_factor_map, is_expensive) 121 | { 122 | move_dir_ = {-1, 0}; 123 | }; 124 | }; 125 | 126 | 127 | class MoveLeftUpAction : public RobotNav2dAction 128 | { 129 | 130 | public: 131 | MoveLeftUpAction(const std::string& type, ParamsType params, 132 | std::vector> map, std::vector> cost_factor_map = std::vector>(), 133 | bool is_expensive = true): 134 | RobotNav2dAction(type, params, map, cost_factor_map, is_expensive) 135 | { 136 | move_dir_ = {-1, 1}; 137 | }; 138 | }; 139 | 140 | } 141 | 142 | #endif 143 | -------------------------------------------------------------------------------- /examples/robot_nav_2d/resources/brc203d/nav2d_goals.txt: -------------------------------------------------------------------------------- 1 | 35.5626 42.5442 2 | 135.752 297.346 3 | 37.8773 140.933 4 | 90.7997 35.6391 5 | 167.066 205.567 6 | 191.21 121.973 7 | 159.655 174.676 8 | 125.085 169.85 9 | 164.535 50.8206 10 | 124.026 62.6233 11 | 137.658 226.316 12 | 243.431 39.9732 13 | 124.035 336.567 14 | 142.512 350.083 15 | 23.1229 52.2172 16 | 163.565 213.05 17 | 39.1515 128.76 18 | 126.217 328.186 19 | 151.057 280.052 20 | 224.328 50.6511 21 | 150.835 356.094 22 | 81.8434 44.8062 23 | 195.7 312.674 24 | 86.8429 310.223 25 | 68.2992 277.926 26 | 137.018 381.59 27 | 70.6313 186.259 28 | 184.342 198.965 29 | 182.056 203.265 30 | 115.296 68.6614 31 | 163.436 134.565 32 | 6.07471 51.213 33 | 70.8052 64.3798 34 | 66.932 176.085 35 | 160.157 206.145 36 | 197.441 269.22 37 | 152.592 262.76 38 | 174.762 210.387 39 | 22.4333 123.447 40 | 134.419 290.178 41 | 110.031 191.365 42 | 209.853 51.4764 43 | 115.959 385.618 44 | 194.875 267.956 45 | 104.269 158.37 46 | 163.442 325.774 47 | 129.355 181.486 48 | 84.384 56.365 49 | 134.69 221.849 50 | 120.379 174.403 51 | -------------------------------------------------------------------------------- /examples/robot_nav_2d/resources/brc203d/nav2d_starts.txt: -------------------------------------------------------------------------------- 1 | 42.9301 156.769 1127.29 42 2 | 28.2689 49.2955 1597.35 62 3 | 136.596 47.4122 1093.35 41 4 | 99.6259 112.713 413.823 16 5 | 130.584 66.3997 987.411 37 6 | 187.96 316.626 1522.94 56 7 | 157.2 317.862 1045.35 39 8 | 93.5819 270.753 823.529 31 9 | 179.204 126.677 1243.18 46 10 | 109.408 82.7221 125.823 4 11 | 141.422 244.241 81.9411 3 12 | 85.7591 41.2803 787.882 32 13 | 61.096 44.1483 1724.88 64 14 | 169.735 271.234 587.647 22 15 | 107.709 188.12 991.529 38 16 | 139.293 248.381 891.411 33 17 | 97.5884 187.756 505.706 19 18 | 126.785 369.05 211.882 8 19 | 31.2953 132.835 1123.18 41 20 | 164.283 185.603 1363.18 51 21 | 123.032 319.019 241.706 8 22 | 92.8664 172.931 683.647 26 23 | 202.106 43.0816 2065 79 24 | 136.022 381.309 491.647 18 25 | 38.6906 168.033 1217.47 47 26 | 118.83 354.748 149.823 5 27 | 158.598 178.091 461.823 18 28 | 77.4179 29.4818 1209.23 45 29 | 145.564 309.565 1239.06 45 30 | 108.501 221.95 753.941 31 31 | 222.735 41.7947 1517.12 57 32 | 81.3315 187.355 1093.35 41 33 | 127.517 181.505 723.411 26 34 | 144.247 48.0141 839.294 30 35 | 164.53 201.014 24 1 36 | 60.2948 48.4205 1579.18 60 37 | 87.9301 74.4083 1159.53 45 38 | 131.332 159.512 371.647 13 39 | 133.365 295.359 1223.29 46 40 | 171.681 183.541 943.529 36 41 | 177.035 314.235 837.588 32 42 | 114.145 66.0554 505.706 19 43 | 132.982 330.265 293.823 11 44 | 27.2528 140.879 1319.29 50 45 | 169.527 225.167 577.706 22 46 | 112.254 268.676 373.352 11 47 | 89.6485 181.622 192 8 48 | 88.2471 274.733 1419.41 55 49 | 200.891 42.5497 1371.41 53 50 | 128.456 171.02 24 1 51 | -------------------------------------------------------------------------------- /examples/robot_nav_2d/resources/den501d/den501d.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shohinm/parallel_search/885afd00cc09f818a88dc66e2acce7a20c23bfd2/examples/robot_nav_2d/resources/den501d/den501d.png -------------------------------------------------------------------------------- /examples/robot_nav_2d/resources/den501d/nav2d_goals.txt: -------------------------------------------------------------------------------- 1 | 45.3128 205.155 2 | 204.017 177.209 3 | 288.707 287.611 4 | 48.7647 247.466 5 | 215.66 215.522 6 | 209.002 86.9557 7 | 38.7658 231.796 8 | 185.163 179.106 9 | 74.77 280.475 10 | 35.1145 228.017 11 | 260.524 200.94 12 | 204.087 256.651 13 | 84.0911 204.28 14 | 131.994 206.849 15 | 172.338 50.1396 16 | 186.458 150.999 17 | 152.258 92.2696 18 | 223.588 230.49 19 | 187.067 161.008 20 | 84.535 214.152 21 | 141.868 90.9296 22 | 102.873 225.771 23 | 228.618 214.699 24 | 266.417 274.793 25 | 191.886 23.0634 26 | 285.544 240.624 27 | 192.585 282.41 28 | 166.438 302.63 29 | 96.8256 198.249 30 | 67.7715 229.218 31 | 230.893 245.674 32 | 185.525 138.045 33 | 301.068 279.152 34 | 72.5797 275.569 35 | 60.9605 278.132 36 | 150.504 97.0672 37 | 48.0316 265.092 38 | 94.499 208.186 39 | 233.164 240.465 40 | 237.729 245.988 41 | 46.2343 215.324 42 | 47.0599 287.196 43 | 248.44 281.575 44 | 142.668 220.55 45 | 208.025 225.635 46 | 181.318 5.23372 47 | 260.852 221.91 48 | 180.493 238.804 49 | 214.359 248.275 50 | 99.1941 263.778 51 | -------------------------------------------------------------------------------- /examples/robot_nav_2d/resources/den501d/nav2d_starts.txt: -------------------------------------------------------------------------------- 1 | 203.428 242.446 1025.47 39 2 | 196.045 100.059 433.706 16 3 | 76.2496 328.074 1331.65 53 4 | 187.036 82.6115 1695.76 69 5 | 132.475 235.243 623.294 21 6 | 294.693 231.448 1627.18 62 7 | 244.759 236.287 1351.53 53 8 | 38.1156 199.297 1355.65 54 9 | 198.056 201.514 885.588 34 10 | 63.3666 207.005 159.765 5 11 | 300.521 270.28 415.529 14 12 | 279.779 199.135 757.352 27 13 | 248.135 268.68 1083.41 41 14 | 279.367 224.572 1151.29 43 15 | 191.008 218.213 855.765 34 16 | 183.591 274.776 600 25 17 | 287.818 252.702 1555.18 59 18 | 245.344 271.526 501.588 18 19 | 187.844 57.067 504 21 20 | 233.407 76.1296 1795.18 69 21 | 217.569 218.055 857.47 32 22 | 272.772 203.058 1436.88 52 23 | 271.767 281.034 415.529 14 24 | 215.679 212.723 673.706 26 25 | 126.156 92 563.647 21 26 | 149.067 280.049 833.47 31 27 | 130.873 155.459 841.706 33 28 | 198.23 234.468 395.647 14 29 | 143.495 154.895 480 20 30 | 232.728 208.172 1469.12 55 31 | 300.688 266.434 389.823 15 32 | 140.546 242.609 869.823 35 33 | 75.2785 319.308 1365.59 54 34 | 185.615 285.236 639.765 25 35 | 294.234 236.378 1299.41 50 36 | 275.033 286.632 1457.47 57 37 | 266.312 219.842 1313.47 51 38 | 192.65 198.707 1011.41 38 39 | 149.609 229.187 765.588 29 40 | 283.756 248.534 337.706 12 41 | 289.96 284.995 1487.29 57 42 | 65.3394 282.998 96 4 43 | 122.893 199.424 891.411 33 44 | 189.041 22.8813 1561.71 63 45 | 44.3736 253.614 947.647 37 46 | 88.9194 137.793 2277.59 92 47 | 165.714 94.9277 1473.23 56 48 | 47.693 212.761 1025.47 39 49 | 146.134 251.604 375.765 14 50 | 182.272 45.1269 1497.94 62 51 | -------------------------------------------------------------------------------- /examples/robot_nav_2d/resources/den520d/den520d.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shohinm/parallel_search/885afd00cc09f818a88dc66e2acce7a20c23bfd2/examples/robot_nav_2d/resources/den520d/den520d.png -------------------------------------------------------------------------------- /examples/robot_nav_2d/resources/den520d/nav2d_goals.txt: -------------------------------------------------------------------------------- 1 | 200.473 205.199 2 | 85.817 197.435 3 | 122.214 161.62 4 | 36.2503 155.991 5 | 163.213 134.742 6 | 227.9 89.6655 7 | 230.965 218.686 8 | 96.053 195.384 9 | 136.091 10.095 10 | 163.835 90.9905 11 | 112.667 226.179 12 | 58.6159 229.597 13 | 244.856 151.281 14 | 112.527 237.46 15 | 175.16 234.12 16 | 201.217 79.0167 17 | 48.0085 70.9923 18 | 43.4194 233.049 19 | 75.3051 59.6912 20 | 39.0118 188.162 21 | 42.0101 191.483 22 | 204.236 188.292 23 | 233.735 210.662 24 | 148.334 116.312 25 | 135.887 194.625 26 | 9.06775 192.185 27 | 166.325 63.7473 28 | 18.9852 162.7 29 | 184.133 29.1131 30 | 241.745 115.886 31 | 213.31 60.1102 32 | 123.376 78.3736 33 | 159.187 10.5021 34 | 172.528 163.874 35 | 155.931 161.18 36 | 189.552 51.9687 37 | 167.201 66.1172 38 | 66.6872 225.488 39 | 28.4866 92.9314 40 | 170.638 74.2159 41 | 84.3884 48.766 42 | 31.0126 176.247 43 | 232.869 224.613 44 | 118.698 218.178 45 | 45.6211 57.0451 46 | 35.389 92.6337 47 | 232.572 159.879 48 | 126.995 86.0877 49 | 155.874 66.5389 50 | 84.8348 23.4252 51 | -------------------------------------------------------------------------------- /examples/robot_nav_2d/resources/den520d/nav2d_starts.txt: -------------------------------------------------------------------------------- 1 | 215.088 101.356 573.588 21 2 | 233.382 50.7707 1074.47 34 3 | 71.1103 142.37 279.765 10 4 | 162.742 184.345 673.706 26 5 | 156.836 76.0801 297.941 12 6 | 49.2067 170.449 1047.06 37 7 | 60.9997 249.453 909.588 35 8 | 68.2664 138.718 323.647 11 9 | 131.209 171.605 801.941 33 10 | 72.7791 189.803 725.117 24 11 | 176.093 42.6554 1065.23 39 12 | 212.275 84.8966 1070.35 33 13 | 89.6922 176.474 817.706 32 14 | 168.27 220.68 293.823 11 15 | 102 209.395 409.706 15 16 | 158.617 72.2323 201.941 8 17 | 114.441 58.1094 341.823 13 18 | 142.45 107.041 860.881 28 19 | 191.941 94.7465 645.588 24 20 | 149.629 62.8141 852.646 26 21 | 32.1216 203.922 67.8823 2 22 | 45.1099 61.696 1078.59 35 23 | 41.7617 100.664 1260.65 43 24 | 91.9284 141.989 313.706 11 25 | 175.971 25.6075 919.529 35 26 | 191.439 161.63 971.647 38 27 | 227.571 19.7876 419.647 15 28 | 84.1669 59.477 709.352 25 29 | 71.6908 140.349 850.94 28 30 | 120.7 152.283 703.529 26 31 | 88.3054 153.81 812.881 26 32 | 172.922 124.118 343.529 11 33 | 182.294 46.9168 355.882 14 34 | 105.98 178.868 341.823 13 35 | 88.8617 47.448 691.176 23 36 | 187.067 84.3922 144 6 37 | 235.754 175.982 681.235 23 38 | 136.305 22.5244 1147.18 42 39 | 175.648 24.0912 859.176 30 40 | 147.633 152.455 423.765 16 41 | 198.596 74.1135 601.706 23 42 | 195.807 179.662 825.941 34 43 | 76.1058 232.568 827.647 32 44 | 221.332 126.546 749.117 25 45 | 214.562 186.133 1126.59 37 46 | 127.623 31.1635 575.294 19 47 | 83.1506 239.497 903.058 31 48 | 214.228 210.259 792.999 26 49 | 100.948 169.32 613.352 21 50 | 93.0812 74.0853 249.941 10 51 | -------------------------------------------------------------------------------- /examples/robot_nav_2d/resources/gen_cost_factor.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import os 3 | import pdb 4 | 5 | dim = {'hrt201n': [305, 294], 'den501d': [338, 320], 'den520d': [257, 256], 'ht_chantry': [141, 162], 'brc203d': [391, 274]} 6 | 7 | scale = 5 8 | 9 | for map_name, dims in dim.items(): 10 | print("Map: {} | dims: {}".format(map_name, np.multiply(dims,scale))) 11 | cost_mat = np.random.uniform(low=1, high=100, size=np.multiply(dims,scale)) 12 | np.savetxt(os.path.join(map_name, map_name +'_cost_factor.map'), cost_mat, fmt='%d') 13 | -------------------------------------------------------------------------------- /examples/robot_nav_2d/resources/hrt201n/hrt201n.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shohinm/parallel_search/885afd00cc09f818a88dc66e2acce7a20c23bfd2/examples/robot_nav_2d/resources/hrt201n/hrt201n.png -------------------------------------------------------------------------------- /examples/robot_nav_2d/resources/hrt201n/nav2d_goals.txt: -------------------------------------------------------------------------------- 1 | 230.231 243.524 2 | 117.667 271.916 3 | 281.202 179.535 4 | 55.1347 84.2516 5 | 145.661 231.945 6 | 135.658 250.002 7 | 170.352 138.036 8 | 150.142 296.297 9 | 141.689 93.0115 10 | 136.317 258.927 11 | 52.393 67.6995 12 | 145.846 102.166 13 | 203.47 134.981 14 | 67.9128 117.653 15 | 162.381 280.475 16 | 152.443 122.216 17 | 53.2614 104.113 18 | 187.505 231.594 19 | 42.3307 125.989 20 | 65.4451 67.0353 21 | 216.954 141.17 22 | 192.241 279.539 23 | 94.5144 203.728 24 | 73.5618 78.1495 25 | 134.215 132.491 26 | 158.588 124.018 27 | 142.845 58.7082 28 | 69.4898 199.191 29 | 151.219 130.056 30 | 129.063 260.404 31 | 176.937 254.837 32 | 152.915 273.083 33 | 212.849 141.64 34 | 62.407 117.511 35 | 217.403 145.439 36 | 88.9586 178.894 37 | 187.177 225.753 38 | 37.2807 51.0085 39 | 62.2651 206.839 40 | 45.3176 27.4896 41 | 209.681 179.095 42 | 92.3287 174.802 43 | 32.9884 98.8494 44 | 104.478 170.387 45 | 110.243 170.25 46 | 96.8264 131.032 47 | 156.472 303.026 48 | 13.6314 86.5947 49 | 183.158 229.645 50 | 138.378 64.5393 51 | -------------------------------------------------------------------------------- /examples/robot_nav_2d/resources/hrt201n/nav2d_starts.txt: -------------------------------------------------------------------------------- 1 | 247.015 120.287 1031.29 38 2 | 154.863 234.824 289.706 10 3 | 103.006 209.434 1257.23 47 4 | 131.428 68.9625 1229.12 45 5 | 30.3323 38.453 2015.29 79 6 | 20.5525 62.4198 2045.12 79 7 | 105.574 168.508 413.823 16 8 | 65.755 198.595 1739.65 70 9 | 198.59 147.3 457.706 17 10 | 254.186 150.182 921.235 33 11 | 246.411 220.897 1897 72 12 | 246.027 249.529 1089.23 40 13 | 134.568 238.165 713.47 26 14 | 92.8035 132.373 149.823 5 15 | 76.5708 198.146 1629.59 65 16 | 181.823 196.021 475.882 19 17 | 192.678 261.554 1761.23 68 18 | 257.047 179.693 611.647 23 19 | 187.471 137.897 971.647 38 20 | 188.385 182.17 1545.23 59 21 | 185.291 281.671 755.647 29 22 | 140.52 109.832 1009.71 40 23 | 250.609 183.233 1147.18 42 24 | 154.888 258.64 2021.12 78 25 | 100.413 211.201 467.647 17 26 | 187.692 213.604 581.823 23 27 | 220.336 146.447 775.529 29 28 | 203.851 135.45 903.765 36 29 | 64.4315 106.544 1035.41 39 30 | 52.9601 109.57 1843.18 71 31 | 120.24 140.281 893.823 36 32 | 182.124 211.576 499.882 20 33 | 70.976 205.973 961.706 38 34 | 40.6054 127.387 699.411 25 35 | 69.2179 127.238 759.765 30 36 | 131.836 139.772 351.765 13 37 | 151.566 98.3476 749.823 30 38 | 153.682 241.231 1995.41 79 39 | 213.819 187.848 1171.18 43 40 | 89.9603 196.921 1345.71 54 41 | 190.379 146.055 183.765 6 42 | 78.5375 132.596 221.823 8 43 | 19.8767 102.831 57.9411 2 44 | 196.458 148.73 577.706 22 45 | 217.692 136.297 625.706 24 46 | 10.7357 164.573 567.765 22 47 | 167.214 166.598 817.706 32 48 | 32.0716 37.8107 269.823 10 49 | 206.586 164.76 351.765 13 50 | 213.486 227.724 1265.47 49 51 | -------------------------------------------------------------------------------- /examples/robot_nav_2d/resources/ht_chantry/ht_chantry.map: -------------------------------------------------------------------------------- 1 | type octile 2 | height 141 3 | width 162 4 | map 5 | @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 6 | @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 7 | @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 8 | @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T.TTT@@@@@@@@@@T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 9 | @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T....T@@@@@@@@T.T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 10 | @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T.....TTTTTTTT..T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 11 | @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTT...............T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 12 | @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T................TT@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 13 | @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T................T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 14 | @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T................T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 15 | @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 16 | @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 17 | @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTTT@@@@@@@@@@@@@@TTT.............T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 18 | @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T....T@@@@@@@@@@@@@@@T.............TT@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 19 | @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T....T@@@@@@@@@@@@@@@T...............T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 20 | @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...T@@@@@@@@@@@@@@@@T...............T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 21 | @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...T@@@@@@@@@@@@@TTTT...............T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 22 | @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T..T@@@@@@@@@@@@T...................T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 23 | @@@@@@@@@@@@@@@@@@@@@@@@@TTTTTTT@@@@@@@@@@@@@@@@@TT..TTT@@@@@@@@@@T...................T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 24 | @@@@@@@@@@@@@@@@@@@@@@@@T.......TTTTTT@@@@TTTTTTT.......TT@@@@@@@T....................TTTTTTTTTTTTTTTT@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 25 | @@@@@@@@@@@@@@@@@@@@@@@@@TTT..........T@@T................TTTTT@TT....................................T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 26 | @@@@@@@@@@@@@@@@@@@@@@@@@@@@T.........T@@T.....................T......................................T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 27 | @@@@@@@@@@@@@@@@@@@@@@@@@@@T..........T@@T............................................................T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 28 | @@@@@@@@@@@@@@@@@@@@@@@@@@@T..........T@@T............................................................T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 29 | @@@@@@@@@@@@@@@@@@@@@@@@@TT...........T@@T............................................................T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 30 | @@@@@@@@@@@@@@@@@@@@@@@@T.............T@@T............................................................T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 31 | @@@@@@@@@@@@@@@@@@@@@@@@@TTT..........T@T..............................................................T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 32 | @@@@@@@@@@@@@@@@@@@@@@@@@@@T...........TT..............................................................T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 33 | @@@@@@@@@@@@@@@@@@@@@@@@@@@@T..........TT..............................................................T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 34 | @@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT........................................................................T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 35 | @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T........................................................................T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 36 | @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T........TT..............TT......TT......................................T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 37 | @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTTTTTTT@@TTTTTTT.......TT......TT.....TTTTTTTTTTTTTTTTTTTTTTTTTT.......T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 38 | @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT.....T@T.....T@T....T@@@@@@@@@@@@@@@@@@@@@@@@@TTTTTTT@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 39 | @@@@@@@@@@@@@@@@@@@@@@@@@TTTTTTT@@@@@@@@@@@@@@@@@T.....T@T.....T@T....T@@@@@@@@@@@@@@@@@@@@@@@@@@TTTTTT@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 40 | @@@@@@@@@@@@@@@@@@@@@@@@T.......TTTTTTTTTTTTTTTTTT.....TTT.....TTT....TTTTTTTTTTTTTTTTTTTTTTTTTTT......T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 41 | @@@@@@@@@@@@@@@@@@@@@@@@T..............................................................................T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 42 | @@@@@@@@@@@@@@@@@@@@@@@@T..............................................................................T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 43 | @@@@@@@@@@@@@@@@@@@@@@@@T..............................................................................T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 44 | @@@@@@@@@@@@@@@@@@@@@@@@T..............................................................................T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 45 | @@@@@@@@@@@@@@@@@@@@@@@@T..............................................................................T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 46 | @@@@@@@@@@@@@@@@@@@@@@@@@T.....................TTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTT......T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 47 | @@@@@@@@@@@@@@@@@@@@@@@@@T.....................T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T.....T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 48 | @@@@@@@@@@@@@@@@@@@@@@@@@T.....................T@@TTTTTTTTTTTTTTTTTTTTTTTTT@@@@@@@@@@@@@@@@@@@@@T....T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 49 | @@@@@@@@@@@@@@@@@@@@@@@@@T.....................T@T.........................TTTTTTTTTTTTTTTTTTTT@T....T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 50 | @@@@@@@@@@@@@@@@@@@@@@@@@T.....................T@T.............................................TT....T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 51 | @@@@@@@@@@@@@@@@@@@@@@@@@T.....................T@T.............................................TT....T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 52 | @@@@@@@@@@@@@@@@@@@@@@@@@T.........TTTTTTTTTT..T@T.............................................TT....T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 53 | @@@@@@@@@@@@@@@@@@@@@@@@@T.........TTTTTTTTTT..T@T.............................................TT....T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 54 | @@@@@@@@@@@@@@@@@@@@@@@@@T.....................T@T.............................................TT.....T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 55 | @@@@@@@@@@@@@@@@@@@@@@@@@T.....................T@T.............................................TT.....T@@@@@@@@@@@@@@@@@@@TTTTT@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 56 | @@@@@@@@@@@@@@@@@@@@@@@@@T.....................T@T.............................................TT.....T@@@@@@@@@@@@@@@@@@T.....TTTT@T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 57 | @@@@@@@@@@@@@@@@@@@@@@@@@T.....................T@T....................................................T@@@@@@@@@@@@@@@@@@T.........T..@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 58 | @@@@@@@@@@@@@@@@@@@@@@@@@T.........TTTTTTTTTT..T@T....................................................T@@@@@@@@@@@@@@@@@@T.............T@@@@@@@@@@@@@@@@@@@@@@@@@@ 59 | @@@@@@@@@@@@@@@@@@@@@@@@@T.........TTTTTTTTTT..T@T....................................................T@@@@@@@@@@@@@@@@@@T.............T@@@@@@@@@@@@@@@@@@@@@@@@@@ 60 | @@@@@@@@@@@@@@@@@@@@@@@@@T.....................T@T....................................................T@@@@@@@@@@@@@@@@@@T.............T@@@@@@@@@@@@@@@@@@@@@@@@@@ 61 | @@@@@@@@@@@@@@@@@@@@@@@@@T....TTTT............T@@T....TTTTTTTTTTTTTTTTTTTT.TT.TTTT....TTTT............T@@@@@@@@@@@@@@@@@@T.........TTTT@@@@@@@@@@@@@@@@@@@@@@@@@@@ 62 | @@@@@@@@@@@@@@@@@@@@@@@@@T....T@@@TTTTTTTTTTTT@@@T....T@@@@@@@@@@@@@@@@@@@T@T.T@@T....T@@T............T@@@@@@@@@@@@@@@@@@@TTT......T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 63 | @@@@@@@@@@@@@@@@@@@@@@@@@T....T@@@@@@@@@@@@@@@@@@T....T@@@@TTT@@@@T@@@@@@@@T..T@@T....T@@T............T@@@@@@@@@@@@@@@@@@@@@T......TTTTT@@@@@@@@@@@@@@@@@TTTTTT@@@ 64 | @@@@@@@@@@@@@@@@@@@@@@@@@T....T@@@@@@@@@@@TTTTT@@T....T@@@T...TTTT.T@@@@@@T...T@@T....T@@T............TTTTTT@@@@@@@@TTTTTTTTT...........TTTT@@@@@@@@@TTTT......T@@ 65 | @@@@@@@@@@@@@@@@@@@@@@@@@T.....T@@@@@@@@@T.....T@T....T@@T.........T@@@@@@T....TT......TT...................T@@@@@@T........................T@@@@@@@T..........T@@ 66 | @@@@@@@@@@@@@@@@@@@@@@@@@T.....T@@@@@@@@@T.....T@T....T@T..........TT@@@@@T.................................T@@@@@@T........................T@@@@@@@T..........T@@ 67 | @@@@@@@@@@@@@@@@@@@@@@@@@T.....T@@@@@@@@T......T@T....T@T............TT@@@T..................................T@@TTT..........................T@@@TTT...........T@@ 68 | @@@@@@@@@@@@@@@@@@@@@@@@@T.....T@@@@@@@@T......T@T....T@T..............T@@T...................................TT..............................TTT...........TTT@@@ 69 | @@@@@@@@@@@@@@@@@@@@@@@@@T.....T@@@@@@@@@T.....TT......TT..............T@@T.................................................................................TT@@@@ 70 | @@@@@@@@@@@@@@@@@@@@@@@@@T.....T@@@@@@@@@@TTT..TT......TT..............T@@T...................................................................................T@@@ 71 | @@@@@@@@@@@@@@@@@@@@@@@@@T.....T@@@@@@@@@@@@T..TT......TT..............T@@T...................................................................................T@@@ 72 | @@@@@@@@@@@@@@@@@@@@@@@@@T.....T@@@@@@@@@@@@T..........................T@@T...................................................................................T@@@ 73 | @@@@@@@@@@@@@@@@@@@@@@@@@T.....T@@@@@@@@@@@@T..........................T@T....................................................................................T@@@ 74 | @@@@@@@@@@@@@@@@@@@@@@@@@T.....T@@@@@@@@@@@@T..........................T@T....................................................................................T@@@ 75 | @@@@@@@@@@@@@@@@@@@@@@@@@T.....T@@@@@@@@@@@@@T.........................T@T....................................................................................T@@@ 76 | @@@@@@@@@@@@@@@@@@@@@@@@@T.....T@@@@@@@@@@@@TT.........................T@T....................................................................................T@@@ 77 | @@@@@@@@@@@@@@@@@@@@@@@@@T.....T@@@@@@@@@@@T...........................T@@TT..................................................................................T@@@ 78 | @@@@@@@@@@@@@@@@@@@@@@@@@T.....T@@@@@@@@@@@T...TT......TT...........TTT@@@@T..................................................................................T@@@ 79 | @@@@@@@@@@@@@@@@@@@@@@@@@T.....T@@@@@@@@@@@T...TT......TT...........T@@@@@T...................................................................................T@@@ 80 | @@@@@@@@@@@@@@@@@@@@@@@@@T.....T@@@@@@@@@@@T...TT......TT............TT@@@@T................................................................................TT@@@@ 81 | @@@@@@@@@@@@@@@@@@@@@@@@@T.....T@@@@@@@@@@TT...T@T....T@T..............T@@@T.................................TT..T............................TTTT..........TTT@@@ 82 | @@@@@@@@@@@@@@@@@@@@@@@@@T.....T@@@@@@@@@T.....T@T....T@T..........TTTT@@@@.................................T@@TT@T..........................T@@@@TT...........T@@ 83 | @@@@@@@@@@@@@@@@@@@@@@@@@T.....T@@@@@@@@@@T....T@T....T@T..........T@@@@@@@T................................T@@@@@T..........................T@@@@@T...........T@@ 84 | @@@@@@@@@@@@@@@@@@@@@@@@@T.....T@@@@@@@@@@T....T@T....T@@T.........T@@@@@@@@T..TT......TT...................T@@@@@@T.........................T@@@@@@T..........T@@ 85 | @@@@@@@@@@@@@@@@@@@@@@@@@T....T@@@@@@@@@@@@TTTT@@T....T@@@T...TTTT.T@@@@@@@@T.T@@T....T@@T............TTTTTT@@@@@@@@TTTT...........TTTTTTTTTT@@@@@@@@TTT.......T@@ 86 | @@@@@@@@@@@@@@@@@@@@@@@@@T....T@@@@@@@@@@@@@@@@@@T....T@@@@TTT@@@@T@@@@@@@@@T.T@@T....T@@T............T@@@@@@@@@@@@@@@@@TTTTT......T@@@@@@@@@@@@@@@@@@@@TTTTTTT@@@ 87 | @@@@@@@@@@@@@@@@@@@@@@@@@T....T@@@TTTTTTTTTTTT@@@T....T@@@@@@@@@@@@@@@@@@@@T..T@@T....T@@T............T@@@@@@@@@@@@@@@@@@@@@T......TTTTTTT@@@@T@@@@@@@@@@@@@@@@@@@ 88 | @@@@@@@@@@@@@@@@@@@@@@@@@T....TTTT............T@@T....TTTTTTTTTTTTTTTTTTTTT...TTTT....TTTT............T@@@@@@@@@@@@@@@@@@@TTT.............TTTT.T@@@@@@@@@@@@@@@@@@ 89 | @@@@@@@@@@@@@@@@@@@@@@@@@T.....................T@T....................................................T@@@@@@@@@@@@@@@@@@T.....................T@@@@@@@@@@@@@@@@@@ 90 | @@@@@@@@@@@@@@@@@@@@@@@@@T.........TTTTTTTTTT..T@T....................................................T@@@@@@@@@@@@@@@@@@T.....................T@@@@@@@@@@@@@@@@@@ 91 | @@@@@@@@@@@@@@@@@@@@@@@@@T.........TTTTTTTTTT..T@T....................................................T@@@@@@@@@@@@@@@@@@T.....................T@@@@@@@@@@@@@@@@@@ 92 | @@@@@@@@@@@@@@@@@@@@@@@@@T.....................T@T....................................................T@@@@@@@@@@@@@@@@@@T.TT..................T@@@@@@@@@@@@@@@@@@ 93 | @@@@@@@@@@@@@@@@@@@@@@@@@T.....................T@T.............................................TT.....T@@@@@@@@@@@@@@@@@@@@@T....TTT...TT.....T@@@@@@@@@@@@@@@@@@@ 94 | @@@@@@@@@@@@@@@@@@@@@@@@@T.....................T@T.............................................TT.....T@@@@@@@@@@@@@@@@@@@@@@TTTT@@@TTT@@TTTTT@@@@@@@@@@@@@@@@@@@@ 95 | @@@@@@@@@@@@@@@@@@@@@@@@@T.....................T@T.............................................TT.....T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 96 | @@@@@@@@@@@@@@@@@@@@@@@@@T.........TTTTTTTTTT..T@T.............................................TT....T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 97 | @@@@@@@@@@@@@@@@@@@@@@@@@T.........TTTTTTTTTT..T@T.............................................TT....T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 98 | @@@@@@@@@@@@@@@@@@@@@@@@@T.....................T@T.............................................TT....T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 99 | @@@@@@@@@@@@@@@@@@@@@@@@@T.....................T@T.............................................TT....T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 100 | @@@@@@@@@@@@@@@@@@@@@@@@@T.....................T@T.........................TTTTTTTTTTTTTTTTTTTT@T....T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 101 | @@@@@@@@@@@@@@@@@@@@@@@@@T.....................T@@TTTTTTTTTTTTTTTTTTTTTTTTT@@@@@@@@@@@@@@@@@@@@@T....T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 102 | @@@@@@@@@@@@@@@@@@@@@@@@@T.....................T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T.....T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 103 | @@@@@@@@@@@@@@@@@@@@@@@@T......................TTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTT.....T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 104 | @@@@@@@@@@@@@@@@@@@@@@@@T..............................................................................T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 105 | @@@@@@@@@@@@@@@@@@@@@@@@T..............................................................................T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 106 | @@@@@@@@@@@@@@@@@@@@@@@@T..............................................................................T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 107 | @@@@@@@@@@@@@@@@@@@@@@@@T..............................................................................T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 108 | @@@@@@@@@@@@@@@@@@@@@@@@T..............................................................................T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 109 | @@@@@@@@@@@@@@@@@@@@@@@@T......TTTTTTTTTTTTTTTTT......................TTTTTTTTTTTTTTTTTTTTTTTTTT.......T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 110 | @@@@@@@@@@@@@@@@@@@@@@@@@TTTTTT@@@@@@@@@@@@@@@@@TT.....TTT.....TTT....T@@@@@@@@@@@@@@@@@@@@@@@@@TTTTTTT@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 111 | @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T.....T@T.....T@T....T@@@@@@@@@@@@@@@@@@@@@@@@@@TTTTTT@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 112 | @@@@@@@@@@@@@@@@@@@@@@@@@@@TTTTTTTTTTTT@@TTTTTTTTT.....T@T.....T@T....TTTTTTTTTTTTTTTTTTTTTTTTTTT......T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 113 | @@@@@@@@@@@@@@@@@@@@@@@@@@T............TT..............TT......TT......................................T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 114 | @@@@@@@@@@@@@@@@@@@@@@@@@@T............................................................................T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 115 | @@@@@@@@@@@@@@@@@@@@@@@@@@T.........................................................................TTT@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 116 | @@@@@@@@@@@@@@@@@@@@@@@@@@T............TT...........................................................T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 117 | @@@@@@@@@@@@@@@@@@@@@@@@@T.............TT...................................................T.......T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 118 | @@@@@@@@@@@@@@@@@@@@@@@@@T.............T@T.............................................TTTTT@TT.....T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 119 | @@@@@@@@@@@@@@@@@@@@@@@@@T............T@@T.............................................T@@@@@@T.....T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 120 | @@@@@@@@@@@@@@@@@@@@@@@@@T............T@@T.............................................TT@@TTTT.....T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 121 | @@@@@@@@@@@@@@@@@@@@@@@@@@T...........T@@T...............................................TT.........T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 122 | @@@@@@@@@@@@@@@@@@@@@@@@@@T...........T@@T..........................................................TTT@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 123 | @@@@@@@@@@@@@@@@@@@@@@@@@@T...........T@@T.............................................................T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 124 | @@@@@@@@@@@@@@@@@@@@@@@@@@@TT.........T@@T.............................................................T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 125 | @@@@@@@@@@@@@@@@@@@@@@@@@@@@@T.TTTTTTT@@@@TTTTTT.......TTTTTTTTTTT....................TTTTTTTTTT.......T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 126 | @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T@@@@@@@@@@@@@@@@@TTT..TT@@@@@@@@@@T....................T@@@@@@@@@TTTTTTT@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 127 | @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T..T@@@@@@@@@@@T....................T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 128 | @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T....T@@@@@@@@@@T....................T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 129 | @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...T@@@@@@@@@@@T....................T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 130 | @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTT@@@@@@@@@@@@T....................T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 131 | @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T....................T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 132 | @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T....................T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 133 | @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T....................T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 134 | @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T....................T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 135 | @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTTT............TTTT@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 136 | @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 137 | @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 138 | @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T.........T..T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 139 | @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T....TTTTT@TT@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 140 | @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T....T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 141 | @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTTT@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 142 | @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 143 | @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 144 | @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 145 | @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ 146 | -------------------------------------------------------------------------------- /examples/robot_nav_2d/resources/ht_chantry/nav2d_goals.txt: -------------------------------------------------------------------------------- 1 | 77.3383 88.6708 2 | 103.283 73.9245 3 | 98.6751 88.4293 4 | 75.1133 119.701 5 | 98.639 36.5058 6 | 98.7761 74.1303 7 | 37.8523 117.003 8 | 37.4213 54.3905 9 | 49.4761 103.974 10 | 83.9994 56.5 11 | 87.2462 20.9162 12 | 131.925 54.2368 13 | 100.791 26.9349 14 | 94.7028 67.166 15 | 52.0794 94.1824 16 | 40.5341 36.1281 17 | 75.7303 11.0344 18 | 73.3293 22.5828 19 | 81.389 81.6126 20 | 32.3553 40.5708 21 | 78.7103 27.1405 22 | 116.169 64.8695 23 | 83.325 60.124 24 | 71.1165 120.384 25 | 97.496 117.81 26 | 83.8173 53.4107 27 | 84.2592 126.245 28 | 117.284 65.4794 29 | 149.705 60.2486 30 | 97.0851 63.2134 31 | 116.024 77.1778 32 | 60.7459 78.7059 33 | 93.922 57.5868 34 | 80.6512 61.8883 35 | 96.7062 76.8288 36 | 97.7526 49.5037 37 | 81.003 65.8857 38 | 48.9604 39.9225 39 | 74.6247 118.348 40 | 74.6445 64.4611 41 | 120.494 74.2719 42 | 154.105 62.7561 43 | 32.338 95.5052 44 | 81.1145 95.5493 45 | 132.191 72.7155 46 | 146.626 72.6319 47 | 88.4798 102.275 48 | 66.1631 74.7184 49 | 52.8862 86.6951 50 | 79.5766 28.2237 51 | -------------------------------------------------------------------------------- /examples/robot_nav_2d/resources/ht_chantry/nav2d_starts.txt: -------------------------------------------------------------------------------- 1 | 44.9995 78.1098 461.823 18 2 | 99.2477 41.7405 321.941 13 3 | 56.2328 26.0318 999.765 40 4 | 140.062 69.4282 1015.53 39 5 | 63.881 92.8952 771.411 28 6 | 77.2067 23.9447 653.823 26 7 | 100.266 84.0637 913.706 36 8 | 51.1366 61.1952 1327.53 52 9 | 61.5948 53.8856 1255.53 49 10 | 100.188 90.6196 405.588 14 11 | 96.6976 91.0298 745.706 29 12 | 82.6765 6.83846 1037.12 37 13 | 90.8471 78.1772 543.765 21 14 | 95.0958 23.806 471.765 18 15 | 138.091 84.7075 959.294 35 16 | 85.3465 119.568 1244.88 44 17 | 132.864 71.5447 983.294 36 18 | 64.6866 29.8307 101.823 3 19 | 83.6144 88.0768 72 3 20 | 77.7239 38.1152 441.941 18 21 | 121.41 67.7019 745.706 29 22 | 74.0728 90.2919 507.411 17 23 | 35.5031 49.2547 943.529 36 24 | 29.1821 50.6538 935.294 34 25 | 66.2544 64.851 1049.47 40 26 | 59.4732 86.7842 537.941 22 27 | 100.354 97.8106 389.823 15 28 | 39.1092 95.2201 1039.53 40 29 | 127.526 72.9988 303.765 11 30 | 129.725 58.0287 341.823 13 31 | 129.621 55.1152 265.706 9 32 | 119.953 63.0095 973.352 36 33 | 71.1516 101.207 725.823 29 34 | 68.8133 72.3247 721.706 28 35 | 82.3558 89.5697 197.823 7 36 | 90.4808 87.9419 389.823 15 37 | 120.063 71.2528 403.882 16 38 | 61.0142 88.8795 1265.47 49 39 | 74.9606 133.085 144 6 40 | 36.0822 48.9674 1049.47 40 41 | 65.7586 46.0884 647.294 22 42 | 99.6249 85.6471 645.588 24 43 | 126.786 71.5962 1187.65 47 44 | 84.1816 27.1754 877.352 32 45 | 72.8776 101.129 807.765 32 46 | 85.2219 114.322 1245.59 49 47 | 97.0016 81.9029 283.882 11 48 | 111.7 69.6987 915.411 34 49 | 83.8092 79.6917 345.941 14 50 | 91.2486 78.8541 707.647 27 51 | -------------------------------------------------------------------------------- /include/common/Action.hpp: -------------------------------------------------------------------------------- 1 | #ifndef ACTION_HPP 2 | #define ACTION_HPP 3 | 4 | #include 5 | 6 | namespace ps 7 | { 8 | 9 | class Action 10 | { 11 | 12 | public: 13 | // Action(){}; 14 | Action(const std::string& type, ParamsType params = ParamsType(), bool is_expensive = true) 15 | : type_(type), params_(params), is_expensive_(is_expensive){}; 16 | virtual ~Action(){}; 17 | virtual bool CheckPreconditions(const StateVarsType& state)=0; 18 | virtual ActionSuccessor GetSuccessor(const StateVarsType& state_vars, int thread_id=0)=0; 19 | virtual ActionSuccessor GetSuccessorLazy(const StateVarsType& state_vars, int thread_id=0){}; 20 | virtual ActionSuccessor Evaluate(const StateVarsType& parent_state_vars, const StateVarsType& child_state_vars, int thread_id=0){}; 21 | std::string GetType() const {return type_;}; 22 | bool IsExpensive() const {return is_expensive_;}; 23 | bool operator==(const Action& other_action) const 24 | { 25 | return type_ == other_action.type_; 26 | } 27 | 28 | protected: 29 | std::string type_; 30 | ParamsType params_; 31 | bool is_expensive_; 32 | 33 | }; 34 | 35 | } 36 | 37 | #endif 38 | -------------------------------------------------------------------------------- /include/common/Edge.hpp: -------------------------------------------------------------------------------- 1 | #ifndef EDGE_HPP 2 | #define EDGE_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace ps 11 | { 12 | 13 | class Edge : public smpl::heap_element 14 | { 15 | public: 16 | // Edge(){}; 17 | Edge(StatePtrType parent_ptr, StatePtrType child_ptr, ActionPtrType action_ptr): 18 | parent_state_ptr_(parent_ptr), child_state_ptr_(child_ptr), action_ptr_(action_ptr), 19 | is_closed_(false), is_eval_(false), is_invalid_(false), 20 | expansion_priority_(-1), evaluation_priority_(-1), 21 | cost_(-1) 22 | {edge_id_ = id_counter_++;}; 23 | Edge(StatePtrType parent_ptr, ActionPtrType action_ptr): 24 | parent_state_ptr_(parent_ptr), child_state_ptr_(NULL), action_ptr_(action_ptr), 25 | is_closed_(false), is_eval_(false), is_invalid_(false), 26 | expansion_priority_(-1), evaluation_priority_(-1), 27 | cost_(-1) 28 | {edge_id_ = id_counter_++;}; 29 | Edge(const Edge& other_edge); 30 | Edge& operator=(const Edge& other_edge); 31 | bool operator==(const Edge& other_edge) const; 32 | 33 | void SetCost(double cost); 34 | double GetCost() const; 35 | 36 | void Print(std::string str=""); 37 | static void ResetStateIDCounter(){id_counter_=0;}; 38 | 39 | static std::size_t id_counter_; 40 | std::size_t edge_id_; 41 | 42 | StatePtrType parent_state_ptr_; 43 | StatePtrType child_state_ptr_; 44 | ActionPtrType action_ptr_; 45 | 46 | double expansion_priority_; 47 | double evaluation_priority_; 48 | 49 | std::atomic is_closed_; 50 | std::atomic is_eval_; 51 | std::atomic is_invalid_; 52 | 53 | private: 54 | double cost_; 55 | mutable std::mutex lock_; 56 | double roundOff(double value, int prec=3); 57 | }; 58 | 59 | class IsLesserEdge 60 | { 61 | public: 62 | bool operator() (const Edge& lhs, const Edge& rhs); 63 | }; 64 | 65 | class IsGreaterEdge 66 | { 67 | public: 68 | bool operator() (const Edge& lhs, const Edge& rhs); 69 | }; 70 | 71 | } 72 | 73 | #endif -------------------------------------------------------------------------------- /include/common/State.hpp: -------------------------------------------------------------------------------- 1 | #ifndef STATE_HPP 2 | #define STATE_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace ps 11 | { 12 | 13 | class State : public smpl::heap_element 14 | { 15 | 16 | public: 17 | 18 | State(const StateVarsType& vars=StateVarsType()); 19 | ~State() {}; 20 | 21 | std::size_t GetStateID() const {return state_id_;}; 22 | static void ResetStateIDCounter() {id_counter_=0;}; 23 | 24 | void SetStateVars(StateVarsType& vars) {vars_ = vars;}; 25 | StateVarsType GetStateVars() const {return vars_;}; 26 | 27 | void SetGValue(const double& g_val) {g_val_ = g_val;}; 28 | double GetGValue() const {return g_val_;}; 29 | void ResetGValue() {g_val_ = std::numeric_limits::max();}; 30 | 31 | void SetVValue(const double& v_val) {v_val_ = v_val;}; 32 | double GetVValue() const {return v_val_;}; 33 | void ResetVValue() {v_val_ = std::numeric_limits::max();}; 34 | 35 | void SetHValue(const double& h_val) {h_val_ = h_val;}; 36 | double GetHValue() const {return h_val_;}; 37 | void ResetHValue() {h_val_ = -1;}; 38 | 39 | void SetFValue(const double& f_val) {f_val_ = f_val;}; 40 | double GetFValue() const {return f_val_;}; 41 | void ResetFValue() {f_val_ = std::numeric_limits::max();}; 42 | 43 | void SetVisited() {is_visited_ = true;}; 44 | void UnsetVisited() {is_visited_ = false;}; 45 | bool IsVisited() {return is_visited_;}; 46 | 47 | void SetBeingExpanded() {being_expanded_ = true;}; 48 | void UnsetBeingExpanded() {being_expanded_ = false;}; 49 | bool IsBeingExpanded() {return being_expanded_;}; 50 | 51 | void SetIncomingEdgePtr(EdgePtrType edge_ptr) {incoming_edge_ptr_ = edge_ptr;}; 52 | EdgePtrType GetIncomingEdgePtr() {return incoming_edge_ptr_;}; 53 | void ResetIncomingEdgePtr() {incoming_edge_ptr_ = NULL;}; 54 | 55 | void Print(std::string str=""); 56 | 57 | std::atomic num_successors_; 58 | std::atomic num_expanded_successors_; 59 | 60 | protected: 61 | 62 | private: 63 | static std::size_t id_counter_; 64 | 65 | std::size_t state_id_; 66 | StateVarsType vars_; 67 | double g_val_; 68 | double h_val_; 69 | double f_val_; 70 | double v_val_; 71 | std::atomic is_visited_; 72 | std::atomic being_expanded_; 73 | EdgePtrType incoming_edge_ptr_; 74 | }; 75 | 76 | class IsLesserState 77 | { 78 | public: 79 | bool operator() (const State& lhs, const State& rhs); 80 | }; 81 | 82 | class IsGreaterState 83 | { 84 | public: 85 | bool operator() (const State& lhs, const State& rhs); 86 | }; 87 | 88 | } 89 | 90 | 91 | 92 | #endif 93 | -------------------------------------------------------------------------------- /include/common/Types.hpp: -------------------------------------------------------------------------------- 1 | #ifndef TYPES_HPP 2 | #define TYPES_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include "intrusive_heap.h" 10 | 11 | namespace ps 12 | { 13 | class State; 14 | class Edge; 15 | class Action; 16 | 17 | #define DINF std::numeric_limits::infinity() 18 | 19 | typedef std::vector StateVarsType; 20 | typedef State* StatePtrType; 21 | typedef std::shared_ptr ActionPtrType; 22 | typedef Edge* EdgePtrType; 23 | typedef std::mutex LockType; 24 | typedef std::unordered_map ParamsType; 25 | 26 | struct ActionSuccessor 27 | { 28 | ActionSuccessor(){}; 29 | ActionSuccessor(bool success, std::vector> successor_state_vars_costs): 30 | success_(success), successor_state_vars_costs_(successor_state_vars_costs){}; 31 | bool success_; 32 | std::vector> successor_state_vars_costs_; 33 | }; 34 | 35 | struct PlanElement 36 | { 37 | PlanElement(StateVarsType state, ActionPtrType action_ptr, double cost): state_(state), incoming_action_ptr_(action_ptr), cost_(cost) {}; 38 | ~PlanElement(){}; 39 | StateVarsType state_; 40 | ActionPtrType incoming_action_ptr_; 41 | double cost_; 42 | }; 43 | 44 | struct PlannerStats 45 | { 46 | double total_time = 0; // seconds 47 | double path_cost = 0; 48 | double path_length = 0; 49 | double processed_path_cost = 0; 50 | double processed_path_length = 0; 51 | 52 | int num_state_expansions = 0; 53 | int num_evaluated_edges = 0; 54 | int num_threads_spawned = 0; 55 | 56 | std::vector num_jobs_per_thread; 57 | 58 | double lock_time = 0; 59 | double cumulative_expansions_time = 0; 60 | 61 | std::unordered_map> action_eval_times; 62 | }; 63 | } 64 | 65 | #endif 66 | -------------------------------------------------------------------------------- /include/common/intrusive_heap.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // Copyright (c) 2016, Andrew Dornbush 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are met: 7 | // 8 | // 1. Redistributions of source code must retain the above copyright notice 9 | // this list of conditions and the following disclaimer. 10 | // 2. Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 3. Neither the name of the copyright holder nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | //////////////////////////////////////////////////////////////////////////////// 29 | 30 | /// \author Andrew Dornbush 31 | 32 | #ifndef SMPL_INTRUSIVE_HEAP_H 33 | #define SMPL_INTRUSIVE_HEAP_H 34 | 35 | #include 36 | #include 37 | 38 | namespace smpl { 39 | 40 | template 41 | class intrusive_heap; 42 | 43 | struct heap_element 44 | { 45 | 46 | heap_element() : m_heap_index(0) { } 47 | 48 | private: 49 | 50 | std::size_t m_heap_index; 51 | 52 | template 53 | friend class intrusive_heap; 54 | }; 55 | 56 | /// Provides an intrusive binary heap implementation. Objects inserted into the 57 | /// heap must derive from the \p heap_element class to implement efficient 58 | /// mutability. The implementation stores pointers to inserted objects, which 59 | /// must remain valid throughout the lifetime of the heap. 60 | /// 61 | /// The binary heap data structure provides constant time access to the minimum 62 | /// element of the heap, logarithmic insertion and erasure of elements, 63 | /// logarithmic updates of element priorities, and linear time construction from 64 | /// a new set of elements. All times are proportional to the number of elements 65 | /// in the heap. 66 | /// 67 | /// Priorities of elements in the heap are not explicitly stored, but determined 68 | /// from the result of calling the \p Compare function object on two elements. 69 | /// If the priorities of multiple elements are implicitly changed (via external 70 | /// modification of the function object), the heap may be reordered in-place 71 | /// in linear time by calling the make() member function. 72 | template 73 | class intrusive_heap 74 | { 75 | public: 76 | 77 | static_assert(std::is_base_of::value, "T must extend heap_element"); 78 | 79 | typedef Compare compare; 80 | 81 | typedef std::vector container_type; 82 | typedef typename container_type::size_type size_type; 83 | 84 | typedef typename container_type::iterator iterator; 85 | typedef typename container_type::const_iterator const_iterator; 86 | 87 | intrusive_heap(const compare& comp = compare()); 88 | 89 | template 90 | intrusive_heap(InputIt first, InputIt last); 91 | 92 | template 93 | intrusive_heap(const compare& comp, InputIt first, InputIt last); 94 | 95 | intrusive_heap(const intrusive_heap&) = delete; 96 | 97 | intrusive_heap(intrusive_heap&& o); 98 | 99 | intrusive_heap& operator=(const intrusive_heap&) = delete; 100 | intrusive_heap& operator=(intrusive_heap&& rhs); 101 | 102 | T* min() const; 103 | 104 | const_iterator begin() const; 105 | const_iterator end() const; 106 | 107 | bool empty() const; 108 | size_type size() const; 109 | size_type max_size() const; 110 | void reserve(size_type new_cap); 111 | 112 | void clear(); 113 | void push(T* e); 114 | void pop(); 115 | bool contains(T* e); 116 | void update(T* e); 117 | void increase(T* e); 118 | void decrease(T* e); 119 | void erase(T* e); 120 | 121 | void make(); 122 | 123 | void swap(intrusive_heap& o); 124 | 125 | private: 126 | 127 | container_type m_data; 128 | Compare m_comp; 129 | 130 | size_type ipow2(size_type i); 131 | size_type ilog2(size_type i); 132 | bool ispow2(size_type val); 133 | 134 | template 135 | void make_heap(InputIt first, InputIt last); 136 | 137 | template 138 | void make_heap(InputIt first, InputIt last, size_type root); 139 | 140 | size_type parent(size_type index) const; 141 | size_type right_child(size_type index) const; 142 | size_type left_child(size_type index) const; 143 | 144 | void percolate_down(size_type pivot); 145 | void percolate_up(size_type pivot); 146 | 147 | bool is_internal(size_type index) const; 148 | bool is_external(size_type index) const; 149 | 150 | void print() const; 151 | 152 | bool check_heap() const; 153 | bool check_heap(size_type index) const; 154 | }; 155 | 156 | template 157 | void swap(intrusive_heap& lhs, intrusive_heap& rhs); 158 | 159 | } // namespace smpl 160 | 161 | #include "intrusive_heap.hpp" 162 | 163 | #endif 164 | -------------------------------------------------------------------------------- /include/common/intrusive_heap.hpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // Copyright (c) 2016, Andrew Dornbush 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are met: 7 | // 8 | // 1. Redistributions of source code must retain the above copyright notice 9 | // this list of conditions and the following disclaimer. 10 | // 2. Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 3. Neither the name of the copyright holder nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | //////////////////////////////////////////////////////////////////////////////// 29 | 30 | /// \author Andrew Dornbush 31 | 32 | #ifndef SMPL_INTRUSIVE_HEAP_HPP 33 | #define SMPL_INTRUSIVE_HEAP_HPP 34 | 35 | #include "intrusive_heap.h" 36 | 37 | #include 38 | #include 39 | 40 | namespace smpl { 41 | 42 | template 43 | intrusive_heap::intrusive_heap(const compare& comp) : 44 | m_data(1, nullptr), 45 | m_comp(comp) 46 | { 47 | } 48 | 49 | template 50 | template 51 | intrusive_heap::intrusive_heap( 52 | const compare& comp, 53 | InputIt first, 54 | InputIt last) 55 | : 56 | m_data(), 57 | m_comp(comp) 58 | { 59 | make_heap(first, last); 60 | } 61 | 62 | template 63 | template 64 | intrusive_heap::intrusive_heap(InputIt first, InputIt last) : 65 | m_data(), 66 | m_comp() 67 | { 68 | make_heap(first, last); 69 | } 70 | 71 | template 72 | intrusive_heap::intrusive_heap(intrusive_heap&& o) : 73 | m_data(std::move(o.m_data)), 74 | m_comp(std::move(o.m_comp)) 75 | { 76 | } 77 | 78 | template 79 | intrusive_heap& 80 | intrusive_heap::operator=(intrusive_heap&& rhs) 81 | { 82 | if (this != &rhs) { 83 | m_data = std::move(rhs.m_data); 84 | m_comp = std::move(rhs.m_comp); 85 | } 86 | return *this; 87 | } 88 | 89 | template 90 | T* intrusive_heap::min() const 91 | { 92 | assert(m_data.size() > 1); 93 | return m_data[1]; 94 | } 95 | 96 | template 97 | typename intrusive_heap::const_iterator 98 | intrusive_heap::begin() const 99 | { 100 | return m_data.begin() + 1; 101 | } 102 | 103 | template 104 | typename intrusive_heap::const_iterator 105 | intrusive_heap::end() const 106 | { 107 | return m_data.end(); 108 | } 109 | 110 | template 111 | bool intrusive_heap::empty() const 112 | { 113 | return m_data.size() == 1; 114 | } 115 | 116 | template 117 | typename intrusive_heap::size_type 118 | intrusive_heap::size() const 119 | { 120 | return m_data.size() - 1; 121 | } 122 | 123 | template 124 | typename intrusive_heap::size_type 125 | intrusive_heap::max_size() const 126 | { 127 | return m_data.max_size() - 1; 128 | } 129 | 130 | template 131 | void intrusive_heap::reserve(size_type new_cap) 132 | { 133 | m_data.reserve(new_cap + 1); 134 | } 135 | 136 | template 137 | void intrusive_heap::clear() 138 | { 139 | for (size_t i = 1; i < m_data.size(); ++i) { 140 | m_data[i]->m_heap_index = 0; 141 | } 142 | m_data.resize(1); 143 | } 144 | 145 | template 146 | void intrusive_heap::push(T* e) 147 | { 148 | assert(e); 149 | e->m_heap_index = m_data.size(); 150 | m_data.push_back(e); 151 | percolate_up(m_data.size() - 1); 152 | } 153 | 154 | template 155 | void intrusive_heap::pop() 156 | { 157 | assert(!empty()); 158 | m_data[1]->m_heap_index = 0; 159 | m_data[1] = m_data.back(); 160 | // NOTE: no need to set heap index here, as it is not required in 161 | // percolate_down(); also produces incorrect behavior when popping from a 162 | // heap of size 1 without the required identity check here 163 | // m_data[1]->m_heap_index = 1; 164 | m_data.pop_back(); 165 | percolate_down(1); 166 | } 167 | 168 | template 169 | bool intrusive_heap::contains(T* e) 170 | { 171 | assert(e); 172 | return e->m_heap_index != 0; 173 | } 174 | 175 | template 176 | void intrusive_heap::update(T* e) 177 | { 178 | assert(e && contains(e)); 179 | percolate_up(e->m_heap_index); 180 | percolate_down(e->m_heap_index); 181 | } 182 | 183 | template 184 | void intrusive_heap::increase(T* e) 185 | { 186 | assert(e && contains(e)); 187 | percolate_down(e->m_heap_index); 188 | } 189 | 190 | template 191 | void intrusive_heap::decrease(T* e) 192 | { 193 | assert(e && contains(e)); 194 | percolate_up(e->m_heap_index); 195 | } 196 | 197 | template 198 | void intrusive_heap::erase(T* e) 199 | { 200 | assert(e && contains(e)); 201 | size_type pos = e->m_heap_index; 202 | m_data[pos] = m_data.back(); 203 | m_data[pos]->m_heap_index = pos; 204 | e->m_heap_index = 0; 205 | m_data.pop_back(); 206 | if (pos < m_data.size()) { 207 | update(m_data[pos]); 208 | } 209 | } 210 | 211 | template 212 | void intrusive_heap::make() 213 | { 214 | for (auto i = (m_data.size() - 1) >> 1; i >= 1; --i) { 215 | percolate_down(i); 216 | } 217 | } 218 | 219 | template 220 | void intrusive_heap::swap(intrusive_heap& o) 221 | { 222 | if (this != &o) { 223 | using std::swap; 224 | swap(m_data, o.m_data); 225 | swap(m_comp, o.m_comp); 226 | } 227 | } 228 | 229 | template 230 | inline 231 | typename intrusive_heap::size_type 232 | intrusive_heap::ipow2(size_type exp) 233 | { 234 | if (exp == 0) { 235 | return 1; 236 | } 237 | 238 | size_type res = ipow2(exp >> 1) * ipow2(exp >> 1); 239 | if (exp % 2) { 240 | res *= 2; 241 | } 242 | return res; 243 | } 244 | 245 | template 246 | inline 247 | typename intrusive_heap::size_type 248 | intrusive_heap::ilog2(size_type i) 249 | { 250 | std::size_t r = 0; 251 | while (i >>= 1) { 252 | ++r; 253 | } 254 | return r; 255 | } 256 | 257 | template 258 | inline 259 | bool intrusive_heap::ispow2(size_type val) 260 | { 261 | // does not check for val == 0 262 | return !(val & (val - 1)); 263 | } 264 | 265 | template 266 | template 267 | void intrusive_heap::make_heap(InputIt first, InputIt last) 268 | { 269 | auto n = std::distance(first, last); 270 | 271 | m_data.clear(); 272 | m_data.reserve(n + 1); 273 | m_data.push_back(nullptr); 274 | m_data.insert(m_data.end(), first, last); 275 | 276 | for (size_type i = 1; i < m_data.size(); ++i) { 277 | m_data[i]->m_heap_index = i; 278 | } 279 | 280 | for (auto i = n >> 1; i >= 1; --i) { 281 | percolate_down(i); 282 | } 283 | } 284 | 285 | template 286 | template 287 | void intrusive_heap::make_heap( 288 | InputIt first, 289 | InputIt last, 290 | size_type root) 291 | { 292 | const auto n = std::distance(first, last); 293 | printf("make heap from %d elements from %zu\n", n, root); 294 | print(); 295 | 296 | if (n <= 0) { 297 | return; 298 | } 299 | 300 | printf(" -> data[%zu] = %p\n", root, *first); 301 | m_data[root] = *first; 302 | m_data[root]->m_heap_index = root; 303 | 304 | if (n == 1) { 305 | return; 306 | } 307 | 308 | const size_type left = left_child(root); 309 | const size_type right = right_child(root); 310 | 311 | auto f = ilog2(n) - 1; 312 | size_type f2 = ipow2(f); 313 | size_type l = f2 - 1 + std::min(n - 2 * f2 + 1, f2); 314 | size_type r = n - 1 - l; 315 | 316 | InputIt new_start = std::next(first); 317 | InputIt mid = std::next(new_start, l); 318 | 319 | make_heap(new_start, mid, left); 320 | make_heap(mid, last, right); 321 | percolate_down(root); 322 | } 323 | 324 | template 325 | inline 326 | typename intrusive_heap::size_type 327 | intrusive_heap::parent(size_type index) const 328 | { 329 | return index >> 1; 330 | } 331 | 332 | template 333 | inline 334 | typename intrusive_heap::size_type 335 | intrusive_heap::right_child(size_type index) const 336 | { 337 | return (index << 1) + 1; 338 | } 339 | 340 | template 341 | inline 342 | typename intrusive_heap::size_type 343 | intrusive_heap::left_child(size_type index) const 344 | { 345 | return index << 1; 346 | } 347 | 348 | template 349 | inline 350 | void intrusive_heap::percolate_down(size_type pivot) 351 | { 352 | if (is_external(pivot)) { 353 | return; 354 | } 355 | 356 | size_type left = left_child(pivot); 357 | size_type right = right_child(pivot); 358 | 359 | T* tmp = m_data[pivot]; 360 | while (is_internal(left)) { 361 | size_type s = right; 362 | if (is_external(right) || m_comp(*m_data[left], *m_data[right])) { 363 | s = left; 364 | } 365 | 366 | if (m_comp(*m_data[s], *tmp)) { 367 | m_data[pivot] = m_data[s]; 368 | m_data[pivot]->m_heap_index = pivot; 369 | pivot = s; 370 | } else { 371 | break; 372 | } 373 | 374 | left = left_child(pivot); 375 | right = right_child(pivot); 376 | } 377 | m_data[pivot] = tmp; 378 | m_data[pivot]->m_heap_index = pivot; 379 | } 380 | 381 | template 382 | inline 383 | void intrusive_heap::percolate_up(size_type pivot) 384 | { 385 | T* tmp = m_data[pivot]; 386 | while (pivot != 1) { 387 | size_type p = parent(pivot); 388 | if (m_comp(*m_data[p], *tmp)) { 389 | break; 390 | } 391 | m_data[pivot] = m_data[p]; 392 | m_data[pivot]->m_heap_index = pivot; 393 | pivot = p; 394 | } 395 | m_data[pivot] = tmp; 396 | m_data[pivot]->m_heap_index = pivot; 397 | } 398 | 399 | template 400 | inline 401 | bool intrusive_heap::is_internal(size_type index) const 402 | { 403 | return index < m_data.size(); 404 | } 405 | 406 | template 407 | inline 408 | bool intrusive_heap::is_external(size_type index) const 409 | { 410 | return index >= m_data.size(); 411 | } 412 | 413 | template 414 | void intrusive_heap::print() const 415 | { 416 | printf("[ null, "); 417 | for (int i = 1; i < m_data.size(); ++i) { 418 | printf(" (%d, %p)", m_data[i]->m_heap_index, m_data[i]); 419 | if (i == m_data.size() - 1) { 420 | printf(" "); 421 | } else { 422 | printf(", "); 423 | } 424 | } 425 | printf("]\n"); 426 | } 427 | 428 | template 429 | bool intrusive_heap::check_heap(size_type index) const 430 | { 431 | auto right = right_child(index); 432 | if (is_internal(right)) { 433 | // check the ordering of the parent and the right child 434 | if (!m_comp(*m_data[index], *m_data[right])) { 435 | return false; 436 | } 437 | 438 | // check the tree rooted at the right child 439 | if (!check_heap(right)) { 440 | return false; 441 | } 442 | } 443 | 444 | auto left = left_child(index); 445 | if (is_internal(left)) { 446 | // check the ordering of the parent and the left child 447 | if (!m_comp(*m_data[index], *m_data[left])) { 448 | return false; 449 | } 450 | 451 | // check the tree rooted at the left child 452 | if (!check_heap(left)) { 453 | return false; 454 | } 455 | } 456 | 457 | return true; 458 | } 459 | 460 | template 461 | bool intrusive_heap::check_heap() const 462 | { 463 | if (empty()) { 464 | return true; 465 | } 466 | 467 | return check_heap(1); 468 | } 469 | 470 | template 471 | void swap(intrusive_heap& lhs, intrusive_heap& rhs) 472 | { 473 | lhs.swap(rhs); 474 | } 475 | 476 | } // namespace smpl 477 | 478 | #endif 479 | -------------------------------------------------------------------------------- /include/planners/AgepasePlanner.hpp: -------------------------------------------------------------------------------- 1 | #ifndef AGEPASE_PLANNER_HPP 2 | #define AGEPASE_PLANNER_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace ps 9 | { 10 | 11 | class AgepasePlanner : public GepasePlanner 12 | { 13 | public: 14 | AgepasePlanner(ParamsType planner_params); 15 | ~AgepasePlanner(); 16 | bool Plan(); 17 | 18 | protected: 19 | void initialize(); 20 | void improvePath(); 21 | void expand(EdgePtrType edge_ptr, int thread_id); 22 | void expandEdge(EdgePtrType edge_ptr, int thread_id); 23 | void updateEdge(EdgePtrType edge_ptr, StatePtrType successor_state_ptr, double new_g_val); 24 | void exitMultiThread(); 25 | void exit(); 26 | 27 | std::vector edge_incon_list_; 28 | double delta_w_; 29 | double best_cost_; 30 | std::vector best_plan_; 31 | 32 | }; 33 | 34 | } 35 | 36 | #endif 37 | -------------------------------------------------------------------------------- /include/planners/ArastarPlanner.hpp: -------------------------------------------------------------------------------- 1 | #ifndef ARASTAR_PLANNER_HPP 2 | #define ARASTAR_PLANNER_HPP 3 | 4 | #include 5 | #include 6 | 7 | namespace ps 8 | { 9 | 10 | class ArastarPlanner : public WastarPlanner 11 | { 12 | public: 13 | ArastarPlanner(ParamsType planner_params); 14 | ~ArastarPlanner(); 15 | bool Plan(); 16 | 17 | protected: 18 | void initialize(); 19 | void improvePath(); 20 | void expandState(StatePtrType state_ptr); 21 | void updateState(StatePtrType& state_ptr, ActionPtrType& action_ptr, EdgePtrType& edge_ptr); 22 | void updateEdge(EdgePtrType edge_ptr, StatePtrType successor_state_ptr, double new_g_val); 23 | void exit(); 24 | 25 | std::vector state_incon_list_; 26 | double delta_w_; 27 | }; 28 | 29 | } 30 | 31 | #endif 32 | -------------------------------------------------------------------------------- /include/planners/EpasePlanner.hpp: -------------------------------------------------------------------------------- 1 | #ifndef EPASE_PLANNER_HPP 2 | #define EPASE_PLANNER_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace ps 9 | { 10 | 11 | class EpasePlanner : public GepasePlanner 12 | { 13 | public: 14 | EpasePlanner(ParamsType planner_params); 15 | ~EpasePlanner(); 16 | bool Plan(); 17 | 18 | protected: 19 | void expandEdgeLoop(int thread_id); 20 | void expandEdge(EdgePtrType edge_ptr, int thread_id); 21 | void exit(); 22 | 23 | std::vector being_expanded_states_; 24 | }; 25 | 26 | } 27 | 28 | #endif 29 | -------------------------------------------------------------------------------- /include/planners/GepasePlanner.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GEPASE_PLANNER_HPP 2 | #define GEPASE_PLANNER_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace ps 9 | { 10 | 11 | class GepasePlanner : public Planner 12 | { 13 | public: 14 | typedef smpl::intrusive_heap BEType; 15 | GepasePlanner(ParamsType planner_params); 16 | ~GepasePlanner(); 17 | virtual bool Plan(); 18 | 19 | protected: 20 | void initialize(); 21 | void notifyMainThread(); 22 | void expandEdgeLoop(int thread_id); 23 | virtual void expand(EdgePtrType edge_ptr, int thread_id); 24 | void expandEdge(EdgePtrType edge_ptr, int thread_id); 25 | void exit(); 26 | 27 | EdgeQueueMinType edge_open_list_; 28 | BEType being_expanded_states_; 29 | 30 | // Multi-threading members 31 | int num_threads_; 32 | mutable LockType lock_; 33 | mutable std::vector lock_vec_; 34 | std::vector> edge_expansion_futures_; 35 | std::vector edge_expansion_vec_; 36 | std::vector edge_expansion_status_; 37 | std::vector cv_vec_; 38 | std::condition_variable cv_; 39 | 40 | // Control variables 41 | std::atomic recheck_flag_; 42 | std::atomic terminate_; 43 | ActionPtrType dummy_action_ptr_; 44 | }; 45 | 46 | } 47 | 48 | #endif 49 | -------------------------------------------------------------------------------- /include/planners/MplpPlanner.hpp: -------------------------------------------------------------------------------- 1 | #ifndef MPLP_PLANNER_HPP 2 | #define MPLP_PLANNER_HPP 3 | 4 | #include 5 | #include 6 | 7 | namespace ps 8 | { 9 | 10 | class MplpPlanner : public Planner 11 | { 12 | public: 13 | MplpPlanner(ParamsType planner_params); 14 | ~MplpPlanner(); 15 | bool Plan(); 16 | 17 | protected: 18 | void initialize(); 19 | void initializeReplanning(); 20 | bool replanMPLP(); 21 | void expandState(StatePtrType state_ptr); 22 | void delegateEdges(); 23 | void evaluateEdge(EdgePtrType edge_ptr, int thread_id); 24 | void evaluateEdgeLoop(int thread_id); 25 | void constructPlan(StatePtrType goal_state_ptr); 26 | void monitorPaths(); 27 | void assignEdgePriority(EdgePtrType& edge_ptr); 28 | void updateEdgePriority(std::vector& edge_ptr, double factor); 29 | void updateEdgePriority(EdgePtrType& edge_ptr, double factor); 30 | void exit(); 31 | 32 | StateQueueMinType state_open_list_; 33 | EdgeQueueMaxType edges_open_; 34 | std::vector> lazy_plans_; 35 | // -1: no outcome, 1: feasible , 0: infeasible 36 | std::vector plan_evaluation_outcomes_; 37 | int successful_plan_idx_; 38 | double cost_bound_; 39 | 40 | // Multi-threading members 41 | int num_threads_; 42 | mutable LockType lock_; 43 | mutable LockType lock_2_; 44 | mutable std::vector lock_vec_; 45 | std::vector> edge_evaluation_futures_; 46 | std::vector edge_evaluation_vec_; 47 | std::vector edge_evaluation_status_; 48 | std::shared_ptr delegate_edges_process_; 49 | std::shared_ptr monitor_paths_process_; 50 | 51 | // Control variables 52 | std::atomic terminate_; 53 | std::atomic plan_found_; 54 | }; 55 | 56 | } 57 | 58 | #endif 59 | -------------------------------------------------------------------------------- /include/planners/PasePlanner.hpp: -------------------------------------------------------------------------------- 1 | #ifndef PASE_PLANNER_HPP 2 | #define PASE_PLANNER_HPP 3 | 4 | #include 5 | #include 6 | 7 | namespace ps 8 | { 9 | 10 | class PasePlanner : public Planner 11 | { 12 | public: 13 | PasePlanner(ParamsType planner_params); 14 | ~PasePlanner(); 15 | bool Plan(); 16 | 17 | protected: 18 | void initialize(); 19 | void paseThread(int thread_id); 20 | void expandState(StatePtrType state_ptr, int thread_id); 21 | void exit(); 22 | 23 | StateQueueMinType state_open_list_; 24 | std::vector being_expanded_states_; 25 | 26 | // Multi-threading members 27 | int num_threads_; 28 | mutable LockType lock_; 29 | mutable std::vector lock_vec_; 30 | std::vector> state_expansion_futures_; 31 | 32 | // Control variables 33 | std::atomic recheck_flag_; 34 | std::atomic terminate_; 35 | std::atomic plan_found_; 36 | 37 | }; 38 | 39 | } 40 | 41 | #endif -------------------------------------------------------------------------------- /include/planners/Planner.hpp: -------------------------------------------------------------------------------- 1 | #ifndef PLANNER_HPP 2 | #define PLANNER_HPP 3 | 4 | #define VERBOSE 0 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | namespace ps 12 | { 13 | 14 | class Planner 15 | { 16 | public: 17 | 18 | // Typedefs 19 | typedef std::unordered_map StatePtrMapType; 20 | // Lower priority states will be in the front 21 | typedef smpl::intrusive_heap StateQueueMinType; 22 | typedef std::unordered_map EdgePtrMapType; 23 | // Higher priority edge will be in the front 24 | typedef smpl::intrusive_heap EdgeQueueMaxType; 25 | // Lower priority edge will be in the front 26 | typedef smpl::intrusive_heap EdgeQueueMinType; 27 | 28 | Planner(ParamsType planner_params); 29 | virtual ~Planner(); 30 | 31 | virtual bool Plan() = 0; 32 | std::vector GetPlan() const; 33 | PlannerStats GetStats() const; 34 | 35 | void SetActions(std::vector> actions_ptrs); 36 | void SetStartState(const StateVarsType& state_vars); 37 | void SetGoalChecker(std::function callback); 38 | 39 | void SetStateMapKeyGenerator(std::function callback); 40 | void SetEdgeKeyGenerator(std::function callback); 41 | void SetHeuristicGenerator(std::function callback); 42 | void SetStateToStateHeuristicGenerator(std::function callback); 43 | void SetPostProcessor(std::function&, double&)> callback); 44 | 45 | protected: 46 | 47 | virtual void initialize(); 48 | void startTimer(); 49 | bool checkTimeout(); 50 | void resetStates(); 51 | void resetClosed(); 52 | StatePtrType constructState(const StateVarsType& state); 53 | size_t getEdgeKey(const EdgePtrType& edge_ptr); 54 | double computeHeuristic(const StatePtrType& state_ptr); 55 | double computeHeuristic(const StatePtrType& state_ptr_1, const StatePtrType& state_ptr_2); 56 | bool isGoalState(const StatePtrType& state_ptr); 57 | void constructPlan(StatePtrType& state_ptr); 58 | 59 | // Utilities 60 | template bool isFutureReady(T& future){return future.wait_for(std::chrono::milliseconds(0)) == std::future_status::ready;}; 61 | double roundOff(double value, int prec=3); 62 | void cleanUp(); 63 | virtual void exit(); 64 | 65 | std::unordered_map planner_params_; 66 | std::vector> actions_ptrs_; 67 | 68 | StatePtrMapType state_map_; 69 | EdgePtrMapType edge_map_; 70 | StatePtrType start_state_ptr_; 71 | StatePtrType goal_state_ptr_; 72 | double heuristic_w_; 73 | 74 | std::function state_key_generator_; 75 | std::function edge_key_generator_; 76 | std::function unary_heuristic_generator_; 77 | std::function binary_heuristic_generator_; 78 | std::function goal_checker_; 79 | std::function&, double&)> post_processor_; 80 | 81 | // Statistics 82 | std::vector plan_; 83 | PlannerStats planner_stats_; 84 | double h_val_min_; 85 | 86 | std::chrono::time_point t_start_; 87 | }; 88 | 89 | } 90 | 91 | #endif 92 | -------------------------------------------------------------------------------- /include/planners/PwastarPlanner.hpp: -------------------------------------------------------------------------------- 1 | #ifndef PWASTAR_PLANNER_HPP 2 | #define PWASTAR_PLANNER_HPP 3 | 4 | #include 5 | #include 6 | 7 | namespace ps 8 | { 9 | 10 | class PwastarPlanner : public WastarPlanner 11 | { 12 | public: 13 | PwastarPlanner(ParamsType planner_params); 14 | ~PwastarPlanner(); 15 | bool Plan(); 16 | 17 | protected: 18 | void initialize(); 19 | void expandState(StatePtrType state_ptr); 20 | void evaluateEdgeThread(int thread_idx); 21 | void exit(); 22 | 23 | mutable std::vector lock_vec_; 24 | std::vector> edge_evaluation_futures_; 25 | std::vector edge_evaluation_vec_; 26 | std::vector edge_evaluation_status_; 27 | std::vector>> all_successors_; 28 | 29 | bool terminate_; 30 | 31 | }; 32 | 33 | } 34 | 35 | #endif 36 | -------------------------------------------------------------------------------- /include/planners/WastarPlanner.hpp: -------------------------------------------------------------------------------- 1 | #ifndef WASTAR_PLANNER_HPP 2 | #define WASTAR_PLANNER_HPP 3 | 4 | #include 5 | #include 6 | 7 | namespace ps 8 | { 9 | 10 | class WastarPlanner : public Planner 11 | { 12 | public: 13 | WastarPlanner(ParamsType planner_params); 14 | ~WastarPlanner(); 15 | virtual bool Plan(); 16 | 17 | protected: 18 | void initialize(); 19 | void expandState(StatePtrType state_ptr); 20 | void updateState(StatePtrType& state_ptr, ActionPtrType& action_ptr, ActionSuccessor& action_successor); 21 | void exit(); 22 | 23 | int num_threads_; 24 | StateQueueMinType state_open_list_; 25 | 26 | }; 27 | 28 | } 29 | 30 | #endif 31 | -------------------------------------------------------------------------------- /src/common/Edge.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | using namespace std; 6 | using namespace ps; 7 | 8 | size_t Edge::id_counter_ = 0; 9 | 10 | void Edge::SetCost(double cost) 11 | { 12 | lock_.lock(); 13 | cost_ = roundOff(cost); 14 | lock_.unlock(); 15 | } 16 | 17 | double Edge::GetCost() const 18 | { 19 | double cost_local; 20 | lock_.lock(); 21 | cost_local = cost_; 22 | lock_.unlock(); 23 | return cost_local; 24 | } 25 | 26 | Edge::Edge(const Edge& other_edge) 27 | { 28 | parent_state_ptr_ = other_edge.parent_state_ptr_; 29 | child_state_ptr_ = other_edge.child_state_ptr_; 30 | action_ptr_ = other_edge.action_ptr_; 31 | expansion_priority_ = other_edge.expansion_priority_; 32 | evaluation_priority_ = other_edge.evaluation_priority_; 33 | is_closed_ = other_edge.is_closed_.load(); 34 | is_eval_ = other_edge.is_eval_.load(); 35 | is_invalid_ = other_edge.is_invalid_.load(); 36 | edge_id_ = other_edge.edge_id_; 37 | SetCost(other_edge.GetCost()); 38 | } 39 | 40 | Edge& Edge::operator=(const Edge& other_edge) 41 | { 42 | parent_state_ptr_ = other_edge.parent_state_ptr_; 43 | child_state_ptr_ = other_edge.child_state_ptr_; 44 | action_ptr_ = other_edge.action_ptr_; 45 | expansion_priority_ = other_edge.expansion_priority_; 46 | evaluation_priority_ = other_edge.evaluation_priority_; 47 | is_closed_ = other_edge.is_closed_.load(); 48 | is_eval_ = other_edge.is_eval_.load(); 49 | is_invalid_ = other_edge.is_invalid_.load(); 50 | edge_id_ = other_edge.edge_id_; 51 | SetCost(other_edge.GetCost()); 52 | return *this; 53 | } 54 | 55 | bool Edge::operator==(const Edge& other_edge) const 56 | { 57 | return ((parent_state_ptr_==other_edge.parent_state_ptr_)&&(action_ptr_==other_edge.action_ptr_)); 58 | } 59 | 60 | void Edge::Print(std::string str) 61 | { 62 | std::cout << "______________"<< str <<"_________________" << std::endl; 63 | 64 | if (action_ptr_) 65 | std::cout << "Edge: " << edge_id_ << " | Type: " << action_ptr_->GetType() << "| Cost: " << cost_ << "| expansion_priority: " << expansion_priority_<< std::endl; 66 | else 67 | std::cout << "Edge: " << edge_id_ << " | Type: NULL" << "| Cost: " << cost_ << "| expansion_priority: " << expansion_priority_<< std::endl; 68 | 69 | 70 | if (parent_state_ptr_) 71 | parent_state_ptr_->Print("Parent"); 72 | if (child_state_ptr_) 73 | child_state_ptr_->Print("Child"); 74 | std::cout << "_______________________________" << std::endl; 75 | } 76 | 77 | double Edge::roundOff(double value, int prec) 78 | { 79 | double pow_10 = pow(10.0, prec); 80 | return round(value * pow_10) / pow_10; 81 | } 82 | 83 | bool IsLesserEdge::operator() (const Edge& lhs, const Edge& rhs) 84 | { 85 | // Default fifo ordering 86 | if (lhs.expansion_priority_ == rhs.expansion_priority_) // tie breaking 87 | return lhs.edge_id_ < rhs.edge_id_; 88 | else 89 | return lhs.expansion_priority_ < rhs.expansion_priority_; 90 | } 91 | 92 | bool IsGreaterEdge::operator() (const Edge& lhs, const Edge& rhs) 93 | { 94 | // Default fifo ordering 95 | if (lhs.evaluation_priority_ == rhs.evaluation_priority_) // tie breaking 96 | return lhs.edge_id_ < rhs.edge_id_; 97 | else 98 | return lhs.evaluation_priority_ > rhs.evaluation_priority_; 99 | } 100 | 101 | -------------------------------------------------------------------------------- /src/common/State.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | using namespace std; 5 | using namespace ps; 6 | 7 | size_t State::id_counter_ = 0; 8 | 9 | // State::State(): 10 | // g_val_(numeric_limits::infinity()), 11 | // f_val_(numeric_limits::infinity()), 12 | // is_visited_(false) 13 | // { 14 | // state_id_ = id_counter_++; 15 | // } 16 | 17 | State::State(const StateVarsType& vars): 18 | vars_(vars), 19 | g_val_(DINF), 20 | f_val_(DINF), 21 | v_val_(DINF), 22 | h_val_(-1), 23 | is_visited_(false), 24 | being_expanded_(false), 25 | num_successors_(0), 26 | num_expanded_successors_(0), 27 | incoming_edge_ptr_(NULL) 28 | { 29 | state_id_ = id_counter_++; 30 | } 31 | 32 | void State::Print(string str) 33 | { 34 | cout << str 35 | << "State: " << GetStateID() 36 | << " | Vars: ["; 37 | for (auto& v : vars_) 38 | cout << v << ", "; 39 | cout << "] | g: " << GetGValue() 40 | << " | h: " << GetHValue() 41 | << " | f: "<< GetFValue() 42 | << " | v: "<< GetVValue() 43 | << " | is_closed: " << IsVisited() 44 | << " | being_expanded_: " << IsBeingExpanded() 45 | << " | num_successors: " << num_successors_ 46 | << " | num_expanded_successors: " << num_expanded_successors_ 47 | << endl; 48 | } 49 | 50 | bool IsLesserState::operator()(const State& lhs, const State& rhs) 51 | { 52 | if (lhs.GetFValue() == rhs.GetFValue()) // tie breaking 53 | return lhs.GetStateID() < rhs.GetStateID(); 54 | else 55 | return lhs.GetFValue() < rhs.GetFValue(); 56 | } 57 | 58 | 59 | bool IsGreaterState::operator()(const State& lhs, const State& rhs) 60 | { 61 | if (lhs.GetFValue() == rhs.GetFValue()) // tie breaking 62 | return lhs.GetStateID() < rhs.GetStateID(); 63 | else 64 | return lhs.GetFValue() > rhs.GetFValue(); 65 | } -------------------------------------------------------------------------------- /src/planners/AgepasePlanner.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | using namespace std; 7 | using namespace ps; 8 | 9 | AgepasePlanner::AgepasePlanner(ParamsType planner_params): 10 | GepasePlanner(planner_params) 11 | { 12 | delta_w_ = planner_params["heuristic_reduction"]; 13 | } 14 | 15 | AgepasePlanner::~AgepasePlanner() 16 | { 17 | 18 | } 19 | 20 | bool AgepasePlanner::Plan() 21 | { 22 | initialize(); 23 | 24 | startTimer(); 25 | double heuristic_w = heuristic_w_; 26 | while ((heuristic_w_>1 || fabs(heuristic_w_ - 1)<0.001f) && !checkTimeout()) 27 | { 28 | terminate_ = false; 29 | 30 | resetClosed(); 31 | being_expanded_states_.clear(); 32 | improvePath(); 33 | 34 | // Early termination if there's no solution 35 | if (goal_state_ptr_ == NULL) 36 | { 37 | break; 38 | } 39 | 40 | if (goal_state_ptr_->GetFValue() < best_cost_) 41 | { 42 | best_cost_ = goal_state_ptr_->GetFValue(); 43 | best_plan_.clear(); 44 | best_plan_ = plan_; 45 | } 46 | 47 | // Update heuristic weight 48 | heuristic_w_ -= delta_w_; 49 | 50 | EdgeQueueMinType edge_open_list; 51 | // append inconsistent list's edges into Eopen 52 | for(auto it_edge = edge_incon_list_.begin(); it_edge != edge_incon_list_.end(); it_edge++) 53 | { 54 | edge_open_list_.push(*it_edge); 55 | } 56 | 57 | edge_incon_list_.clear(); 58 | 59 | while (!edge_open_list_.empty()) 60 | { 61 | auto edge = edge_open_list_.min(); 62 | edge_open_list_.pop(); 63 | 64 | edge->parent_state_ptr_->SetFValue(edge->parent_state_ptr_->GetGValue() + heuristic_w_*edge->parent_state_ptr_->GetHValue()); 65 | edge->expansion_priority_ = edge->parent_state_ptr_->GetFValue(); 66 | 67 | if (edge->action_ptr_ != dummy_action_ptr_) 68 | { 69 | auto edge_temp = Edge(edge->parent_state_ptr_, dummy_action_ptr_); 70 | auto edge_key = getEdgeKey(&edge_temp); 71 | edge = edge_map_.find(edge_key)->second; 72 | } 73 | 74 | if (edge_open_list.contains(edge)) 75 | { 76 | edge_open_list.decrease(edge); 77 | } 78 | else 79 | { 80 | edge_open_list.push(edge); 81 | } 82 | } 83 | edge_open_list_ = move(edge_open_list); 84 | } 85 | 86 | terminate_ = true; 87 | plan_ = best_plan_; 88 | // Reset heuristic weight 89 | heuristic_w_ = heuristic_w; 90 | auto t_end = chrono::steady_clock::now(); 91 | double t_elapsed = chrono::duration_cast(t_end-t_start_).count(); 92 | planner_stats_.total_time = 1e-9*t_elapsed; 93 | if (!plan_.empty()) { 94 | exit(); 95 | return true; 96 | } 97 | exit(); 98 | return false; 99 | } 100 | 101 | void AgepasePlanner::initialize() 102 | { 103 | GepasePlanner::initialize(); 104 | edge_incon_list_.clear(); 105 | plan_.clear(); 106 | best_plan_.clear(); 107 | best_cost_ = numeric_limits::max(); 108 | } 109 | 110 | void AgepasePlanner::improvePath() 111 | { 112 | vector popped_edges; 113 | 114 | lock_.lock(); 115 | 116 | while(!checkTimeout()) 117 | { 118 | EdgePtrType curr_edge_ptr = NULL; 119 | 120 | while (!curr_edge_ptr && !terminate_) 121 | { 122 | if (goal_state_ptr_ != NULL) 123 | { 124 | if (!edge_open_list_.empty()) 125 | { 126 | // Terminate condition: no state in open/be has f-value < goal's g-value 127 | if (goal_state_ptr_->GetFValue() + 1e-5 < edge_open_list_.min()->expansion_priority_) 128 | { 129 | // Construct path 130 | auto goal_state_ptr = goal_state_ptr_; 131 | plan_.clear(); 132 | constructPlan(goal_state_ptr); 133 | lock_.unlock(); 134 | exitMultiThread(); 135 | return; 136 | } 137 | } 138 | else if (!being_expanded_states_.empty()) 139 | { 140 | // Terminate condition: no state in open/be has f-value < goal's g-value 141 | if (goal_state_ptr_->GetFValue() + 1e-5 < being_expanded_states_.min()->GetFValue()) 142 | { 143 | // Construct path 144 | auto goal_state_ptr = goal_state_ptr_; 145 | plan_.clear(); 146 | constructPlan(goal_state_ptr); 147 | lock_.unlock(); 148 | exitMultiThread(); 149 | return; 150 | } 151 | } 152 | else // Edge case: when open is empty but goal_state just got set 153 | { 154 | // Construct path 155 | auto goal_state_ptr = goal_state_ptr_; 156 | plan_.clear(); 157 | constructPlan(goal_state_ptr); 158 | lock_.unlock(); 159 | exitMultiThread(); 160 | return; 161 | } 162 | } 163 | 164 | if (edge_open_list_.empty() && being_expanded_states_.empty()) 165 | { 166 | lock_.unlock(); 167 | exitMultiThread(); 168 | return; 169 | } 170 | 171 | while(!curr_edge_ptr && !edge_open_list_.empty()) 172 | { 173 | curr_edge_ptr = edge_open_list_.min(); 174 | edge_open_list_.pop(); 175 | popped_edges.emplace_back(curr_edge_ptr); 176 | 177 | // If the parent state is being expanded, then all the outgoing edges are safe to expand 178 | if (curr_edge_ptr->parent_state_ptr_->IsBeingExpanded()) 179 | break; 180 | 181 | // Independence check of curr_edge with edges in BE 182 | for (auto& being_expanded_state : being_expanded_states_) 183 | { 184 | if (being_expanded_state != curr_edge_ptr->parent_state_ptr_) 185 | { 186 | auto h_diff = computeHeuristic(being_expanded_state, curr_edge_ptr->parent_state_ptr_); 187 | if (curr_edge_ptr->parent_state_ptr_->GetGValue() > being_expanded_state->GetGValue() + heuristic_w_*h_diff) 188 | { 189 | curr_edge_ptr = NULL; 190 | break; 191 | } 192 | } 193 | } 194 | 195 | if (curr_edge_ptr) 196 | { 197 | // Independence check of curr_edge with edges in OPEN that are in front of curr_edge 198 | for (auto& popped_edge_ptr : popped_edges) 199 | { 200 | if (popped_edge_ptr->parent_state_ptr_ != curr_edge_ptr->parent_state_ptr_) 201 | { 202 | auto h_diff = computeHeuristic(popped_edge_ptr->parent_state_ptr_, curr_edge_ptr->parent_state_ptr_); 203 | if (curr_edge_ptr->parent_state_ptr_->GetGValue() > popped_edge_ptr->parent_state_ptr_->GetGValue() + heuristic_w_*h_diff) 204 | { 205 | curr_edge_ptr = NULL; 206 | break; 207 | } 208 | } 209 | } 210 | } 211 | } 212 | 213 | 214 | // Re add the popped states except curr state which will be expanded now 215 | for (auto& popped_edge_ptr : popped_edges) 216 | { 217 | if (popped_edge_ptr != curr_edge_ptr) 218 | { 219 | edge_open_list_.push(popped_edge_ptr); 220 | } 221 | } 222 | popped_edges.clear(); 223 | 224 | if (!curr_edge_ptr) 225 | { 226 | lock_.unlock(); 227 | // Wait for recheck_flag_ to be set true; 228 | unique_lock locker(lock_); 229 | cv_.wait(locker, [this](){return (recheck_flag_ == true);}); 230 | recheck_flag_ = false; 231 | locker.unlock(); 232 | lock_.lock(); 233 | continue; 234 | } 235 | 236 | 237 | // Update goal state ptr 238 | if (isGoalState(curr_edge_ptr->parent_state_ptr_) && (!terminate_)) 239 | { 240 | if (goal_state_ptr_ == NULL) 241 | goal_state_ptr_ = curr_edge_ptr->parent_state_ptr_; 242 | else if(curr_edge_ptr->parent_state_ptr_->GetFValue() < goal_state_ptr_->GetFValue()) 243 | goal_state_ptr_ = curr_edge_ptr->parent_state_ptr_; 244 | } 245 | 246 | } 247 | 248 | // Insert the state in BE and mark it closed if the edge being expanded is dummy edge 249 | if (curr_edge_ptr->action_ptr_ == dummy_action_ptr_) 250 | { 251 | planner_stats_.num_state_expansions++; 252 | curr_edge_ptr->parent_state_ptr_->SetVisited(); 253 | curr_edge_ptr->parent_state_ptr_->SetBeingExpanded(); 254 | being_expanded_states_.push(curr_edge_ptr->parent_state_ptr_); 255 | } 256 | 257 | lock_.unlock(); 258 | 259 | int thread_id = 0; 260 | bool edge_expansion_assigned = false; 261 | 262 | if (num_threads_ == 1) 263 | { 264 | expand(curr_edge_ptr, 0); 265 | } 266 | else 267 | { 268 | while (!edge_expansion_assigned) 269 | { 270 | unique_lock locker(lock_vec_[thread_id]); 271 | bool status = edge_expansion_status_[thread_id]; 272 | locker.unlock(); 273 | 274 | if (!status) 275 | { 276 | int num_threads_current = edge_expansion_futures_.size(); 277 | if (thread_id >= num_threads_current) 278 | { 279 | if (VERBOSE) cout << "Spawning edge expansion thread " << thread_id << endl; 280 | edge_expansion_futures_.emplace_back(async(launch::async, &AgepasePlanner::expandEdgeLoop, this, thread_id)); 281 | } 282 | locker.lock(); 283 | edge_expansion_vec_[thread_id] = curr_edge_ptr; 284 | edge_expansion_status_[thread_id] = 1; 285 | edge_expansion_assigned = true; 286 | locker.unlock(); 287 | cv_vec_[thread_id].notify_one(); 288 | } 289 | else 290 | { 291 | thread_id = thread_id == num_threads_-2 ? 0 : thread_id+1; 292 | } 293 | 294 | } 295 | } 296 | 297 | lock_.lock(); 298 | 299 | } 300 | 301 | lock_.unlock(); 302 | exitMultiThread(); 303 | return; 304 | } 305 | 306 | void AgepasePlanner::expand(EdgePtrType edge_ptr, int thread_id) 307 | { 308 | auto t_start = chrono::steady_clock::now(); 309 | lock_.lock(); 310 | auto t_lock_e = chrono::steady_clock::now(); 311 | planner_stats_.lock_time += 1e-9*chrono::duration_cast(t_lock_e-t_start).count(); 312 | 313 | planner_stats_.num_jobs_per_thread[thread_id] +=1; 314 | 315 | // Proxy edge, add the real edges to Eopen 316 | if (edge_ptr->action_ptr_ == dummy_action_ptr_) 317 | { 318 | auto state_ptr = edge_ptr->parent_state_ptr_; 319 | state_ptr->SetVValue(state_ptr->GetGValue()); 320 | 321 | for (auto& action_ptr: actions_ptrs_) 322 | { 323 | if (action_ptr->CheckPreconditions(state_ptr->GetStateVars())) 324 | { 325 | auto edge_temp = Edge(state_ptr, action_ptr); 326 | auto edge_key = getEdgeKey(&edge_temp); 327 | auto it_edge = edge_map_.find(edge_key); 328 | EdgePtrType edge_ptr_next; 329 | 330 | if (it_edge == edge_map_.end()) 331 | { 332 | edge_ptr_next = new Edge(state_ptr, action_ptr); 333 | edge_map_.insert(make_pair(edge_key, edge_ptr_next)); 334 | } 335 | else 336 | { 337 | edge_ptr_next = it_edge->second; 338 | } 339 | 340 | edge_ptr_next->expansion_priority_ = edge_ptr->expansion_priority_; 341 | state_ptr->num_successors_+=1; 342 | 343 | if (action_ptr->IsExpensive() || !edge_ptr_next->is_eval_) 344 | { 345 | if (VERBOSE) cout << "Pushing successor with g_val: " << state_ptr->GetGValue() << " | h_val: " << state_ptr->GetHValue() << endl; 346 | if (edge_open_list_.contains(edge_ptr_next)) 347 | { 348 | edge_open_list_.decrease(edge_ptr_next); 349 | } 350 | else 351 | { 352 | edge_open_list_.push(edge_ptr_next); 353 | } 354 | notifyMainThread(); 355 | } 356 | else 357 | { 358 | expandEdge(edge_ptr_next, thread_id); 359 | } 360 | } 361 | 362 | } 363 | 364 | } 365 | else // Real edge, evaluate and add proxy edges for child 366 | { 367 | expandEdge(edge_ptr, thread_id); 368 | } 369 | 370 | 371 | if (edge_ptr->parent_state_ptr_->num_expanded_successors_ == edge_ptr->parent_state_ptr_->num_successors_) 372 | { 373 | edge_ptr->parent_state_ptr_->UnsetBeingExpanded(); 374 | if (being_expanded_states_.contains(edge_ptr->parent_state_ptr_)) 375 | { 376 | being_expanded_states_.erase(edge_ptr->parent_state_ptr_); 377 | notifyMainThread(); 378 | } 379 | } 380 | 381 | if (edge_ptr->parent_state_ptr_->num_expanded_successors_ > edge_ptr->parent_state_ptr_->num_successors_) 382 | { 383 | edge_ptr->parent_state_ptr_->Print(); 384 | throw runtime_error("Number of expanded edges cannot be greater than number of successors"); 385 | } 386 | else 387 | { 388 | if (VERBOSE) edge_ptr->Print("Expansion completed "); 389 | } 390 | 391 | auto t_end_expansion = chrono::steady_clock::now(); 392 | planner_stats_.cumulative_expansions_time += 1e-9*chrono::duration_cast(t_end_expansion-t_start).count(); 393 | 394 | lock_.unlock(); 395 | } 396 | 397 | void AgepasePlanner::expandEdge(EdgePtrType edge_ptr, int thread_id) 398 | { 399 | if (!edge_ptr->is_eval_) 400 | { 401 | auto action_ptr = edge_ptr->action_ptr_; 402 | 403 | lock_.unlock(); 404 | // Evaluate the edge 405 | auto t_start = chrono::steady_clock::now(); 406 | auto action_successor = action_ptr->GetSuccessor(edge_ptr->parent_state_ptr_->GetStateVars(), thread_id); 407 | auto t_end = chrono::steady_clock::now(); 408 | 409 | auto t_lock_s = chrono::steady_clock::now(); 410 | lock_.lock(); 411 | auto t_lock_e = chrono::steady_clock::now(); 412 | planner_stats_.lock_time += 1e-9*chrono::duration_cast(t_lock_e-t_lock_s).count(); 413 | 414 | edge_ptr->is_eval_ = true; 415 | planner_stats_.num_evaluated_edges++; // Only the edges controllers that satisfied pre-conditions and args are in the open list 416 | 417 | if (action_successor.success_) 418 | { 419 | auto successor_state_ptr = constructState(action_successor.successor_state_vars_costs_.back().first); 420 | double cost = action_successor.successor_state_vars_costs_.back().second; 421 | 422 | // Set successor and cost in expanded edge 423 | edge_ptr->child_state_ptr_ = successor_state_ptr; 424 | edge_ptr->SetCost(cost); 425 | 426 | double new_g_val = edge_ptr->parent_state_ptr_->GetGValue() + cost; 427 | 428 | if (successor_state_ptr->GetGValue() > new_g_val) 429 | { 430 | updateEdge(edge_ptr, successor_state_ptr, new_g_val); 431 | } 432 | } 433 | else 434 | { 435 | edge_ptr->is_invalid_ = true; 436 | if (VERBOSE) edge_ptr->Print("No successors for"); 437 | } 438 | } 439 | // Pull the edge from map if it's already evaluated 440 | else if (!edge_ptr->is_invalid_) 441 | { 442 | double new_g_val = edge_ptr->parent_state_ptr_->GetGValue() + edge_ptr->GetCost(); 443 | auto successor_state_ptr = edge_ptr->child_state_ptr_; 444 | 445 | if (successor_state_ptr->GetGValue() > new_g_val) 446 | { 447 | updateEdge(edge_ptr, successor_state_ptr, new_g_val); 448 | } 449 | } 450 | 451 | edge_ptr->parent_state_ptr_->num_expanded_successors_ += 1; 452 | 453 | } 454 | 455 | void AgepasePlanner::updateEdge(EdgePtrType edge_ptr, StatePtrType successor_state_ptr, double new_g_val) 456 | { 457 | double h_val = successor_state_ptr->GetHValue(); 458 | 459 | if (h_val == -1) 460 | { 461 | h_val = computeHeuristic(successor_state_ptr); 462 | successor_state_ptr->SetHValue(h_val); 463 | } 464 | 465 | if (h_val != DINF) 466 | { 467 | h_val_min_ = h_val < h_val_min_ ? h_val : h_val_min_; 468 | successor_state_ptr->SetGValue(new_g_val); 469 | successor_state_ptr->SetFValue(new_g_val + heuristic_w_*h_val); 470 | successor_state_ptr->SetIncomingEdgePtr(edge_ptr); 471 | 472 | // Insert poxy edge 473 | auto edge_temp = Edge(successor_state_ptr, dummy_action_ptr_); 474 | auto edge_key = getEdgeKey(&edge_temp); 475 | auto it_edge = edge_map_.find(edge_key); 476 | EdgePtrType proxy_edge_ptr; 477 | 478 | if (it_edge == edge_map_.end()) 479 | { 480 | proxy_edge_ptr = new Edge(successor_state_ptr, dummy_action_ptr_); 481 | edge_map_.insert(make_pair(edge_key, proxy_edge_ptr)); 482 | } 483 | else 484 | { 485 | proxy_edge_ptr = it_edge->second; 486 | } 487 | 488 | proxy_edge_ptr->expansion_priority_ = new_g_val + heuristic_w_*h_val; 489 | 490 | if (!successor_state_ptr->IsVisited()) 491 | { 492 | if (edge_open_list_.contains(proxy_edge_ptr)) 493 | { 494 | edge_open_list_.decrease(proxy_edge_ptr); 495 | } 496 | else 497 | { 498 | edge_open_list_.push(proxy_edge_ptr); 499 | } 500 | } 501 | else 502 | { 503 | if (find(edge_incon_list_.begin(), edge_incon_list_.end(), proxy_edge_ptr) == edge_incon_list_.end()) 504 | { 505 | edge_incon_list_.emplace_back(proxy_edge_ptr); 506 | } 507 | } 508 | 509 | notifyMainThread(); 510 | } 511 | } 512 | 513 | void AgepasePlanner::exit() 514 | { 515 | terminate_ = true; 516 | 517 | for (int thread_id = 0; thread_id < num_threads_-1; ++thread_id) 518 | { 519 | unique_lock locker(lock_vec_[thread_id]); 520 | edge_expansion_status_[thread_id] = 1; 521 | locker.unlock(); 522 | cv_vec_[thread_id].notify_one(); 523 | } 524 | 525 | planner_stats_.num_threads_spawned = edge_expansion_futures_.size()+1; 526 | bool all_expansion_threads_terminated = false; 527 | while (!all_expansion_threads_terminated) 528 | { 529 | all_expansion_threads_terminated = true; 530 | int i = 0; 531 | for (auto& fut : edge_expansion_futures_) 532 | { 533 | if (!isFutureReady(fut)) 534 | { 535 | all_expansion_threads_terminated = false; 536 | break; 537 | } 538 | i++; 539 | } 540 | } 541 | edge_expansion_futures_.clear(); 542 | 543 | edge_incon_list_.clear(); 544 | 545 | while (!edge_open_list_.empty()) 546 | { 547 | edge_open_list_.pop(); 548 | } 549 | being_expanded_states_.clear(); 550 | 551 | Planner::exit(); 552 | } 553 | 554 | void AgepasePlanner::exitMultiThread() 555 | { 556 | bool all_expansion_threads_terminated = false; 557 | while (!all_expansion_threads_terminated) 558 | { 559 | all_expansion_threads_terminated = true; 560 | for (int thread_id = 0; thread_id < num_threads_-1; ++thread_id) 561 | { 562 | unique_lock locker(lock_vec_[thread_id]); 563 | auto status = edge_expansion_status_[thread_id]; 564 | if (status) 565 | { 566 | all_expansion_threads_terminated = false; 567 | break; 568 | } 569 | 570 | } 571 | } 572 | } -------------------------------------------------------------------------------- /src/planners/ArastarPlanner.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | using namespace std; 7 | using namespace ps; 8 | 9 | ArastarPlanner::ArastarPlanner(ParamsType planner_params): 10 | WastarPlanner(planner_params) 11 | { 12 | delta_w_ = planner_params_["heuristic_reduction"]; 13 | } 14 | 15 | ArastarPlanner::~ArastarPlanner() 16 | { 17 | 18 | } 19 | 20 | bool ArastarPlanner::Plan() 21 | { 22 | initialize(); 23 | 24 | // Experiment 25 | std::vector data_list; 26 | 27 | startTimer(); 28 | double heuristic_w = heuristic_w_; 29 | while ((heuristic_w_>1 || fabs(heuristic_w_ - 1)<0.001f) && !checkTimeout()) 30 | { 31 | resetClosed(); 32 | improvePath(); 33 | 34 | // Early termination if there's no solution 35 | if (goal_state_ptr_ == NULL || heuristic_w_ == 1) 36 | { 37 | break; 38 | } 39 | 40 | // Update heuristic weight 41 | heuristic_w_ -= delta_w_; 42 | 43 | // Append inconsistent list to open 44 | for(auto it_state = state_incon_list_.begin(); it_state != state_incon_list_.end(); it_state++) 45 | { 46 | state_open_list_.push(*it_state); 47 | } 48 | state_incon_list_.clear(); 49 | 50 | StateQueueMinType state_open_list; 51 | while (!state_open_list_.empty()) 52 | { 53 | auto state = state_open_list_.min(); 54 | state_open_list_.pop(); 55 | 56 | state->SetFValue(state->GetGValue() + heuristic_w_*state->GetHValue()); 57 | state_open_list.push(state); 58 | } 59 | state_open_list_ = move(state_open_list); 60 | 61 | } 62 | 63 | // Reset heuristic weight 64 | heuristic_w_ = heuristic_w; 65 | auto t_end = chrono::steady_clock::now(); 66 | double t_elapsed = chrono::duration_cast(t_end-t_start_).count(); 67 | planner_stats_.total_time = 1e-9*t_elapsed; 68 | if (goal_state_ptr_ != NULL) { 69 | exit(); 70 | return true; 71 | } 72 | exit(); 73 | return false; 74 | } 75 | 76 | void ArastarPlanner::initialize() 77 | { 78 | WastarPlanner::initialize(); 79 | state_incon_list_.clear(); 80 | } 81 | 82 | void ArastarPlanner::improvePath() 83 | { 84 | while (!checkTimeout()) 85 | { 86 | // Terminate condition check 87 | if (goal_state_ptr_ != NULL) 88 | { 89 | if (!state_open_list_.empty()) 90 | { 91 | if (goal_state_ptr_->GetFValue() + 1e-5 < state_open_list_.min()->GetFValue()) 92 | { 93 | // Construct path and return 94 | auto goal_state_ptr = goal_state_ptr_; 95 | plan_.clear(); 96 | constructPlan(goal_state_ptr); 97 | return; 98 | } 99 | } 100 | else 101 | { 102 | // Construct path and return 103 | auto goal_state_ptr = goal_state_ptr_; 104 | plan_.clear(); 105 | constructPlan(goal_state_ptr); 106 | return; 107 | } 108 | } 109 | 110 | if (state_open_list_.empty()) 111 | { 112 | return; 113 | } 114 | 115 | auto state_ptr = state_open_list_.min(); 116 | state_open_list_.pop(); 117 | 118 | // Update goal state ptr 119 | if (isGoalState(state_ptr)) 120 | { 121 | if (goal_state_ptr_ == NULL) 122 | goal_state_ptr_ = state_ptr; 123 | else if(state_ptr->GetGValue() < goal_state_ptr_->GetGValue()) 124 | goal_state_ptr_ = state_ptr; 125 | } 126 | 127 | expandState(state_ptr); 128 | } 129 | 130 | } 131 | 132 | void ArastarPlanner::expandState(StatePtrType state_ptr) 133 | { 134 | 135 | if (VERBOSE) state_ptr->Print("Expanding"); 136 | 137 | planner_stats_.num_jobs_per_thread[0] +=1; 138 | planner_stats_.num_state_expansions++; 139 | 140 | state_ptr->SetVisited(); 141 | state_ptr->SetVValue(state_ptr->GetGValue()); 142 | 143 | for (auto& action_ptr: actions_ptrs_) 144 | { 145 | if (action_ptr->CheckPreconditions(state_ptr->GetStateVars())) 146 | { 147 | // Check if the edge is already in the edge map 148 | auto edge_temp = Edge(state_ptr, action_ptr); 149 | auto edge_key = getEdgeKey(&edge_temp); 150 | auto it_edge = edge_map_.find(edge_key); 151 | EdgePtrType edge_ptr_next; 152 | 153 | if (it_edge == edge_map_.end()) 154 | { 155 | edge_ptr_next = new Edge(state_ptr, action_ptr); 156 | edge_map_.insert(make_pair(edge_key, edge_ptr_next)); 157 | } 158 | else 159 | { 160 | edge_ptr_next = it_edge->second; 161 | } 162 | // Update state 163 | updateState(state_ptr, action_ptr, edge_ptr_next); 164 | } 165 | } 166 | } 167 | 168 | void ArastarPlanner::updateState(StatePtrType& state_ptr, ActionPtrType& action_ptr, EdgePtrType& edge_ptr) 169 | { 170 | if (!edge_ptr->is_eval_) 171 | { 172 | auto action_successor = action_ptr->GetSuccessor(state_ptr->GetStateVars()); 173 | edge_ptr->is_eval_ = true; 174 | planner_stats_.num_evaluated_edges++; 175 | if (action_successor.success_) 176 | { 177 | auto successor_state_ptr = constructState(action_successor.successor_state_vars_costs_.back().first); 178 | double cost = action_successor.successor_state_vars_costs_.back().second; 179 | double new_g_val = state_ptr->GetGValue() + cost; 180 | 181 | // Set successor and cost in expanded edge 182 | edge_ptr->child_state_ptr_ = successor_state_ptr; 183 | edge_ptr->SetCost(cost); 184 | updateEdge(edge_ptr, successor_state_ptr, new_g_val); 185 | } 186 | else 187 | { 188 | edge_ptr->is_invalid_ = true; 189 | } 190 | } 191 | else if (!edge_ptr->is_invalid_) 192 | { 193 | double new_g_val = edge_ptr->parent_state_ptr_->GetGValue() + edge_ptr->GetCost(); 194 | auto successor_state_ptr = edge_ptr->child_state_ptr_; 195 | updateEdge(edge_ptr, successor_state_ptr, new_g_val); 196 | } 197 | } 198 | 199 | void ArastarPlanner::updateEdge(EdgePtrType edge_ptr, StatePtrType successor_state_ptr, double new_g_val) 200 | { 201 | if (successor_state_ptr->GetGValue() > new_g_val) 202 | { 203 | double h_val = successor_state_ptr->GetHValue(); 204 | 205 | if (h_val == -1) 206 | { 207 | h_val = computeHeuristic(successor_state_ptr); 208 | successor_state_ptr->SetHValue(h_val); 209 | } 210 | 211 | if (h_val != DINF) 212 | { 213 | h_val_min_ = h_val < h_val_min_ ? h_val : h_val_min_; 214 | successor_state_ptr->SetGValue(new_g_val); 215 | successor_state_ptr->SetFValue(new_g_val + heuristic_w_*h_val); 216 | successor_state_ptr->SetIncomingEdgePtr(edge_ptr); 217 | 218 | if (!successor_state_ptr->IsVisited()) 219 | { 220 | if (state_open_list_.contains(successor_state_ptr)) 221 | { 222 | state_open_list_.decrease(successor_state_ptr); 223 | } 224 | else 225 | { 226 | state_open_list_.push(successor_state_ptr); 227 | } 228 | } 229 | else 230 | { 231 | if (find(state_incon_list_.begin(), state_incon_list_.end(), successor_state_ptr) == state_incon_list_.end()) 232 | { 233 | state_incon_list_.push_back(successor_state_ptr); 234 | } 235 | } 236 | } 237 | } 238 | } 239 | 240 | void ArastarPlanner::exit() 241 | { 242 | state_incon_list_.clear(); 243 | WastarPlanner::exit(); 244 | } 245 | -------------------------------------------------------------------------------- /src/planners/EpasePlanner.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | using namespace std; 6 | using namespace ps; 7 | 8 | EpasePlanner::EpasePlanner(ParamsType planner_params): 9 | GepasePlanner(planner_params) 10 | { 11 | 12 | } 13 | 14 | EpasePlanner::~EpasePlanner() 15 | { 16 | 17 | } 18 | 19 | bool EpasePlanner::Plan() 20 | { 21 | 22 | initialize(); 23 | 24 | vector popped_edges; 25 | 26 | lock_.lock(); 27 | startTimer(); 28 | while(!terminate_ && !checkTimeout()) 29 | { 30 | EdgePtrType curr_edge_ptr = NULL; 31 | 32 | while (!curr_edge_ptr && !terminate_) 33 | { 34 | if (edge_open_list_.empty() && being_expanded_states_.empty()) 35 | { 36 | terminate_ = true; 37 | auto t_end = chrono::steady_clock::now(); 38 | double t_elapsed = chrono::duration_cast(t_end-t_start_).count(); 39 | planner_stats_.total_time = 1e-9*t_elapsed; 40 | cout << "Goal Not Reached" << endl; 41 | lock_.unlock(); 42 | exit(); 43 | return false; 44 | } 45 | 46 | while(!curr_edge_ptr && !edge_open_list_.empty()) 47 | { 48 | curr_edge_ptr = edge_open_list_.min(); 49 | edge_open_list_.pop(); 50 | popped_edges.emplace_back(curr_edge_ptr); 51 | 52 | // If the parent state is being expanded, then all the outgoing edges are safe to expand 53 | if (curr_edge_ptr->parent_state_ptr_->IsBeingExpanded()) 54 | break; 55 | 56 | // Independence check of curr_edge with edges in BE 57 | for (auto& being_expanded_state : being_expanded_states_) 58 | { 59 | if (being_expanded_state != curr_edge_ptr->parent_state_ptr_) 60 | { 61 | auto h_diff = computeHeuristic(being_expanded_state, curr_edge_ptr->parent_state_ptr_); 62 | if (curr_edge_ptr->parent_state_ptr_->GetGValue() > being_expanded_state->GetGValue() + heuristic_w_*h_diff) 63 | { 64 | curr_edge_ptr = NULL; 65 | break; 66 | } 67 | } 68 | } 69 | 70 | if (curr_edge_ptr) 71 | { 72 | // Independence check of curr_edge with edges in OPEN that are in front of curr_edge 73 | for (auto& popped_edge_ptr : popped_edges) 74 | { 75 | if (popped_edge_ptr->parent_state_ptr_ != curr_edge_ptr->parent_state_ptr_) 76 | { 77 | auto h_diff = computeHeuristic(popped_edge_ptr->parent_state_ptr_, curr_edge_ptr->parent_state_ptr_); 78 | if (curr_edge_ptr->parent_state_ptr_->GetGValue() > popped_edge_ptr->parent_state_ptr_->GetGValue() + heuristic_w_*h_diff) 79 | { 80 | curr_edge_ptr = NULL; 81 | break; 82 | } 83 | } 84 | } 85 | } 86 | } 87 | 88 | 89 | // Re add the popped states except curr state which will be expanded now 90 | for (auto& popped_edge_ptr : popped_edges) 91 | { 92 | if (popped_edge_ptr != curr_edge_ptr) 93 | { 94 | edge_open_list_.push(popped_edge_ptr); 95 | } 96 | } 97 | popped_edges.clear(); 98 | 99 | if (!curr_edge_ptr) 100 | { 101 | lock_.unlock(); 102 | // Wait for recheck_flag_ to be set true; 103 | unique_lock locker(lock_); 104 | cv_.wait(locker, [this](){return (recheck_flag_ == true);}); 105 | recheck_flag_ = false; 106 | locker.unlock(); 107 | lock_.lock(); 108 | continue; 109 | } 110 | 111 | 112 | // Return solution if goal state is expanded 113 | if (isGoalState(curr_edge_ptr->parent_state_ptr_) && (!terminate_)) 114 | { 115 | auto t_end = chrono::steady_clock::now(); 116 | double t_elapsed = chrono::duration_cast(t_end-t_start_).count(); 117 | planner_stats_.total_time = 1e-9*t_elapsed; 118 | 119 | // cout << "--------------------------------------------------------" << endl; 120 | // cout << "Goal Reached!" << endl; 121 | // cout << "--------------------------------------------------------" << endl; 122 | 123 | // Construct path 124 | goal_state_ptr_ = curr_edge_ptr->parent_state_ptr_; 125 | constructPlan(goal_state_ptr_); 126 | terminate_ = true; 127 | recheck_flag_ = true; 128 | lock_.unlock(); 129 | exit(); 130 | 131 | return true; 132 | } 133 | 134 | } 135 | 136 | // Insert the state in BE and mark it closed if the edge being expanded is dummy edge 137 | if (curr_edge_ptr->action_ptr_ == dummy_action_ptr_) 138 | { 139 | planner_stats_.num_state_expansions++; 140 | curr_edge_ptr->parent_state_ptr_->SetVisited(); 141 | curr_edge_ptr->parent_state_ptr_->SetBeingExpanded(); 142 | being_expanded_states_.emplace_back(curr_edge_ptr->parent_state_ptr_); 143 | } 144 | 145 | lock_.unlock(); 146 | 147 | int thread_id = 0; 148 | bool edge_expansion_assigned = false; 149 | 150 | if (num_threads_ == 1) 151 | { 152 | expandEdge(curr_edge_ptr, 0); 153 | } 154 | else 155 | { 156 | while (!edge_expansion_assigned) 157 | { 158 | unique_lock locker(lock_vec_[thread_id]); 159 | bool status = edge_expansion_status_[thread_id]; 160 | locker.unlock(); 161 | 162 | if (!status) 163 | { 164 | int num_threads_current = edge_expansion_futures_.size(); 165 | if (thread_id >= num_threads_current) 166 | { 167 | if (VERBOSE) cout << "Spawning edge expansion thread " << thread_id << endl; 168 | edge_expansion_futures_.emplace_back(async(launch::async, &EpasePlanner::expandEdgeLoop, this, thread_id)); 169 | } 170 | locker.lock(); 171 | edge_expansion_vec_[thread_id] = curr_edge_ptr; 172 | edge_expansion_status_[thread_id] = 1; 173 | edge_expansion_assigned = true; 174 | locker.unlock(); 175 | cv_vec_[thread_id].notify_one(); 176 | } 177 | else 178 | { 179 | thread_id = thread_id == num_threads_-2 ? 0 : thread_id+1; 180 | } 181 | 182 | } 183 | } 184 | 185 | lock_.lock(); 186 | } 187 | 188 | terminate_ = true; 189 | auto t_end = chrono::steady_clock::now(); 190 | double t_elapsed = chrono::duration_cast(t_end-t_start_).count(); 191 | planner_stats_.total_time = 1e-9*t_elapsed; 192 | // cout << "Goal Not Reached, Number of states expanded: " << planner_stats_.num_state_expansions_ << endl; 193 | lock_.unlock(); 194 | exit(); 195 | return false; 196 | } 197 | 198 | void EpasePlanner::expandEdgeLoop(int thread_id) 199 | { 200 | while (!terminate_) 201 | { 202 | unique_lock locker(lock_vec_[thread_id]); 203 | cv_vec_[thread_id].wait(locker, [this, thread_id](){return (edge_expansion_status_[thread_id] == 1);}); 204 | locker.unlock(); 205 | 206 | if (terminate_) 207 | break; 208 | 209 | expandEdge(edge_expansion_vec_[thread_id], thread_id); 210 | 211 | locker.lock(); 212 | edge_expansion_vec_[thread_id] = NULL; 213 | edge_expansion_status_[thread_id] = 0; 214 | locker.unlock(); 215 | 216 | } 217 | } 218 | 219 | void EpasePlanner::expandEdge(EdgePtrType edge_ptr, int thread_id) 220 | { 221 | auto t_start = chrono::steady_clock::now(); 222 | lock_.lock(); 223 | auto t_lock_e = chrono::steady_clock::now(); 224 | planner_stats_.lock_time += 1e-9*chrono::duration_cast(t_lock_e-t_start).count(); 225 | 226 | if (VERBOSE) edge_ptr->Print("Expanding"); 227 | 228 | planner_stats_.num_jobs_per_thread[thread_id] +=1; 229 | 230 | auto state_ptr = edge_ptr->parent_state_ptr_; 231 | 232 | // Proxy edge, add the real edges to Eopen 233 | if (edge_ptr->action_ptr_ == dummy_action_ptr_) 234 | { 235 | for (auto& action_ptr: actions_ptrs_) 236 | { 237 | if (action_ptr->CheckPreconditions(state_ptr->GetStateVars())) 238 | { 239 | auto edge_ptr_real = new Edge(state_ptr, action_ptr); 240 | edge_map_.insert(make_pair(getEdgeKey(edge_ptr_real), edge_ptr_real)); 241 | 242 | // edge_ptr_real->exp_priority_ = state_ptr->GetGValue() + heuristic_w_*state_ptr->GetHValue(); 243 | edge_ptr_real->expansion_priority_ = edge_ptr->expansion_priority_; 244 | 245 | if (VERBOSE) cout << "Pushing successor with g_val: " << state_ptr->GetGValue() << " | h_val: " << state_ptr->GetHValue() << endl; 246 | 247 | state_ptr->num_successors_+=1; 248 | edge_open_list_.push(edge_ptr_real); 249 | notifyMainThread(); 250 | } 251 | } 252 | } 253 | else // Real edge, evaluate and add proxy edges for child 254 | { 255 | 256 | auto action_ptr = edge_ptr->action_ptr_; 257 | 258 | lock_.unlock(); 259 | // Evaluate the edge 260 | auto t_start = chrono::steady_clock::now(); 261 | auto action_successor = action_ptr->GetSuccessor(state_ptr->GetStateVars(), thread_id); 262 | auto t_end = chrono::steady_clock::now(); 263 | //******************** 264 | 265 | auto t_lock_s = chrono::steady_clock::now(); 266 | lock_.lock(); 267 | auto t_lock_e = chrono::steady_clock::now(); 268 | 269 | planner_stats_.action_eval_times[action_ptr->GetType()].emplace_back(1e-9*chrono::duration_cast(t_end-t_start).count()); 270 | planner_stats_.lock_time += 1e-9*chrono::duration_cast(t_lock_e-t_lock_s).count(); 271 | 272 | planner_stats_.num_evaluated_edges++; // Only the edges controllers that satisfied pre-conditions and args are in the open list 273 | 274 | if (action_successor.success_) 275 | { 276 | auto successor_state_ptr = constructState(action_successor.successor_state_vars_costs_.back().first); 277 | double cost = action_successor.successor_state_vars_costs_.back().second; 278 | 279 | // Set successor and cost in expanded edge 280 | edge_ptr->child_state_ptr_ = successor_state_ptr; 281 | edge_ptr->SetCost(cost); 282 | 283 | if (!successor_state_ptr->IsVisited()) 284 | { 285 | double new_g_val = edge_ptr->parent_state_ptr_->GetGValue() + cost; 286 | 287 | if (successor_state_ptr->GetGValue() > new_g_val) 288 | { 289 | 290 | double h_val = successor_state_ptr->GetHValue(); 291 | 292 | if (h_val == -1) 293 | { 294 | h_val = computeHeuristic(successor_state_ptr); 295 | successor_state_ptr->SetHValue(h_val); 296 | } 297 | 298 | if (h_val != DINF) 299 | { 300 | h_val_min_ = h_val < h_val_min_ ? h_val : h_val_min_; 301 | successor_state_ptr->SetGValue(new_g_val); 302 | successor_state_ptr->SetFValue(new_g_val + heuristic_w_*h_val); 303 | successor_state_ptr->SetIncomingEdgePtr(edge_ptr); 304 | 305 | // Insert poxy edge 306 | auto edge_temp = Edge(successor_state_ptr, dummy_action_ptr_); 307 | auto edge_key = getEdgeKey(&edge_temp); 308 | auto it_edge = edge_map_.find(edge_key); 309 | EdgePtrType proxy_edge_ptr; 310 | 311 | if (it_edge == edge_map_.end()) 312 | { 313 | proxy_edge_ptr = new Edge(successor_state_ptr, dummy_action_ptr_); 314 | edge_map_.insert(make_pair(edge_key, proxy_edge_ptr)); 315 | } 316 | else 317 | { 318 | proxy_edge_ptr = it_edge->second; 319 | } 320 | 321 | proxy_edge_ptr->expansion_priority_ = new_g_val + heuristic_w_*h_val; 322 | 323 | if (edge_open_list_.contains(proxy_edge_ptr)) 324 | { 325 | edge_open_list_.decrease(proxy_edge_ptr); 326 | } 327 | else 328 | { 329 | edge_open_list_.push(proxy_edge_ptr); 330 | } 331 | 332 | notifyMainThread(); 333 | } 334 | 335 | } 336 | } 337 | } 338 | else 339 | { 340 | if (VERBOSE) edge_ptr->Print("No successors for"); 341 | } 342 | 343 | edge_ptr->parent_state_ptr_->num_expanded_successors_ += 1; 344 | 345 | if (edge_ptr->parent_state_ptr_->num_expanded_successors_ == edge_ptr->parent_state_ptr_->num_successors_) 346 | { 347 | edge_ptr->parent_state_ptr_->UnsetBeingExpanded(); 348 | auto it_state_be = find(being_expanded_states_.begin(), being_expanded_states_.end(), edge_ptr->parent_state_ptr_); 349 | if (it_state_be != being_expanded_states_.end()) 350 | { 351 | being_expanded_states_.erase(it_state_be); 352 | notifyMainThread(); 353 | } 354 | } 355 | 356 | if (edge_ptr->parent_state_ptr_->num_expanded_successors_ > edge_ptr->parent_state_ptr_->num_successors_) 357 | { 358 | edge_ptr->parent_state_ptr_->Print(); 359 | throw runtime_error("Number of expanded edges cannot be greater than number of successors"); 360 | } 361 | 362 | } 363 | 364 | auto t_end = chrono::steady_clock::now(); 365 | planner_stats_.cumulative_expansions_time += 1e-9*chrono::duration_cast(t_end-t_start).count(); 366 | 367 | lock_.unlock(); 368 | } 369 | 370 | 371 | void EpasePlanner::exit() 372 | { 373 | being_expanded_states_.clear(); 374 | GepasePlanner::exit(); 375 | } -------------------------------------------------------------------------------- /src/planners/GepasePlanner.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | using namespace std; 6 | using namespace ps; 7 | 8 | GepasePlanner::GepasePlanner(ParamsType planner_params): 9 | Planner(planner_params) 10 | { 11 | num_threads_ = planner_params["num_threads"]; 12 | vector lock_vec(num_threads_-1); 13 | lock_vec_.swap(lock_vec); 14 | } 15 | 16 | GepasePlanner::~GepasePlanner() 17 | { 18 | 19 | } 20 | 21 | bool GepasePlanner::Plan() 22 | { 23 | 24 | initialize(); 25 | vector popped_edges; 26 | lock_.lock(); 27 | 28 | startTimer(); 29 | while(!terminate_ && !checkTimeout()) 30 | { 31 | EdgePtrType curr_edge_ptr = NULL; 32 | 33 | while (!curr_edge_ptr && !terminate_) 34 | { 35 | if (edge_open_list_.empty() && being_expanded_states_.empty()) 36 | { 37 | terminate_ = true; 38 | auto t_end = chrono::steady_clock::now(); 39 | double t_elapsed = chrono::duration_cast(t_end-t_start_).count(); 40 | planner_stats_.total_time = 1e-9*t_elapsed; 41 | cout << "Goal Not Reached" << endl; 42 | lock_.unlock(); 43 | exit(); 44 | return false; 45 | } 46 | 47 | while(!curr_edge_ptr && !edge_open_list_.empty()) 48 | { 49 | curr_edge_ptr = edge_open_list_.min(); 50 | edge_open_list_.pop(); 51 | popped_edges.emplace_back(curr_edge_ptr); 52 | 53 | // If the parent state is being expanded, then all the outgoing edges are safe to expand 54 | if (curr_edge_ptr->parent_state_ptr_->IsBeingExpanded()) 55 | break; 56 | 57 | // Independence check of curr_edge with edges in BE 58 | for (auto& being_expanded_state : being_expanded_states_) 59 | { 60 | if (being_expanded_state != curr_edge_ptr->parent_state_ptr_) 61 | { 62 | auto h_diff = computeHeuristic(being_expanded_state, curr_edge_ptr->parent_state_ptr_); 63 | if (curr_edge_ptr->parent_state_ptr_->GetGValue() > being_expanded_state->GetGValue() + heuristic_w_*h_diff) 64 | { 65 | curr_edge_ptr = NULL; 66 | break; 67 | } 68 | } 69 | } 70 | 71 | if (curr_edge_ptr) 72 | { 73 | // Independence check of curr_edge with edges in OPEN that are in front of curr_edge 74 | for (auto& popped_edge_ptr : popped_edges) 75 | { 76 | if (popped_edge_ptr->parent_state_ptr_ != curr_edge_ptr->parent_state_ptr_) 77 | { 78 | auto h_diff = computeHeuristic(popped_edge_ptr->parent_state_ptr_, curr_edge_ptr->parent_state_ptr_); 79 | if (curr_edge_ptr->parent_state_ptr_->GetGValue() > popped_edge_ptr->parent_state_ptr_->GetGValue() + heuristic_w_*h_diff) 80 | { 81 | curr_edge_ptr = NULL; 82 | break; 83 | } 84 | } 85 | } 86 | } 87 | } 88 | 89 | 90 | // Re add the popped states except curr state which will be expanded now 91 | for (auto& popped_edge_ptr : popped_edges) 92 | { 93 | if (popped_edge_ptr != curr_edge_ptr) 94 | { 95 | edge_open_list_.push(popped_edge_ptr); 96 | } 97 | } 98 | popped_edges.clear(); 99 | 100 | if (!curr_edge_ptr) 101 | { 102 | lock_.unlock(); 103 | // Wait for recheck_flag_ to be set true; 104 | unique_lock locker(lock_); 105 | cv_.wait(locker, [this](){return (recheck_flag_ == true);}); 106 | recheck_flag_ = false; 107 | locker.unlock(); 108 | lock_.lock(); 109 | continue; 110 | } 111 | 112 | 113 | // Return solution if goal state is expanded 114 | if (isGoalState(curr_edge_ptr->parent_state_ptr_) && (!terminate_)) 115 | { 116 | auto t_end = chrono::steady_clock::now(); 117 | double t_elapsed = chrono::duration_cast(t_end-t_start_).count(); 118 | planner_stats_.total_time = 1e-9*t_elapsed; 119 | 120 | // cout << "--------------------------------------------------------" << endl; 121 | // cout << "Goal Reached!" << endl; 122 | // cout << "--------------------------------------------------------" << endl; 123 | 124 | // Construct path 125 | goal_state_ptr_ = curr_edge_ptr->parent_state_ptr_; 126 | constructPlan(goal_state_ptr_); 127 | terminate_ = true; 128 | recheck_flag_ = true; 129 | lock_.unlock(); 130 | exit(); 131 | 132 | return true; 133 | } 134 | 135 | } 136 | 137 | // Insert the state in BE and mark it closed if the edge being expanded is dummy edge 138 | if (curr_edge_ptr->action_ptr_ == dummy_action_ptr_) 139 | { 140 | planner_stats_.num_state_expansions++; 141 | curr_edge_ptr->parent_state_ptr_->SetVisited(); 142 | curr_edge_ptr->parent_state_ptr_->SetBeingExpanded(); 143 | being_expanded_states_.push(curr_edge_ptr->parent_state_ptr_); 144 | } 145 | 146 | lock_.unlock(); 147 | 148 | int thread_id = 0; 149 | bool edge_expansion_assigned = false; 150 | 151 | if (num_threads_ == 1) 152 | { 153 | expand(curr_edge_ptr, 0); 154 | } 155 | else 156 | { 157 | while (!edge_expansion_assigned) 158 | { 159 | unique_lock locker(lock_vec_[thread_id]); 160 | bool status = edge_expansion_status_[thread_id]; 161 | locker.unlock(); 162 | 163 | if (!status) 164 | { 165 | int num_threads_current = edge_expansion_futures_.size(); 166 | if (thread_id >= num_threads_current) 167 | { 168 | if (VERBOSE) cout << "Spawning edge expansion thread " << thread_id << endl; 169 | edge_expansion_futures_.emplace_back(async(launch::async, &GepasePlanner::expandEdgeLoop, this, thread_id)); 170 | } 171 | locker.lock(); 172 | edge_expansion_vec_[thread_id] = curr_edge_ptr; 173 | edge_expansion_status_[thread_id] = 1; 174 | edge_expansion_assigned = true; 175 | locker.unlock(); 176 | cv_vec_[thread_id].notify_one(); 177 | } 178 | else 179 | { 180 | thread_id = thread_id == num_threads_-2 ? 0 : thread_id+1; 181 | } 182 | 183 | } 184 | } 185 | 186 | lock_.lock(); 187 | } 188 | 189 | terminate_ = true; 190 | auto t_end = chrono::steady_clock::now(); 191 | double t_elapsed = chrono::duration_cast(t_end-t_start_).count(); 192 | planner_stats_.total_time = 1e-9*t_elapsed; 193 | // cout << "Goal Not Reached, Number of states expanded: " << planner_stats_.num_state_expansions_ << endl; 194 | lock_.unlock(); 195 | exit(); 196 | return false; 197 | } 198 | 199 | void GepasePlanner::initialize() 200 | { 201 | Planner::initialize(); 202 | planner_stats_.num_jobs_per_thread.resize(num_threads_, 0); 203 | 204 | terminate_ = false; 205 | recheck_flag_ = true; 206 | 207 | edge_expansion_vec_.clear(); 208 | edge_expansion_vec_.resize(num_threads_-1, NULL); 209 | 210 | edge_expansion_status_.clear(); 211 | edge_expansion_status_.resize(num_threads_-1, 0); 212 | 213 | vector cv_vec(num_threads_-1); 214 | cv_vec_.swap(cv_vec); 215 | 216 | edge_expansion_futures_.clear(); 217 | 218 | // Insert proxy edge with start state 219 | dummy_action_ptr_ = NULL; 220 | auto edge_ptr = new Edge(start_state_ptr_, dummy_action_ptr_); 221 | edge_ptr->expansion_priority_ = heuristic_w_*computeHeuristic(start_state_ptr_); 222 | start_state_ptr_->SetFValue(heuristic_w_*computeHeuristic(start_state_ptr_)); 223 | 224 | edge_map_.insert(make_pair(getEdgeKey(edge_ptr), edge_ptr)); 225 | edge_open_list_.push(edge_ptr); 226 | } 227 | 228 | void GepasePlanner::notifyMainThread() 229 | { 230 | recheck_flag_ = true; 231 | cv_.notify_one(); 232 | } 233 | 234 | void GepasePlanner::expandEdgeLoop(int thread_id) 235 | { 236 | try{ 237 | while (!terminate_) 238 | { 239 | unique_lock locker(lock_vec_[thread_id]); 240 | cv_vec_[thread_id].wait(locker, [this, thread_id](){return (edge_expansion_status_[thread_id] == 1);}); 241 | locker.unlock(); 242 | 243 | if (terminate_) 244 | break; 245 | 246 | expand(edge_expansion_vec_[thread_id], thread_id); 247 | 248 | locker.lock(); 249 | edge_expansion_vec_[thread_id] = NULL; 250 | edge_expansion_status_[thread_id] = 0; 251 | locker.unlock(); 252 | } 253 | } catch (const exception &exc) 254 | { 255 | cout << "Caught Exception\n"; 256 | cerr << exc.what(); 257 | } 258 | } 259 | 260 | void GepasePlanner::expand(EdgePtrType edge_ptr, int thread_id) 261 | { 262 | auto t_start = chrono::steady_clock::now(); 263 | lock_.lock(); 264 | auto t_lock_e = chrono::steady_clock::now(); 265 | planner_stats_.lock_time += 1e-9*chrono::duration_cast(t_lock_e-t_start).count(); 266 | 267 | planner_stats_.num_jobs_per_thread[thread_id] +=1; 268 | 269 | // Proxy edge, add the real edges to Eopen 270 | if (edge_ptr->action_ptr_ == dummy_action_ptr_) 271 | { 272 | auto state_ptr = edge_ptr->parent_state_ptr_; 273 | 274 | for (auto& action_ptr: actions_ptrs_) 275 | { 276 | if (action_ptr->CheckPreconditions(state_ptr->GetStateVars())) 277 | { 278 | auto edge_ptr_next = new Edge(state_ptr, action_ptr); 279 | edge_map_.insert(make_pair(getEdgeKey(edge_ptr_next), edge_ptr_next)); 280 | edge_ptr_next->expansion_priority_ = edge_ptr->expansion_priority_; 281 | state_ptr->num_successors_+=1; 282 | 283 | if (action_ptr->IsExpensive()) 284 | { 285 | if (VERBOSE) cout << "Pushing successor with g_val: " << state_ptr->GetGValue() << " | h_val: " << state_ptr->GetHValue() << endl; 286 | edge_open_list_.push(edge_ptr_next); 287 | notifyMainThread(); 288 | } 289 | else 290 | { 291 | expandEdge(edge_ptr_next, thread_id); 292 | } 293 | } 294 | } 295 | 296 | } 297 | else // Real edge, evaluate and add proxy edges for child 298 | { 299 | expandEdge(edge_ptr, thread_id); 300 | } 301 | 302 | 303 | if (edge_ptr->parent_state_ptr_->num_expanded_successors_ == edge_ptr->parent_state_ptr_->num_successors_) 304 | { 305 | edge_ptr->parent_state_ptr_->UnsetBeingExpanded(); 306 | if (being_expanded_states_.contains(edge_ptr->parent_state_ptr_)) 307 | { 308 | being_expanded_states_.erase(edge_ptr->parent_state_ptr_); 309 | notifyMainThread(); 310 | } 311 | } 312 | 313 | if (edge_ptr->parent_state_ptr_->num_expanded_successors_ > edge_ptr->parent_state_ptr_->num_successors_) 314 | { 315 | edge_ptr->parent_state_ptr_->Print(); 316 | throw runtime_error("Number of expanded edges cannot be greater than number of successors"); 317 | } 318 | else 319 | { 320 | if (VERBOSE) edge_ptr->Print("Expansion completed "); 321 | } 322 | 323 | auto t_end_expansion = chrono::steady_clock::now(); 324 | planner_stats_.cumulative_expansions_time += 1e-9*chrono::duration_cast(t_end_expansion-t_start).count(); 325 | 326 | lock_.unlock(); 327 | } 328 | 329 | void GepasePlanner::expandEdge(EdgePtrType edge_ptr, int thread_id) 330 | { 331 | 332 | auto action_ptr = edge_ptr->action_ptr_; 333 | 334 | lock_.unlock(); 335 | // Evaluate the edge 336 | auto t_start = chrono::steady_clock::now(); 337 | auto action_successor = action_ptr->GetSuccessor(edge_ptr->parent_state_ptr_->GetStateVars(), thread_id); 338 | auto t_end = chrono::steady_clock::now(); 339 | //******************** 340 | 341 | auto t_lock_s = chrono::steady_clock::now(); 342 | lock_.lock(); 343 | auto t_lock_e = chrono::steady_clock::now(); 344 | 345 | planner_stats_.lock_time += 1e-9*chrono::duration_cast(t_lock_e-t_lock_s).count(); 346 | planner_stats_.action_eval_times[action_ptr->GetType()].emplace_back(1e-9*chrono::duration_cast(t_end-t_start).count()); 347 | 348 | planner_stats_.num_evaluated_edges++; // Only the edges controllers that satisfied pre-conditions and args are in the open list 349 | 350 | if (action_successor.success_) 351 | { 352 | auto successor_state_ptr = constructState(action_successor.successor_state_vars_costs_.back().first); 353 | double cost = action_successor.successor_state_vars_costs_.back().second; 354 | 355 | // Set successor and cost in expanded edge 356 | edge_ptr->child_state_ptr_ = successor_state_ptr; 357 | edge_ptr->SetCost(cost); 358 | 359 | if (!successor_state_ptr->IsVisited()) 360 | { 361 | double new_g_val = edge_ptr->parent_state_ptr_->GetGValue() + cost; 362 | 363 | if (successor_state_ptr->GetGValue() > new_g_val) 364 | { 365 | 366 | double h_val = successor_state_ptr->GetHValue(); 367 | 368 | if (h_val == -1) 369 | { 370 | h_val = computeHeuristic(successor_state_ptr); 371 | successor_state_ptr->SetHValue(h_val); 372 | } 373 | 374 | if (h_val != DINF) 375 | { 376 | h_val_min_ = h_val < h_val_min_ ? h_val : h_val_min_; 377 | successor_state_ptr->SetGValue(new_g_val); 378 | successor_state_ptr->SetFValue(new_g_val + heuristic_w_*h_val); 379 | successor_state_ptr->SetIncomingEdgePtr(edge_ptr); 380 | 381 | // Insert poxy edge 382 | auto edge_temp = Edge(successor_state_ptr, dummy_action_ptr_); 383 | auto edge_key = getEdgeKey(&edge_temp); 384 | auto it_edge = edge_map_.find(edge_key); 385 | EdgePtrType proxy_edge_ptr; 386 | 387 | if (it_edge == edge_map_.end()) 388 | { 389 | proxy_edge_ptr = new Edge(successor_state_ptr, dummy_action_ptr_); 390 | edge_map_.insert(make_pair(edge_key, proxy_edge_ptr)); 391 | } 392 | else 393 | { 394 | proxy_edge_ptr = it_edge->second; 395 | } 396 | 397 | proxy_edge_ptr->expansion_priority_ = new_g_val + heuristic_w_*h_val; 398 | 399 | if (edge_open_list_.contains(proxy_edge_ptr)) 400 | { 401 | edge_open_list_.decrease(proxy_edge_ptr); 402 | } 403 | else 404 | { 405 | edge_open_list_.push(proxy_edge_ptr); 406 | } 407 | 408 | notifyMainThread(); 409 | 410 | } 411 | 412 | } 413 | } 414 | } 415 | else 416 | { 417 | if (VERBOSE) edge_ptr->Print("No successors for"); 418 | } 419 | 420 | edge_ptr->parent_state_ptr_->num_expanded_successors_ += 1; 421 | 422 | } 423 | 424 | void GepasePlanner::exit() 425 | { 426 | for (int thread_id = 0; thread_id < num_threads_-1; ++thread_id) 427 | { 428 | unique_lock locker(lock_vec_[thread_id]); 429 | edge_expansion_status_[thread_id] = 1; 430 | locker.unlock(); 431 | cv_vec_[thread_id].notify_one(); 432 | } 433 | 434 | planner_stats_.num_threads_spawned = edge_expansion_futures_.size()+1; 435 | bool all_expansion_threads_terminated = false; 436 | while (!all_expansion_threads_terminated) 437 | { 438 | all_expansion_threads_terminated = true; 439 | for (auto& fut : edge_expansion_futures_) 440 | { 441 | if (!isFutureReady(fut)) 442 | { 443 | all_expansion_threads_terminated = false; 444 | break; 445 | } 446 | } 447 | } 448 | edge_expansion_futures_.clear(); 449 | 450 | while (!edge_open_list_.empty()) 451 | { 452 | edge_open_list_.pop(); 453 | } 454 | being_expanded_states_.clear(); 455 | 456 | Planner::exit(); 457 | } 458 | -------------------------------------------------------------------------------- /src/planners/MplpPlanner.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | using namespace std; 6 | using namespace ps; 7 | 8 | MplpPlanner::MplpPlanner(ParamsType planner_params): 9 | Planner(planner_params) 10 | { 11 | num_threads_ = planner_params["num_threads"]; 12 | vector lock_vec(num_threads_); 13 | lock_vec_.swap(lock_vec); 14 | } 15 | 16 | MplpPlanner::~MplpPlanner() 17 | { 18 | 19 | } 20 | 21 | bool MplpPlanner::Plan() 22 | { 23 | initialize(); 24 | startTimer(); 25 | 26 | delegate_edges_process_ = shared_ptr(new thread(&MplpPlanner::delegateEdges, this)); 27 | monitor_paths_process_ = shared_ptr(new thread(&MplpPlanner::monitorPaths, this)); 28 | planner_stats_.num_threads_spawned = 3; 29 | 30 | int plan_idx = 0; 31 | bool path_exists = true; 32 | while (!plan_found_ and path_exists) 33 | { 34 | 35 | lock_2_.lock(); 36 | plan_evaluation_outcomes_.resize(plan_idx+1); 37 | plan_evaluation_outcomes_[plan_idx] = -1; 38 | lock_2_.unlock(); 39 | 40 | path_exists = replanMPLP(); 41 | 42 | auto t_end = chrono::steady_clock::now(); 43 | double t_elapsed = chrono::duration_cast(t_end-t_start_).count(); 44 | plan_idx++; 45 | 46 | } 47 | 48 | if (VERBOSE) 49 | { 50 | if (plan_found_) 51 | { 52 | cout << "Success! Valid plan found! " << endl; 53 | } 54 | else 55 | { 56 | cout << "Failure! Valid plan not found! " << endl; 57 | } 58 | } 59 | 60 | terminate_ = true; 61 | 62 | auto t_end = chrono::steady_clock::now(); 63 | double t_elapsed = chrono::duration_cast(t_end-t_start_).count(); 64 | planner_stats_.total_time = 1e-9*t_elapsed; 65 | 66 | exit(); 67 | 68 | return plan_found_; 69 | } 70 | 71 | void MplpPlanner::initialize() 72 | { 73 | Planner::initialize(); 74 | planner_stats_.num_jobs_per_thread.resize(num_threads_, 0); 75 | 76 | terminate_ = false; 77 | plan_found_ = false; 78 | 79 | edge_evaluation_vec_.clear(); 80 | edge_evaluation_vec_.resize(num_threads_-1, NULL); 81 | 82 | edge_evaluation_status_.clear(); 83 | edge_evaluation_status_.resize(num_threads_-1, 0); 84 | 85 | edge_evaluation_futures_.clear(); 86 | 87 | lazy_plans_.clear(); 88 | plan_evaluation_outcomes_.clear(); 89 | successful_plan_idx_ = -1; 90 | cost_bound_ = -1; 91 | } 92 | 93 | void MplpPlanner::initializeReplanning() 94 | { 95 | resetStates(); 96 | 97 | while (!state_open_list_.empty()) 98 | { 99 | state_open_list_.pop(); 100 | } 101 | 102 | start_state_ptr_->SetGValue(0); 103 | state_open_list_.push(start_state_ptr_); 104 | } 105 | 106 | bool MplpPlanner::replanMPLP() 107 | { 108 | if (VERBOSE) cout << "------------ Replanning MPLP ------------" << endl; 109 | 110 | auto t_start_replanning = chrono::steady_clock::now(); 111 | 112 | initializeReplanning(); 113 | int num_expansions = 0; 114 | 115 | while (!state_open_list_.empty() && !checkTimeout()) 116 | { 117 | 118 | if (plan_found_) 119 | { 120 | // auto t_end_replanning = chrono::steady_clock::now(); 121 | // double t_elapsed_replanning = chrono::duration_cast(t_end_replanning-t_start_replanning).count(); 122 | // replanning_times_.emplace_back(1e-9*t_elapsed_replanning); 123 | return true; 124 | } 125 | 126 | // if ((PRINT_EXPANSIONS)&&(num_expansions%100 == 0)) 127 | // cout << "Expansion: " << num_expansions << endl; 128 | 129 | auto state_ptr = state_open_list_.min(); 130 | state_open_list_.pop(); 131 | 132 | // Return solution if goal state is expanded 133 | if (isGoalState(state_ptr)) 134 | { 135 | if (VERBOSE) cout << "Goal Reached, Number of states expanded: " << num_expansions << endl; 136 | 137 | goal_state_ptr_ = state_ptr; 138 | 139 | lock_2_.lock(); 140 | cost_bound_ = max(cost_bound_, goal_state_ptr_->GetGValue()); 141 | lock_2_.unlock(); 142 | 143 | // auto t_end_replanning = chrono::steady_clock::now(); 144 | // double t_elapsed_replanning = chrono::duration_cast(t_end_replanning-t_start_replanning).count(); 145 | // replanning_times_.emplace_back(1e-9*t_elapsed_replanning); 146 | 147 | // Reconstruct and return path 148 | constructPlan(goal_state_ptr_); 149 | return true; 150 | } 151 | 152 | expandState(state_ptr); 153 | state_ptr->SetVisited(); 154 | 155 | if (VERBOSE) 156 | { 157 | cout << "Open list size: " << state_open_list_.size() << endl; 158 | cout << "EQ size: " << edges_open_.size() << endl; 159 | cout << "Graph size: " << state_map_.size() << endl; 160 | cout << "State with min h_val in Open list: " << h_val_min_ << endl; 161 | } 162 | 163 | num_expansions++; 164 | // auto t_end = chrono::steady_clock::now(); 165 | // double t_elapsed = chrono::duration_cast(t_end-t_start).count(); 166 | // if ((timeout_>0) && (1e-9*t_elapsed > timeout_)) 167 | // return false; 168 | } 169 | 170 | // cout << "Goal not reached Number of states expanded: " << num_expansions << endl; 171 | // auto t_end_replanning = chrono::steady_clock::now(); 172 | // double t_elapsed_replanning = chrono::duration_cast(t_end_replanning-t_start_replanning).count(); 173 | // replanning_times_.emplace_back(1e-9*t_elapsed_replanning); 174 | return false; 175 | 176 | } 177 | 178 | void MplpPlanner::expandState(StatePtrType state_ptr) 179 | { 180 | 181 | if (VERBOSE) state_ptr->Print("Expanding"); 182 | 183 | bool state_expanded_before = false; 184 | 185 | for (auto& action_ptr: actions_ptrs_) 186 | { 187 | 188 | EdgePtrType edge_ptr = NULL; 189 | edge_ptr = new Edge(state_ptr, action_ptr); 190 | auto edge_key = getEdgeKey(edge_ptr); 191 | delete edge_ptr; 192 | edge_ptr = NULL; 193 | // Don't need a lock since no other thread is adding to edge_map_ except this. Which means 194 | // that when this line is being executed, no thread is modifying (writing) edge_map_. 195 | auto it_edge = edge_map_.find(edge_key); 196 | 197 | bool edge_generated = true; 198 | 199 | if (it_edge == edge_map_.end()) 200 | { 201 | if (VERBOSE) cout << "Expand: Edge not generated " << endl; 202 | edge_generated = false; 203 | } 204 | else if (it_edge->second->is_closed_) 205 | { 206 | // Edge in Eclosed 207 | if (VERBOSE) cout << "Expand: Edge in closed " << endl; 208 | edge_ptr = it_edge->second; 209 | } 210 | else if (edges_open_.contains(it_edge->second)) 211 | { 212 | // Edge in Eopen 213 | if (VERBOSE) cout << "Expand: Edge in open " << endl; 214 | edge_ptr = it_edge->second; 215 | } 216 | else if (it_edge->second->is_eval_) 217 | { 218 | // Edge in Eeval 219 | if (VERBOSE) cout << "Expand: Edge in eval " << endl; 220 | edge_ptr = it_edge->second; 221 | } 222 | else if (it_edge->second->is_invalid_) 223 | { 224 | if (VERBOSE) cout << "Invalid edge" << endl; 225 | continue; 226 | } 227 | 228 | if (!edge_generated && action_ptr->CheckPreconditions(state_ptr->GetStateVars())) 229 | { 230 | 231 | // Evaluate the edge 232 | auto t_start = chrono::steady_clock::now(); 233 | auto action_successor = action_ptr->GetSuccessorLazy(state_ptr->GetStateVars()); 234 | auto t_end = chrono::steady_clock::now(); 235 | //******************** 236 | 237 | // Only the actions that satisfied pre-conditions and args are in the open list 238 | 239 | if (action_successor.success_) 240 | { 241 | auto successor_state_ptr = constructState(action_successor.successor_state_vars_costs_.back().first); 242 | edge_ptr = new Edge(state_ptr, successor_state_ptr, action_ptr); 243 | edge_ptr->SetCost(action_successor.successor_state_vars_costs_.back().second); 244 | assignEdgePriority(edge_ptr); 245 | 246 | lock_.lock(); 247 | edge_map_.insert(make_pair(getEdgeKey(edge_ptr), edge_ptr)); 248 | edges_open_.push(edge_ptr); 249 | lock_.unlock(); 250 | 251 | } 252 | } 253 | 254 | if (edge_ptr && edge_ptr->child_state_ptr_ && (!edge_ptr->child_state_ptr_->IsVisited())) 255 | { 256 | auto successor_state_ptr = edge_ptr->child_state_ptr_; 257 | double new_g_val = state_ptr->GetGValue() + edge_ptr->GetCost(); 258 | 259 | if (successor_state_ptr->GetGValue() > new_g_val) 260 | { 261 | 262 | double h_val = successor_state_ptr->GetHValue(); 263 | 264 | if (h_val == -1) 265 | { 266 | h_val = computeHeuristic(successor_state_ptr); 267 | successor_state_ptr->SetHValue(h_val); 268 | } 269 | 270 | if (h_val != DINF) 271 | { 272 | h_val_min_ = h_val < h_val_min_ ? h_val : h_val_min_; 273 | successor_state_ptr->SetGValue(new_g_val); 274 | successor_state_ptr->SetFValue(new_g_val + heuristic_w_*h_val); 275 | successor_state_ptr->SetIncomingEdgePtr(edge_ptr); 276 | 277 | if (state_open_list_.contains(successor_state_ptr)) 278 | { 279 | state_open_list_.decrease(successor_state_ptr); 280 | } 281 | else 282 | { 283 | state_open_list_.push(successor_state_ptr); 284 | } 285 | 286 | } 287 | 288 | } 289 | } 290 | else 291 | { 292 | // Insert into Einvalid if no valid successor is generated 293 | edge_ptr = new Edge(state_ptr, action_ptr); 294 | edge_ptr->SetCost(DINF); 295 | edge_ptr->is_invalid_ = true; 296 | 297 | // Insert invalid edge into edge_map_. edge_map_ insert has to be under lock because edge_map_.find is happening 298 | // in updateEdgeCbk 299 | lock_.lock(); 300 | edge_map_.insert(make_pair(getEdgeKey(edge_ptr), edge_ptr)); 301 | lock_.unlock(); 302 | } 303 | 304 | if (edge_generated) 305 | state_expanded_before = true; 306 | 307 | } 308 | 309 | // if (state_expanded_before) 310 | // { 311 | // num_reexpansions += 1; 312 | // t_reexpansion_ += 1e-9*t_elapsed_expand; 313 | // } 314 | // else 315 | // { 316 | // num_firstexpansions += 1; 317 | // t_firstexpansion_ += 1e-9*t_elapsed_expand; 318 | // } 319 | 320 | } 321 | 322 | void MplpPlanner::delegateEdges() 323 | { 324 | while (!terminate_) 325 | { 326 | if (edges_open_.size() == 0) 327 | { 328 | continue; 329 | } 330 | 331 | lock_.lock(); 332 | auto edge_ptr = edges_open_.min(); 333 | edges_open_.pop(); 334 | lock_.unlock(); 335 | 336 | edge_ptr->is_eval_= true; 337 | 338 | // Find a free thread, spawn if necessary 339 | int thread_id = 3; // T0: lazy search, T1: monitorPaths, T2: delegateEdges 340 | bool edge_evaluation_assigned = false; 341 | 342 | while(!edge_evaluation_assigned) 343 | { 344 | lock_vec_[thread_id].lock(); 345 | // cout << "Checking thread " << thread_id << endl; 346 | bool status = edge_evaluation_status_[thread_id]; 347 | lock_vec_[thread_id].unlock(); 348 | 349 | if (!status) 350 | { 351 | 352 | if (thread_id-3 >= edge_evaluation_futures_.size()) 353 | { 354 | if (VERBOSE) cout << "Spawning edge evaluation thread " << thread_id << endl; 355 | edge_evaluation_futures_.emplace_back(async(launch::async, &MplpPlanner::evaluateEdgeLoop, this, thread_id)); 356 | planner_stats_.num_threads_spawned+=1; 357 | } 358 | 359 | if (VERBOSE) edge_ptr->Print("Delegating"); 360 | 361 | lock_vec_[thread_id].lock(); 362 | edge_evaluation_vec_[thread_id] = edge_ptr; 363 | edge_evaluation_status_[thread_id] = 1; 364 | edge_evaluation_assigned = true; 365 | lock_vec_[thread_id].unlock(); 366 | 367 | } 368 | else 369 | { 370 | thread_id = thread_id == num_threads_-1 ? 3 : thread_id+1; 371 | } 372 | } 373 | } 374 | } 375 | 376 | void MplpPlanner::evaluateEdge(EdgePtrType edge_ptr, int thread_id) 377 | { 378 | lock_.lock(); 379 | planner_stats_.num_evaluated_edges++; 380 | planner_stats_.num_jobs_per_thread[thread_id] +=1; 381 | lock_.unlock(); 382 | 383 | auto action = edge_ptr->action_ptr_; 384 | auto parent_state_ptr = edge_ptr->parent_state_ptr_; 385 | auto child_state_ptr = edge_ptr->child_state_ptr_; 386 | 387 | // auto t_start = chrono::steady_clock::now(); 388 | auto action_successor = action->Evaluate(parent_state_ptr->GetStateVars(), child_state_ptr->GetStateVars(), thread_id); 389 | // auto t_end = chrono::steady_clock::now(); 390 | // double t_elapsed = chrono::duration_cast(t_end-t_start).count(); 391 | // edge_ptr->real_eval_time_ = 1e-9*t_elapsed; 392 | 393 | if (action_successor.success_) 394 | { 395 | edge_ptr->SetCost(action_successor.successor_state_vars_costs_.back().second); 396 | } 397 | else 398 | { 399 | edge_ptr->SetCost(DINF); 400 | } 401 | 402 | edge_ptr->is_eval_ = false; 403 | 404 | // Edge must be marked close after all other info like cost and booleans are set 405 | edge_ptr->is_closed_ = true; 406 | } 407 | 408 | void MplpPlanner::evaluateEdgeLoop(int thread_id) 409 | { 410 | while (!terminate_) 411 | { 412 | 413 | lock_vec_[thread_id].lock(); 414 | bool status = edge_evaluation_status_[thread_id]; 415 | lock_vec_[thread_id].unlock(); 416 | 417 | while ((!status) && (!terminate_)) 418 | { 419 | lock_vec_[thread_id].lock(); 420 | status = edge_evaluation_status_[thread_id]; 421 | lock_vec_[thread_id].unlock(); 422 | // cout << "Evaluation thread " << thread_id << " waiting! " << edge_evaluation_status_[thread_id] << endl; 423 | } 424 | 425 | if (terminate_) 426 | break; 427 | 428 | 429 | evaluateEdge(edge_evaluation_vec_[thread_id], thread_id); 430 | 431 | lock_vec_[thread_id].lock(); 432 | edge_evaluation_vec_[thread_id] = NULL; 433 | edge_evaluation_status_[thread_id] = 0; 434 | lock_vec_[thread_id].unlock(); 435 | } 436 | 437 | } 438 | 439 | void MplpPlanner::constructPlan(StatePtrType goal_state_ptr) 440 | { 441 | auto state_ptr = goal_state_ptr; 442 | vector edges_in_plan; 443 | 444 | while(state_ptr->GetIncomingEdgePtr()) 445 | { 446 | edges_in_plan.insert(edges_in_plan.begin(), state_ptr->GetIncomingEdgePtr()); 447 | state_ptr = state_ptr->GetIncomingEdgePtr()->parent_state_ptr_; 448 | } 449 | 450 | // Detect duplicate paths in Pi (lazy_plans_) 451 | bool duplicate_path = false; 452 | for (auto path: lazy_plans_) 453 | { 454 | if ((edges_in_plan == path) && (!duplicate_path)) 455 | { 456 | duplicate_path = true; 457 | auto it_edges_in_plan = edges_in_plan.begin(); 458 | 459 | for (auto edge_in_plan: path) 460 | { 461 | if ((*it_edges_in_plan)->edge_id_ != edge_in_plan->edge_id_) 462 | { 463 | duplicate_path=false; 464 | break; 465 | } 466 | 467 | it_edges_in_plan++; 468 | } 469 | } 470 | } 471 | 472 | if (!duplicate_path) 473 | { 474 | lock_.lock(); 475 | updateEdgePriority(edges_in_plan, 2); 476 | lock_.unlock(); 477 | 478 | lock_2_.lock(); 479 | lazy_plans_.emplace_back(edges_in_plan); 480 | lock_2_.unlock(); 481 | } 482 | } 483 | 484 | void MplpPlanner::monitorPaths() 485 | { 486 | while (!terminate_) 487 | { 488 | lock_2_.lock(); 489 | auto lazy_plans = lazy_plans_; 490 | lock_2_.unlock(); 491 | 492 | for (int plan_idx = 0; plan_idx < lazy_plans.size(); ++plan_idx) 493 | { 494 | lock_2_.lock(); 495 | int plan_outcome = plan_evaluation_outcomes_[plan_idx]; 496 | lock_2_.unlock(); 497 | 498 | if ((plan_outcome == 0) || (plan_outcome == 1)) 499 | continue; 500 | 501 | auto lazy_plan = lazy_plans[plan_idx]; 502 | 503 | std::vector plan; 504 | bool curr_plan_valid = true; 505 | 506 | for (auto& edge_ptr: lazy_plan) 507 | { 508 | 509 | if (edge_ptr->is_closed_) 510 | { 511 | if (edge_ptr->GetCost() == DINF) 512 | { 513 | curr_plan_valid = false; 514 | lock_2_.lock(); 515 | plan_evaluation_outcomes_[plan_idx] = 0; 516 | lock_2_.unlock(); 517 | break; 518 | } 519 | } 520 | else 521 | { 522 | curr_plan_valid = false; 523 | break; 524 | } 525 | 526 | } 527 | 528 | if (curr_plan_valid) 529 | { 530 | 531 | double cost = 0; 532 | for (auto& edge_ptr: lazy_plan) 533 | { 534 | if (edge_ptr->parent_state_ptr_) 535 | { 536 | plan.emplace_back(PlanElement(edge_ptr->parent_state_ptr_->GetStateVars(), edge_ptr->action_ptr_, edge_ptr->GetCost())); 537 | } 538 | cost = cost + edge_ptr->GetCost(); 539 | } 540 | 541 | lock_2_.lock(); 542 | // This condition needs to hold true to gurantee sub-optimality bound 543 | if (cost <= cost_bound_) 544 | { 545 | 546 | if (VERBOSE) 547 | { 548 | cout << "VVVVVVVVVVVVVVVVVVVVVVVVVVVVVV Valid plan found! VVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVV" << endl; 549 | cout << "Plan index: " << plan_idx << endl; 550 | cout << "Plan length: " << plan.size() << endl; 551 | cout << "Plan cost: " << cost << endl; 552 | cout << "VVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVV" << endl; 553 | } 554 | 555 | plan_evaluation_outcomes_[plan_idx] = 1; 556 | 557 | // Only update these for the first valid plan 558 | if (!plan_found_) 559 | { 560 | plan_ = plan; 561 | successful_plan_idx_ = plan_idx; 562 | planner_stats_.path_cost = cost; 563 | planner_stats_.path_length += plan_.size(); 564 | plan_found_ = true; 565 | } 566 | 567 | } 568 | else 569 | { 570 | plan_evaluation_outcomes_[plan_idx] = 0; 571 | if (VERBOSE) cout << "Valid plan not optimal, cost bound: " << cost_bound_ << " | true cost of plan: " << cost << endl; 572 | } 573 | 574 | lock_2_.unlock(); 575 | 576 | } 577 | 578 | } 579 | 580 | } 581 | 582 | } 583 | 584 | void MplpPlanner::assignEdgePriority(EdgePtrType& edge_ptr) 585 | { 586 | edge_ptr->evaluation_priority_ = 1; 587 | } 588 | 589 | void MplpPlanner::updateEdgePriority(vector& edge_ptrs, double factor) 590 | { 591 | for (auto& edge_ptr : edge_ptrs) 592 | { 593 | updateEdgePriority(edge_ptr, factor); 594 | } 595 | } 596 | 597 | void MplpPlanner::updateEdgePriority(EdgePtrType& edge_ptr, double factor) 598 | { 599 | bool edge_found = false; 600 | 601 | if (edges_open_.contains(edge_ptr)) 602 | { 603 | edge_found = true; 604 | edge_ptr->evaluation_priority_ = factor*edge_ptr->evaluation_priority_; 605 | 606 | if (factor > 1) 607 | { 608 | edges_open_.decrease(edge_ptr); 609 | } 610 | else 611 | { 612 | edges_open_.increase(edge_ptr); 613 | } 614 | } 615 | } 616 | 617 | void MplpPlanner::exit() 618 | { 619 | delegate_edges_process_->join(); 620 | monitor_paths_process_->join(); 621 | 622 | bool all_edge_eval_threads_terminated = false; 623 | while (!all_edge_eval_threads_terminated) 624 | { 625 | all_edge_eval_threads_terminated = true; 626 | for (auto& fut : edge_evaluation_futures_) 627 | { 628 | if (!isFutureReady(fut)) 629 | { 630 | all_edge_eval_threads_terminated = false; 631 | break; 632 | } 633 | } 634 | } 635 | 636 | // Clear open list 637 | while (!state_open_list_.empty()) 638 | { 639 | state_open_list_.pop(); 640 | } 641 | 642 | // Clear Eopen 643 | while (!edges_open_.empty()) 644 | { 645 | edges_open_.pop(); 646 | } 647 | 648 | lazy_plans_.clear(); 649 | plan_evaluation_outcomes_.clear(); 650 | edge_evaluation_vec_.clear(); 651 | edge_evaluation_status_.clear(); 652 | 653 | Planner::exit(); 654 | } 655 | -------------------------------------------------------------------------------- /src/planners/PasePlanner.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | using namespace std; 6 | using namespace ps; 7 | 8 | PasePlanner::PasePlanner(ParamsType planner_params): 9 | Planner(planner_params) 10 | { 11 | num_threads_ = planner_params["num_threads"]; 12 | vector lock_vec(num_threads_-1); 13 | lock_vec_.swap(lock_vec); 14 | } 15 | 16 | PasePlanner::~PasePlanner() 17 | { 18 | 19 | } 20 | 21 | bool PasePlanner::Plan() 22 | { 23 | initialize(); 24 | 25 | planner_stats_.num_threads_spawned = 1; 26 | startTimer(); 27 | if (num_threads_ == 1) 28 | { 29 | paseThread(0); 30 | } 31 | else 32 | { 33 | for (int thread_id = 0; thread_id < num_threads_-1; ++thread_id) 34 | { 35 | if (VERBOSE) cout << "Spawining state expansion thread " << thread_id << endl; 36 | state_expansion_futures_.emplace_back(async(launch::async, &PasePlanner::paseThread, this, thread_id)); 37 | } 38 | 39 | planner_stats_.num_threads_spawned += state_expansion_futures_.size(); 40 | } 41 | 42 | // Spin till termination, should be replaced by conditional variable 43 | while(!terminate_&& !checkTimeout()){} 44 | 45 | terminate_ = true; 46 | auto t_end = chrono::steady_clock::now(); 47 | double t_elapsed = chrono::duration_cast(t_end-t_start_).count(); 48 | planner_stats_.total_time = 1e-9*t_elapsed; 49 | exit(); 50 | 51 | return plan_found_; 52 | } 53 | 54 | void PasePlanner::initialize() 55 | { 56 | Planner::initialize(); 57 | planner_stats_.num_jobs_per_thread.resize(num_threads_, 0); 58 | 59 | terminate_ = false; 60 | recheck_flag_ = true; 61 | plan_found_ = false; 62 | 63 | state_expansion_futures_.clear(); 64 | being_expanded_states_.clear(); 65 | 66 | // Initialize open list 67 | start_state_ptr_->SetFValue(start_state_ptr_->GetGValue() + heuristic_w_*start_state_ptr_->GetHValue()); 68 | state_open_list_.push(start_state_ptr_); 69 | 70 | } 71 | 72 | void PasePlanner::paseThread(int thread_id) 73 | { 74 | lock_.lock(); 75 | vector popped_state_ptrs; 76 | 77 | while(!terminate_) 78 | { 79 | 80 | StatePtrType state_ptr = NULL; 81 | 82 | while (!state_ptr && !terminate_) 83 | { 84 | 85 | // No solution check 86 | if (state_open_list_.empty() && being_expanded_states_.empty()) 87 | { 88 | terminate_ = true; 89 | plan_found_ = false; 90 | lock_.unlock(); 91 | return; 92 | } 93 | 94 | while(!state_ptr && !state_open_list_.empty()) 95 | { 96 | state_ptr = state_open_list_.min(); 97 | state_open_list_.pop(); 98 | popped_state_ptrs.emplace_back(state_ptr); 99 | 100 | 101 | if (state_ptr->IsBeingExpanded()) 102 | continue; 103 | 104 | for (auto& popped_state_ptr : popped_state_ptrs) 105 | { 106 | auto h_diff = computeHeuristic(popped_state_ptr, state_ptr); 107 | 108 | if (state_ptr->GetGValue() > popped_state_ptr->GetGValue() + heuristic_w_*h_diff) 109 | { 110 | state_ptr = NULL; 111 | break; 112 | } 113 | } 114 | 115 | if (state_ptr) 116 | { 117 | for (auto& being_expanded_state : being_expanded_states_) 118 | { 119 | auto h_diff = computeHeuristic(being_expanded_state, state_ptr); 120 | if (state_ptr->GetGValue() > being_expanded_state->GetGValue() + heuristic_w_*h_diff) 121 | { 122 | state_ptr = NULL; 123 | break; 124 | } 125 | } 126 | } 127 | 128 | 129 | } 130 | 131 | 132 | // Re add the popped states except curr state which will be expanded now 133 | for (auto& popped_state_ptr : popped_state_ptrs) 134 | { 135 | if (popped_state_ptr != state_ptr) 136 | { 137 | state_open_list_.push(popped_state_ptr); 138 | } 139 | } 140 | popped_state_ptrs.clear(); 141 | 142 | if (!state_ptr) 143 | { 144 | lock_.unlock(); 145 | // Wait for recheck_flag_ to be set true; 146 | while(!recheck_flag_ && !terminate_){} 147 | lock_.lock(); 148 | continue; 149 | } 150 | 151 | recheck_flag_ = false; 152 | 153 | // Return solution if goal state is expanded 154 | if (isGoalState(state_ptr) && (!terminate_)) 155 | { 156 | // Reconstruct path and return 157 | goal_state_ptr_ = state_ptr; 158 | constructPlan(goal_state_ptr_); 159 | plan_found_ = true; 160 | terminate_ = true; 161 | recheck_flag_ = true; 162 | lock_.unlock(); 163 | return; 164 | } 165 | 166 | } 167 | 168 | // Needed since the while loop may exit when plan is found and state_ptr is NULL 169 | if (!state_ptr) 170 | { 171 | lock_.unlock(); 172 | return; 173 | } 174 | 175 | state_ptr->SetBeingExpanded(); 176 | being_expanded_states_.emplace_back(state_ptr); 177 | state_ptr->SetVisited(); 178 | 179 | expandState(state_ptr, thread_id); 180 | } 181 | 182 | lock_.unlock(); 183 | 184 | } 185 | 186 | void PasePlanner::expandState(StatePtrType state_ptr, int thread_id) 187 | { 188 | 189 | if (VERBOSE) state_ptr->Print("Expanding"); 190 | 191 | planner_stats_.num_jobs_per_thread[thread_id] +=1; 192 | planner_stats_.num_state_expansions++; 193 | 194 | for (auto& action_ptr: actions_ptrs_) 195 | { 196 | if (action_ptr->CheckPreconditions(state_ptr->GetStateVars())) 197 | { 198 | 199 | lock_.unlock(); 200 | // Evaluate the edge 201 | auto t_start = chrono::steady_clock::now(); 202 | auto action_successor = action_ptr->GetSuccessor(state_ptr->GetStateVars(), thread_id); 203 | auto t_end = chrono::steady_clock::now(); 204 | //******************** 205 | lock_.lock(); 206 | planner_stats_.action_eval_times[action_ptr->GetType()].emplace_back(1e-9*chrono::duration_cast(t_end-t_start).count()); 207 | planner_stats_.num_evaluated_edges++; // Only the edges controllers that satisfied pre-conditions and args are in the open list 208 | 209 | if (action_successor.success_) 210 | { 211 | auto successor_state_ptr = constructState(action_successor.successor_state_vars_costs_.back().first); 212 | double cost = action_successor.successor_state_vars_costs_.back().second; 213 | 214 | if (!successor_state_ptr->IsVisited()) 215 | { 216 | double new_g_val = state_ptr->GetGValue() + cost; 217 | 218 | if (successor_state_ptr->GetGValue() > new_g_val) 219 | { 220 | 221 | double h_val = successor_state_ptr->GetHValue(); 222 | 223 | if (h_val == -1) 224 | { 225 | h_val = computeHeuristic(successor_state_ptr); 226 | successor_state_ptr->SetHValue(h_val); 227 | } 228 | 229 | if (h_val != DINF) 230 | { 231 | h_val_min_ = h_val < h_val_min_ ? h_val : h_val_min_; 232 | successor_state_ptr->SetGValue(new_g_val); 233 | successor_state_ptr->SetFValue(new_g_val + heuristic_w_*h_val); 234 | 235 | auto edge_ptr = new Edge(state_ptr, successor_state_ptr, action_ptr); 236 | edge_ptr->SetCost(cost); 237 | edge_map_.insert(make_pair(getEdgeKey(edge_ptr), edge_ptr)); 238 | 239 | successor_state_ptr->SetIncomingEdgePtr(edge_ptr); 240 | 241 | if (state_open_list_.contains(successor_state_ptr)) 242 | { 243 | state_open_list_.decrease(successor_state_ptr); 244 | } 245 | else 246 | { 247 | state_open_list_.push(successor_state_ptr); 248 | } 249 | 250 | } 251 | 252 | } 253 | } 254 | } 255 | } 256 | } 257 | 258 | state_ptr->UnsetBeingExpanded(); 259 | auto it_state_be = find(being_expanded_states_.begin(), being_expanded_states_.end(), state_ptr); 260 | if (it_state_be != being_expanded_states_.end()) 261 | { 262 | being_expanded_states_.erase(it_state_be); 263 | } 264 | 265 | recheck_flag_ = true; 266 | } 267 | 268 | void PasePlanner::exit() 269 | { 270 | bool all_expansion_threads_terminated = false; 271 | while (!all_expansion_threads_terminated) 272 | { 273 | all_expansion_threads_terminated = true; 274 | for (auto& fut : state_expansion_futures_) 275 | { 276 | if (!isFutureReady(fut)) 277 | { 278 | all_expansion_threads_terminated = false; 279 | break; 280 | } 281 | } 282 | } 283 | state_expansion_futures_.clear(); 284 | 285 | // Clear open list 286 | while (!state_open_list_.empty()) 287 | { 288 | state_open_list_.pop(); 289 | } 290 | 291 | Planner::exit(); 292 | } 293 | -------------------------------------------------------------------------------- /src/planners/Planner.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | using namespace std; 6 | using namespace ps; 7 | 8 | Planner::Planner(ParamsType planner_params): 9 | planner_params_(planner_params) 10 | { 11 | heuristic_w_ = planner_params_["heuristic_weight"]; 12 | } 13 | 14 | Planner::~Planner() 15 | { 16 | cleanUp(); 17 | } 18 | 19 | void Planner::SetActions(vector> actions_ptrs) 20 | { 21 | actions_ptrs_ = actions_ptrs; 22 | } 23 | 24 | void Planner::SetStartState(const StateVarsType& state_vars) 25 | { 26 | start_state_ptr_ = constructState(state_vars); 27 | } 28 | 29 | void Planner::SetGoalChecker(function callback) 30 | { 31 | goal_checker_ = callback; 32 | } 33 | 34 | void Planner::SetStateMapKeyGenerator(function callback) 35 | { 36 | state_key_generator_ = callback; 37 | } 38 | 39 | void Planner::SetEdgeKeyGenerator(function callback) 40 | { 41 | edge_key_generator_ = callback; 42 | } 43 | 44 | void Planner::SetHeuristicGenerator(function callback) 45 | { 46 | unary_heuristic_generator_ = callback; 47 | } 48 | 49 | void Planner::SetStateToStateHeuristicGenerator(function callback) 50 | { 51 | binary_heuristic_generator_ = callback; 52 | } 53 | 54 | void Planner::SetPostProcessor(std::function& plan, double& cost)> callback) 55 | { 56 | post_processor_ = callback; 57 | } 58 | 59 | std::vector Planner::GetPlan() const 60 | { 61 | return plan_; 62 | } 63 | 64 | PlannerStats Planner::GetStats() const 65 | { 66 | return planner_stats_; 67 | } 68 | 69 | void Planner::initialize() 70 | { 71 | plan_.clear(); 72 | 73 | // Initialize planner stats 74 | planner_stats_ = PlannerStats(); 75 | 76 | // Initialize start state 77 | start_state_ptr_->SetGValue(0); 78 | start_state_ptr_->SetHValue(computeHeuristic(start_state_ptr_)); 79 | 80 | // Reset goal state 81 | goal_state_ptr_ = NULL; 82 | 83 | // Reset h_min 84 | h_val_min_ = DINF; 85 | 86 | } 87 | 88 | void Planner::startTimer() 89 | { 90 | t_start_ = chrono::steady_clock::now(); 91 | } 92 | 93 | bool Planner::checkTimeout() 94 | { 95 | auto t_end = chrono::steady_clock::now(); 96 | double t_elapsed = 1e-9*chrono::duration_cast(t_end-t_start_).count(); 97 | return t_elapsed > planner_params_["timeout"]; 98 | } 99 | 100 | void Planner::resetStates() 101 | { 102 | for (auto it = state_map_.begin(); it != state_map_.end(); ++it) 103 | { 104 | it->second->ResetGValue(); 105 | it->second->ResetFValue(); 106 | it->second->ResetVValue(); 107 | it->second->ResetIncomingEdgePtr(); 108 | it->second->UnsetVisited(); 109 | it->second->UnsetBeingExpanded(); 110 | it->second->num_successors_ = 0; 111 | it->second->num_expanded_successors_ = 0; 112 | } 113 | } 114 | 115 | void Planner::resetClosed() 116 | { 117 | for (auto it = state_map_.begin(); it != state_map_.end(); ++it) 118 | { 119 | it->second->UnsetVisited(); 120 | it->second->UnsetBeingExpanded(); 121 | it->second->num_successors_ = 0; 122 | it->second->num_expanded_successors_ = 0; 123 | } 124 | } 125 | 126 | size_t Planner::getEdgeKey(const EdgePtrType& edge_ptr) 127 | { 128 | return edge_key_generator_(edge_ptr); 129 | } 130 | 131 | StatePtrType Planner::constructState(const StateVarsType& state) 132 | { 133 | size_t key = state_key_generator_(state); 134 | StatePtrMapType::iterator it = state_map_.find(key); 135 | StatePtrType state_ptr; 136 | 137 | // Check if state exists in the search state map 138 | if (it == state_map_.end()) 139 | { 140 | state_ptr = new State(state); 141 | state_map_.insert(pair(key, state_ptr)); 142 | } 143 | else 144 | { 145 | state_ptr = it->second; 146 | } 147 | 148 | return state_ptr; 149 | } 150 | 151 | double Planner::computeHeuristic(const StatePtrType& state_ptr) 152 | { 153 | return roundOff(unary_heuristic_generator_(state_ptr->GetStateVars())); 154 | } 155 | 156 | double Planner::computeHeuristic(const StatePtrType& state_ptr_1, const StatePtrType& state_ptr_2) 157 | { 158 | return roundOff(binary_heuristic_generator_(state_ptr_1->GetStateVars(), state_ptr_2->GetStateVars())); 159 | } 160 | 161 | bool Planner::isGoalState(const StatePtrType& state_ptr) 162 | { 163 | return goal_checker_(state_ptr->GetStateVars()); 164 | } 165 | 166 | void Planner::constructPlan(StatePtrType& state_ptr) 167 | { 168 | double cost = 0; 169 | while(state_ptr) 170 | { 171 | // state_ptr->Print("Plan state"); 172 | if (state_ptr->GetIncomingEdgePtr()) 173 | { 174 | plan_.insert(plan_.begin(), PlanElement(state_ptr->GetStateVars(), state_ptr->GetIncomingEdgePtr()->action_ptr_, state_ptr->GetIncomingEdgePtr()->GetCost())); 175 | cost += state_ptr->GetIncomingEdgePtr()->GetCost(); 176 | state_ptr = state_ptr->GetIncomingEdgePtr()->parent_state_ptr_; 177 | } 178 | else 179 | { 180 | // For start state_ptr, there is no incoming edge 181 | plan_.insert(plan_.begin(), PlanElement(state_ptr->GetStateVars(), NULL, 0)); 182 | state_ptr = NULL; 183 | } 184 | } 185 | 186 | planner_stats_.path_cost= cost; 187 | planner_stats_.path_length = plan_.size(); 188 | 189 | if (post_processor_) 190 | { 191 | post_processor_(plan_, cost); 192 | } 193 | 194 | planner_stats_.processed_path_cost= cost; 195 | planner_stats_.processed_path_length = plan_.size(); 196 | } 197 | 198 | double Planner::roundOff(double value, int prec) 199 | { 200 | double pow_10 = pow(10.0, prec); 201 | return round(value * pow_10) / pow_10; 202 | } 203 | 204 | void Planner::cleanUp() 205 | { 206 | for (auto& state_it : state_map_) 207 | { 208 | if (state_it.second) 209 | { 210 | delete state_it.second; 211 | state_it.second = NULL; 212 | } 213 | } 214 | state_map_.clear(); 215 | 216 | for (auto& edge_it : edge_map_) 217 | { 218 | if (edge_it.second) 219 | { 220 | delete edge_it.second; 221 | edge_it.second = NULL; 222 | } 223 | } 224 | edge_map_.clear(); 225 | 226 | State::ResetStateIDCounter(); 227 | Edge::ResetStateIDCounter(); 228 | } 229 | 230 | void Planner::exit() 231 | { 232 | cleanUp(); 233 | } 234 | -------------------------------------------------------------------------------- /src/planners/PwastarPlanner.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | using namespace std; 6 | using namespace ps; 7 | 8 | PwastarPlanner::PwastarPlanner(ParamsType planner_params): 9 | WastarPlanner(planner_params) 10 | { 11 | num_threads_ = planner_params["num_threads"]; 12 | vector lock_vec(num_threads_-1); 13 | lock_vec_.swap(lock_vec); 14 | } 15 | 16 | PwastarPlanner::~PwastarPlanner() 17 | { 18 | 19 | } 20 | 21 | bool PwastarPlanner::Plan() 22 | { 23 | initialize(); 24 | startTimer(); 25 | while (!state_open_list_.empty() && !checkTimeout()) 26 | { 27 | auto state_ptr = state_open_list_.min(); 28 | state_open_list_.pop(); 29 | 30 | // Return solution if goal state is expanded 31 | if (isGoalState(state_ptr)) 32 | { 33 | auto t_end = chrono::steady_clock::now(); 34 | double t_elapsed = chrono::duration_cast(t_end-t_start_).count(); 35 | goal_state_ptr_ = state_ptr; 36 | 37 | // Reconstruct and return path 38 | constructPlan(state_ptr); 39 | planner_stats_.total_time = 1e-9*t_elapsed; 40 | terminate_ = true; 41 | exit(); 42 | return true; 43 | } 44 | 45 | expandState(state_ptr); 46 | 47 | } 48 | 49 | auto t_end = chrono::steady_clock::now(); 50 | double t_elapsed = chrono::duration_cast(t_end-t_start_).count(); 51 | planner_stats_.total_time = 1e-9*t_elapsed; 52 | return false; 53 | } 54 | 55 | void PwastarPlanner::initialize() 56 | { 57 | WastarPlanner::initialize(); 58 | 59 | terminate_ = false; 60 | 61 | edge_evaluation_vec_.clear(); 62 | edge_evaluation_vec_.resize(num_threads_-1, NULL); 63 | 64 | edge_evaluation_status_.clear(); 65 | edge_evaluation_status_.resize(num_threads_-1, 0); 66 | 67 | edge_evaluation_futures_.clear(); 68 | 69 | all_successors_.resize(max(1, num_threads_-1)); 70 | } 71 | 72 | void PwastarPlanner::expandState(StatePtrType state_ptr) 73 | { 74 | 75 | if (num_threads_ == 1) 76 | WastarPlanner::expandState(state_ptr); 77 | else 78 | { 79 | if (VERBOSE) state_ptr->Print("Expanding"); 80 | 81 | state_ptr->SetVisited(); 82 | planner_stats_.num_state_expansions++; 83 | 84 | vector> valid_action_ptrs; 85 | for (auto& action_ptr: actions_ptrs_) 86 | { 87 | if (action_ptr->CheckPreconditions(state_ptr->GetStateVars())) 88 | { 89 | valid_action_ptrs.emplace_back(action_ptr); 90 | } 91 | } 92 | 93 | random_shuffle(valid_action_ptrs.begin(), valid_action_ptrs.end()); 94 | 95 | 96 | for (auto& action_ptr: valid_action_ptrs) 97 | { 98 | int thread_id = 0; 99 | bool edge_evaluation_assigned = false; 100 | while (!edge_evaluation_assigned) 101 | { 102 | lock_vec_[thread_id].lock(); 103 | bool status = edge_evaluation_status_[thread_id]; 104 | lock_vec_[thread_id].unlock(); 105 | 106 | if (!status) 107 | { 108 | int num_threads_current = edge_evaluation_futures_.size(); 109 | if (thread_id >= num_threads_current) 110 | { 111 | // cout << "Spawning thread " << thread_id << endl; 112 | edge_evaluation_futures_.emplace_back(async(launch::async, &PwastarPlanner::evaluateEdgeThread, this, thread_id)); 113 | } 114 | 115 | lock_vec_[thread_id].lock(); 116 | // cout << "Assigning thread_id: " << thread_id <action_ptr_; 175 | auto state_ptr = edge_evaluation_vec_[thread_id]->parent_state_ptr_; 176 | 177 | auto t_start = chrono::steady_clock::now(); 178 | auto action_successor = action_ptr->GetSuccessor(state_ptr->GetStateVars()); 179 | auto t_end = chrono::steady_clock::now(); 180 | 181 | lock_vec_[thread_id].lock(); 182 | planner_stats_.action_eval_times[action_ptr->GetType()].emplace_back(1e-9*chrono::duration_cast(t_end-t_start).count()); 183 | all_successors_[thread_id].emplace_back(make_pair(action_ptr, action_successor)); 184 | delete edge_evaluation_vec_[thread_id]; 185 | edge_evaluation_status_[thread_id] = 0; 186 | planner_stats_.num_jobs_per_thread[thread_id] +=1; 187 | lock_vec_[thread_id].unlock(); 188 | } 189 | } 190 | 191 | void PwastarPlanner::exit() 192 | { 193 | planner_stats_.num_threads_spawned += edge_evaluation_futures_.size(); 194 | WastarPlanner::exit(); 195 | } 196 | -------------------------------------------------------------------------------- /src/planners/WastarPlanner.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | using namespace std; 5 | using namespace ps; 6 | 7 | WastarPlanner::WastarPlanner(ParamsType planner_params): 8 | Planner(planner_params) 9 | { 10 | 11 | } 12 | 13 | WastarPlanner::~WastarPlanner() 14 | { 15 | 16 | } 17 | 18 | bool WastarPlanner::Plan() 19 | { 20 | initialize(); 21 | startTimer(); 22 | while (!state_open_list_.empty() && !checkTimeout()) 23 | { 24 | auto state_ptr = state_open_list_.min(); 25 | state_open_list_.pop(); 26 | 27 | // Return solution if goal state is expanded 28 | if (isGoalState(state_ptr)) 29 | { 30 | auto t_end = chrono::steady_clock::now(); 31 | double t_elapsed = chrono::duration_cast(t_end-t_start_).count(); 32 | goal_state_ptr_ = state_ptr; 33 | 34 | // Reconstruct and return path 35 | constructPlan(state_ptr); 36 | planner_stats_.total_time = 1e-9*t_elapsed; 37 | exit(); 38 | return true; 39 | } 40 | 41 | expandState(state_ptr); 42 | 43 | } 44 | 45 | auto t_end = chrono::steady_clock::now(); 46 | double t_elapsed = chrono::duration_cast(t_end-t_start_).count(); 47 | planner_stats_.total_time = 1e-9*t_elapsed; 48 | return false; 49 | } 50 | 51 | void WastarPlanner::initialize() 52 | { 53 | Planner::initialize(); 54 | planner_stats_.num_jobs_per_thread.resize(1, 0); 55 | 56 | // Initialize open list 57 | start_state_ptr_->SetFValue(start_state_ptr_->GetGValue() + heuristic_w_*start_state_ptr_->GetHValue()); 58 | state_open_list_.push(start_state_ptr_); 59 | 60 | planner_stats_.num_threads_spawned = 1; 61 | } 62 | 63 | void WastarPlanner::expandState(StatePtrType state_ptr) 64 | { 65 | 66 | if (VERBOSE) state_ptr->Print("Expanding"); 67 | 68 | planner_stats_.num_jobs_per_thread[0] +=1; 69 | planner_stats_.num_state_expansions++; 70 | 71 | state_ptr->SetVisited(); 72 | 73 | for (auto& action_ptr: actions_ptrs_) 74 | { 75 | if (action_ptr->CheckPreconditions(state_ptr->GetStateVars())) 76 | { 77 | // Evaluate the edge 78 | auto t_start = chrono::steady_clock::now(); 79 | auto action_successor = action_ptr->GetSuccessor(state_ptr->GetStateVars()); 80 | auto t_end = chrono::steady_clock::now(); 81 | planner_stats_.action_eval_times[action_ptr->GetType()].emplace_back(1e-9*chrono::duration_cast(t_end-t_start).count()); 82 | planner_stats_.num_evaluated_edges++; // Only the edges controllers that satisfied pre-conditions and args are in the open list 83 | //******************** 84 | 85 | updateState(state_ptr, action_ptr, action_successor); 86 | } 87 | } 88 | } 89 | 90 | void WastarPlanner::updateState(StatePtrType& state_ptr, ActionPtrType& action_ptr, ActionSuccessor& action_successor) 91 | { 92 | if (action_successor.success_) 93 | { 94 | auto successor_state_ptr = constructState(action_successor.successor_state_vars_costs_.back().first); 95 | double cost = action_successor.successor_state_vars_costs_.back().second; 96 | 97 | if (!successor_state_ptr->IsVisited()) 98 | { 99 | double new_g_val = state_ptr->GetGValue() + cost; 100 | 101 | if (successor_state_ptr->GetGValue() > new_g_val) 102 | { 103 | 104 | double h_val = successor_state_ptr->GetHValue(); 105 | if (h_val == -1) 106 | { 107 | h_val = computeHeuristic(successor_state_ptr); 108 | successor_state_ptr->SetHValue(h_val); 109 | } 110 | 111 | if (h_val != DINF) 112 | { 113 | h_val_min_ = h_val < h_val_min_ ? h_val : h_val_min_; 114 | successor_state_ptr->SetGValue(new_g_val); 115 | successor_state_ptr->SetFValue(new_g_val + heuristic_w_*h_val); 116 | 117 | auto edge_ptr = new Edge(state_ptr, successor_state_ptr, action_ptr); 118 | edge_ptr->SetCost(cost); 119 | edge_map_.insert(make_pair(getEdgeKey(edge_ptr), edge_ptr)); 120 | 121 | successor_state_ptr->SetIncomingEdgePtr(edge_ptr); 122 | 123 | if (state_open_list_.contains(successor_state_ptr)) 124 | { 125 | state_open_list_.decrease(successor_state_ptr); 126 | } 127 | else 128 | { 129 | state_open_list_.push(successor_state_ptr); 130 | } 131 | 132 | } 133 | 134 | } 135 | } 136 | } 137 | } 138 | 139 | void WastarPlanner::exit() 140 | { 141 | // Clear open list 142 | while (!state_open_list_.empty()) 143 | { 144 | state_open_list_.pop(); 145 | } 146 | 147 | Planner::exit(); 148 | } 149 | --------------------------------------------------------------------------------