├── .gitignore ├── CMakeLists.txt ├── README.md ├── include └── epic │ ├── epic_nav_core_plugin.h │ ├── epic_navigation_node.h │ ├── epic_navigation_node_constants.h │ ├── epic_navigation_node_harmonic.h │ ├── epic_navigation_node_harmonic_rviz.h │ └── epic_navigation_node_ompl.h ├── launch ├── epic_nav_core_plugin_maze.launch ├── epic_nav_core_plugin_umass.launch ├── epic_navigation_node_maze.launch ├── epic_navigation_node_umass.launch └── epic_rviz_nav.launch ├── libepic ├── Makefile ├── include │ └── epic │ │ ├── constants.h │ │ ├── error_codes.h │ │ └── harmonic │ │ ├── harmonic.h │ │ ├── harmonic_cpu.h │ │ ├── harmonic_gpu.h │ │ ├── harmonic_legacy_cpu.h │ │ ├── harmonic_legacy_path_cpu.h │ │ ├── harmonic_model_gpu.h │ │ ├── harmonic_path_cpu.h │ │ ├── harmonic_utilities_cpu.h │ │ └── harmonic_utilities_gpu.h ├── lib │ └── libepic.so ├── python │ ├── __init__.py │ └── epic │ │ ├── __init__.py │ │ ├── epic_harmonic.py │ │ ├── harmonic.py │ │ ├── harmonic_legacy.py │ │ ├── harmonic_legacy_map.py │ │ └── harmonic_map.py ├── src │ └── harmonic │ │ ├── harmonic_cpu.cpp │ │ ├── harmonic_gpu.cu │ │ ├── harmonic_legacy_cpu.cpp │ │ ├── harmonic_legacy_path_cpu.cpp │ │ ├── harmonic_model_gpu.cu │ │ ├── harmonic_path_cpu.cpp │ │ ├── harmonic_utilities_cpu.cpp │ │ └── harmonic_utilities_gpu.cu └── tests │ ├── batch │ ├── batch.py │ ├── c_space.png │ ├── compare_precision.py │ ├── large_maze.png │ ├── large_mine.png │ ├── small_maze.png │ ├── small_mine.png │ ├── umass.png │ └── willow_garage.png │ └── maps │ ├── basic.png │ ├── c_space.png │ ├── maps.py │ ├── maze_1.png │ ├── maze_2.png │ ├── maze_3.png │ ├── maze_4.png │ ├── mine_1.png │ ├── mine_2.png │ ├── trivial.png │ └── umass_lpr.png ├── maps ├── maze.png ├── maze.yaml ├── umass.png └── umass.yaml ├── package.xml ├── plugins └── epic_nav_core_plugin.xml ├── rviz └── default.rviz ├── src ├── epic_nav_core_plugin.cpp ├── epic_navigation_node.cpp ├── epic_navigation_node_harmonic.cpp ├── epic_navigation_node_harmonic_rviz.cpp ├── epic_navigation_node_main.cpp └── epic_navigation_node_ompl.cpp └── srv ├── ComputePath.srv ├── GetCell.srv ├── ModifyGoals.srv ├── ResetFreeCells.srv ├── SetCells.srv └── SetStatus.srv /.gitignore: -------------------------------------------------------------------------------- 1 | __pycache__ 2 | *.swp 3 | obj 4 | *.pyc 5 | *.csv 6 | ve_epic 7 | 8 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(epic) 3 | 4 | # Important: Must be C++11 since I assume in EpicNavigationNode that vector.h can be dereferenced to 5 | # get the array. 6 | set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") 7 | 8 | find_package(catkin REQUIRED COMPONENTS 9 | roscpp 10 | std_msgs 11 | geometry_msgs 12 | nav_msgs 13 | nav_core 14 | costmap_2d 15 | message_generation 16 | ) 17 | 18 | add_service_files( 19 | FILES 20 | SetStatus.srv 21 | ModifyGoals.srv 22 | GetCell.srv 23 | SetCells.srv 24 | ResetFreeCells.srv 25 | ComputePath.srv 26 | ) 27 | 28 | generate_messages( 29 | DEPENDENCIES 30 | std_msgs 31 | geometry_msgs 32 | nav_msgs 33 | ) 34 | 35 | catkin_package( 36 | INCLUDE_DIRS include 37 | # LIBRARIES epic 38 | CATKIN_DEPENDS message_runtime 39 | # DEPENDS ompl #system_lib # STEP 2/5 FOR OMPL SUPPORT 40 | ) 41 | 42 | include_directories( 43 | ${catkin_INCLUDE_DIRS} 44 | include 45 | libepic/include 46 | # ${OMPL_INCLUDE_DIRS} # STEP 3/5 FOR OMPL SUPPORT 47 | ) 48 | 49 | link_directories(libepic/lib) 50 | 51 | add_library(epic_nav_core_plugin 52 | src/epic_nav_core_plugin.cpp 53 | ) 54 | 55 | add_executable(epic_navigation_node 56 | src/epic_navigation_node.cpp 57 | src/epic_navigation_node_harmonic.cpp 58 | src/epic_navigation_node_harmonic_rviz.cpp 59 | # src/epic_navigation_node_ompl.cpp # STEP 4/5 FOR OMPL SUPPORT 60 | src/epic_navigation_node_main.cpp 61 | ) 62 | 63 | add_dependencies(epic_nav_core_plugin epic_generate_messages_cpp) 64 | add_dependencies(epic_navigation_node epic_generate_messages_cpp) 65 | 66 | target_link_libraries(epic_nav_core_plugin 67 | ${catkin_LIBRARIES} 68 | epic 69 | ) 70 | 71 | target_link_libraries(epic_navigation_node 72 | ${catkin_LIBRARIES} 73 | epic 74 | # ${OMPL_LIBRARIES} # STEP 5/5 FOR OMPL SUPPORT 75 | ) 76 | 77 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # epic 2 | Robotic path planning using CUDA-optimized potential fields (harmonic function) in log-space. This library intends to support other mechanisms for this path planning approach, such as a control basis method for high dimensions and Robot Operating System (ROS) support. 3 | 4 | If you use this library, then please cite our IROS 2016 paper: 5 | 6 | Wray, Kyle H. and Ruiken, Dirk and Grupen, Roderic A. and Zilberstein, Shlomo. "Log-Space Harmonic Function Path Planning." Twenty-Ninth International Conference on Intelligent Robots and Systems (IROS), Daejeon, Korea, October 2016. 7 | 8 | -------------------------------------------------------------------------------- /include/epic/epic_nav_core_plugin.h: -------------------------------------------------------------------------------- 1 | /** 2 | * The MIT License (MIT) 3 | * 4 | * Copyright (c) 2015 Kyle Hollins Wray, University of Massachusetts 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | * this software and associated documentation files (the "Software"), to deal in 8 | * the Software without restriction, including without limitation the rights to 9 | * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | * the Software, and to permit persons to whom the Software is furnished to do so, 11 | * subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in all 14 | * copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | 25 | #ifndef EPIC_NAV_CORE_PLUGIN_H 26 | #define EPIC_NAV_CORE_PLUGIN_H 27 | 28 | 29 | #include 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | #include 37 | 38 | #include 39 | 40 | using std::string; 41 | 42 | namespace epic { 43 | 44 | class EpicNavCorePlugin : public nav_core::BaseGlobalPlanner { 45 | public: 46 | /** 47 | * The default constructor for the EpicNavCorePlugin. 48 | */ 49 | EpicNavCorePlugin(); 50 | 51 | /** 52 | * The constructor for the EpicNavCorePlugin used by the plugin. 53 | * @param name The name of the planner. 54 | * @param costmap_ros The cost map to solve. 55 | */ 56 | EpicNavCorePlugin(std::string name, costmap_2d::Costmap2DROS *costmap_ros); 57 | 58 | /** 59 | * The deconstructor for the EpicNavCorePlugin used by the plugin. 60 | */ 61 | virtual ~EpicNavCorePlugin(); 62 | 63 | /** 64 | * Initialize the plugin class EpicNavCorePlugin. Overloads from nav_core::BaseGlobalPlanner. 65 | * @param name The name of the planner. 66 | * @param costmap_ros The cost map to solve. 67 | */ 68 | void initialize(std::string name, costmap_2d::Costmap2DROS *costmap_ros); 69 | 70 | /** 71 | * Uninitialize the plugin class EpicNavCorePlugin. 72 | */ 73 | void uninitialize(); 74 | 75 | /** 76 | * Make the plan for plugin EpicNavCorePlugin. Overloads from nav_core::BaseGlobalPlanner. 77 | * @param start The starting (x, y) location in the cost map. 78 | * @param goal The goal (x, y) location in the cost map. 79 | * @param plan The resulting plan as a sequence of (x, y) waypoints. 80 | * @return True if a plan was found, false otherwise. 81 | */ 82 | bool makePlan(const geometry_msgs::PoseStamped &start, 83 | const geometry_msgs::PoseStamped &goal, 84 | std::vector &plan); 85 | 86 | /** 87 | * Set the goal given an (x, y) location on the cell map. 88 | * @param x_goal The x location. 89 | * @param y_goal The y location. 90 | */ 91 | void setGoal(unsigned int x_goal, unsigned int y_goal); 92 | 93 | private: 94 | /** 95 | * Set harmonic cell potential values from the cost map, assuming both have been defined. 96 | */ 97 | void setCellsFromCostmap(); 98 | 99 | /** 100 | * Set the boundaries as obstacles, provided the harmonic function has been defined. 101 | */ 102 | void setBoundariesAsObstacles(); 103 | 104 | /** 105 | * Convert the float-index map coordinate to the world coordinate. 106 | * @param mx The float-index map x coordinate. 107 | * @param my The float-index map y coordinate. 108 | * @param wx The resultant world x coordinate. This will be modified. 109 | * @param wy The resultant world y coordinate. This will be modified. 110 | */ 111 | void mapToWorld(float mx, float my, float &wx, float &wy) const; 112 | 113 | /** 114 | * Covert the world coordinate to the float-index map coordinate. 115 | * @param wx The world x coordinate. 116 | * @param wy The world y coordinate. 117 | * @param mx The resultant float-index map x coordinate. This will be modified. 118 | * @param my The resultant float-index map y coordinate. This will be modified. 119 | * @return True if the world coordinate is inside the map, false otherwise. 120 | */ 121 | bool worldToMap(float wx, float wy, float &mx, float &my) const; 122 | 123 | /** 124 | * Publish the plan as a path for use in path followers and visualization. 125 | * @param path The path to be published. 126 | */ 127 | void publishPlan(const std::vector &path); 128 | 129 | // The Harmonic struct which holds the potential values for the 'epic' library. 130 | Harmonic harmonic; 131 | 132 | // The costmap object. 133 | costmap_2d::Costmap2D *costmap; 134 | 135 | // We will publish the plan so that the stupid nav_core doesn't truncate it. 136 | ros::Publisher pub_plan; 137 | 138 | // Also publish the potential. 139 | ros::Publisher pub_potential; 140 | 141 | // If this object has been properly initialized or not. 142 | bool initialized; 143 | 144 | }; 145 | 146 | }; 147 | 148 | 149 | #endif // EPIC_NAV_CORE_PLUGIN_H 150 | 151 | -------------------------------------------------------------------------------- /include/epic/epic_navigation_node.h: -------------------------------------------------------------------------------- 1 | /** 2 | * The MIT License (MIT) 3 | * 4 | * Copyright (c) 2016 Kyle Hollins Wray, University of Massachusetts 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | * this software and associated documentation files (the "Software"), to deal in 8 | * the Software without restriction, including without limitation the rights to 9 | * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | * the Software, and to permit persons to whom the Software is furnished to do so, 11 | * subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in all 14 | * copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | 25 | #ifndef EPIC_NAVIGATION_NODE_H 26 | #define EPIC_NAVIGATION_NODE_H 27 | 28 | 29 | #include 30 | 31 | namespace epic { 32 | 33 | class EpicNavigationNode { 34 | public: 35 | /** 36 | * The default constructor for the EpicNavigationNode class. 37 | * @param nh The node handle from main. 38 | */ 39 | EpicNavigationNode(ros::NodeHandle &nh); 40 | 41 | /** 42 | * The deconstructor for the EpicNavigationNode class. 43 | */ 44 | virtual ~EpicNavigationNode(); 45 | 46 | /** 47 | * Initialize the services, messages, and algorithm variables. This must be implemented. 48 | * @return True if successful in registering, subscribing, etc.; false otherwise. 49 | */ 50 | virtual bool initialize() = 0; 51 | 52 | /** 53 | * Update the harmonic function one step. This must be implemented. 54 | * @param num_steps The number of steps to do. 55 | */ 56 | virtual void update(unsigned int num_steps) = 0; 57 | 58 | protected: 59 | // A private node handle; usually a reference to the one created in the node's "main" function. 60 | ros::NodeHandle private_node_handle; 61 | 62 | }; 63 | 64 | }; 65 | 66 | 67 | #endif // EPIC_NAVIGATION_NODE_H 68 | 69 | -------------------------------------------------------------------------------- /include/epic/epic_navigation_node_constants.h: -------------------------------------------------------------------------------- 1 | /** 2 | * The MIT License (MIT) 3 | * 4 | * Copyright (c) 2016 Kyle Hollins Wray, University of Massachusetts 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | * this software and associated documentation files (the "Software"), to deal in 8 | * the Software without restriction, including without limitation the rights to 9 | * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | * the Software, and to permit persons to whom the Software is furnished to do so, 11 | * subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in all 14 | * copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | 25 | #ifndef EPIC_NAVIGATION_NODE_CONSTANTS_H 26 | #define EPIC_NAVIGATION_NODE_CONSTANTS_H 27 | 28 | namespace epic { 29 | 30 | #define EPIC_OCCUPANCY_GRID_MAX 100 31 | #define EPIC_OCCUPANCY_GRID_OBSTACLE_THRESHOLD 50 32 | #define EPIC_OCCUPANCY_GRID_MIN 0 33 | #define EPIC_OCCUPANCY_GRID_UNKNOWN -1 34 | #define EPIC_OCCUPANCY_GRID_NO_CHANGE -2 35 | 36 | }; 37 | 38 | #endif // EPIC_NAVIGATION_NODE_CONSTANTS_H 39 | 40 | -------------------------------------------------------------------------------- /include/epic/epic_navigation_node_harmonic.h: -------------------------------------------------------------------------------- 1 | /** 2 | * The MIT License (MIT) 3 | * 4 | * Copyright (c) 2015 Kyle Hollins Wray, University of Massachusetts 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | * this software and associated documentation files (the "Software"), to deal in 8 | * the Software without restriction, including without limitation the rights to 9 | * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | * the Software, and to permit persons to whom the Software is furnished to do so, 11 | * subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in all 14 | * copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | 25 | #ifndef EPIC_NAVIGATION_NODE_HARMONIC_H 26 | #define EPIC_NAVIGATION_NODE_HARMONIC_H 27 | 28 | 29 | #include 30 | 31 | #include 32 | #include 33 | #include 34 | 35 | #include 36 | 37 | #include 38 | 39 | // Generated by having the .srv files defined. 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | namespace epic { 48 | 49 | class EpicNavigationNodeHarmonic : public EpicNavigationNode { 50 | public: 51 | /** 52 | * The default constructor for the EpicNavigationNodeHarmonic. 53 | * @param nh The node handle from main. 54 | */ 55 | EpicNavigationNodeHarmonic(ros::NodeHandle &nh); 56 | 57 | /** 58 | * The deconstructor for the EpicNavigationNodeHarmonic. 59 | */ 60 | virtual ~EpicNavigationNodeHarmonic(); 61 | 62 | /** 63 | * Initialize the services, messages, and algorithm variables. Implemented for EpicNavigationNode. 64 | * @return True if successful in registering, subscribing, etc.; false otherwise. 65 | */ 66 | bool initialize(); 67 | 68 | /** 69 | * Update the harmonic function one step. Implemented for EpicNavigationNode. 70 | * @param num_steps The number of steps to do. 71 | */ 72 | void update(unsigned int num_steps); 73 | 74 | protected: 75 | /** 76 | * Initialize the algorithm. 77 | * @param w The new width. 78 | * @param h The new height. 79 | */ 80 | void initAlg(unsigned int w, unsigned int h); 81 | 82 | /** 83 | * Uninitialize the algorithm. 84 | */ 85 | void uninitAlg(); 86 | 87 | /** 88 | * Set the boundaries as obstacles for the occupancy grid. 89 | */ 90 | void setBoundariesAsObstacles(); 91 | 92 | /** 93 | * Convert a "float pixel" map coordinate to a world coordinate. 94 | * @param mx The "float pixel" map x coordinate. 95 | * @param my The "float pixel" map y coordinate. 96 | * @param wx The world x coordinate. This will be modified. 97 | * @param wy The world y coordinate. This will be modified. 98 | */ 99 | void mapToWorld(float mx, float my, float &wx, float &wy) const; 100 | 101 | /** 102 | * Convert a "float pixel" map coordinate to a world coordinate. 103 | * @param wx The world x coordinate. 104 | * @param wy The world y coordinate. 105 | * @param mx The "float pixel" map x coordinate. This will be modified. 106 | * @param my The "float pixel" map y coordinate. This will be modified. 107 | */ 108 | bool worldToMap(float wx, float wy, float &mx, float &my) const; 109 | 110 | /** 111 | * Check if a cell is an obstacle or not. 112 | * @param x The x cell index. 113 | * @param y The y cell index. 114 | * @return True if it is an obstacle, false otherwise. 115 | */ 116 | bool isCellObstacle(unsigned int x, unsigned int y); 117 | 118 | /** 119 | * Check if a cell is a goal or not. 120 | * @param x The x cell index. 121 | * @param y The y cell index. 122 | * @return True if it is a goal, false otherwise. 123 | */ 124 | bool isCellGoal(unsigned int x, unsigned int y); 125 | 126 | /** 127 | * Set the cells based on the values and types provided. This handles CPU vs. GPU assignment. 128 | * @param v The n locations (2-n array) of each cell as indexes. 129 | * @param types The n types of each cell to set. 130 | * @return True if successful, false otherwise. 131 | */ 132 | bool setCells(std::vector &v, std::vector &types); 133 | 134 | /** 135 | * Handler for receiving OccupancyGrid messages (see defines above; values differ here). 136 | * @param msg The OccupancyGrid message. 137 | */ 138 | void subOccupancyGrid(const nav_msgs::OccupancyGrid::ConstPtr &msg); 139 | 140 | /** 141 | * Handler for receiving set status service request. 142 | * @param req The SetStatus service request containing the goal location(s) to add. 143 | * @param res The SetStatus service response containing if it was successful or not. 144 | * @return Returns true if successful, false otherwise. 145 | */ 146 | bool srvSetStatus(epic::SetStatus::Request &req, epic::SetStatus::Response &res); 147 | 148 | /** 149 | * Handler for receiving add goal(s) service request. Assumption: These are not initially obstacles. 150 | * @param req The AddGoal service request containing the goal location(s) to add. 151 | * @param res The AddGoal service response containing if it was successful or not. 152 | * @return Returns true if successful, false otherwise. 153 | */ 154 | bool srvAddGoals(epic::ModifyGoals::Request &req, epic::ModifyGoals::Response &res); 155 | 156 | /** 157 | * Handler for receiving remove goal(s) messages. Assumption: Removed goals become free space. 158 | * @param req The RemoveGoal service request containing the goal location(s) to remove. 159 | * @param res The RemoveGoal service response containing if it was successful or not. 160 | * @return Returns true if successful, false otherwise. 161 | */ 162 | bool srvRemoveGoals(epic::ModifyGoals::Request &req, epic::ModifyGoals::Response &res); 163 | 164 | /** 165 | * Handler for receiving get cell messages. This allows us to get the log hitting probability of a cell. 166 | * @param req The GetCell service request containing the cell value. 167 | * @param res The GetCell service response containing if it was successful or not. 168 | * @return Returns true if successful, false otherwise. 169 | */ 170 | bool srvGetCell(epic::GetCell::Request &req, epic::GetCell::Response &res); 171 | 172 | /** 173 | * Handler for receiving set cells messages. This allows for quick updates to the occupancy grid. 174 | * @param req The SetCells service request containing the cells to set and resulting desired type. 175 | * @param res The SetCells service response containing if it was successful or not. 176 | * @return Returns true if successful, false otherwise. 177 | */ 178 | bool srvSetCells(epic::SetCells::Request &req, epic::SetCells::Response &res); 179 | 180 | /** 181 | * Handler for receiving reset messages. This completely resets the map (e.g., for drastic goal changes). 182 | * @param req The ResetFreeCells service request. 183 | * @param res The ResetFreeCells service response containing if it was successful or not. 184 | * @return Returns true if successful, false otherwise. 185 | */ 186 | bool srvResetFreeCells(epic::ResetFreeCells::Request &req, epic::ResetFreeCells::Response &res); 187 | 188 | /** 189 | * Handler for service requests for generating Path messages. 190 | * @param req The ComputePath service request containing the start location, max length, precision, etc. 191 | * @param res The ComputePath service response containing the resulting path. 192 | * @return Returns true if successful, false otherwise. 193 | */ 194 | bool srvComputePath(epic::ComputePath::Request &req, epic::ComputePath::Response &res); 195 | 196 | // The subscriber for the OccupancyGrid message. 197 | ros::Subscriber sub_occupancy_grid; 198 | 199 | // The service for SetStatus. 200 | ros::ServiceServer srv_set_status; 201 | 202 | // The service for AddGoal. 203 | ros::ServiceServer srv_add_goals; 204 | 205 | // The service for RemoveGoal. 206 | ros::ServiceServer srv_remove_goals; 207 | 208 | // The service for GetCell. 209 | ros::ServiceServer srv_get_cell; 210 | 211 | // The service for SetCells. 212 | ros::ServiceServer srv_set_cells; 213 | 214 | // The service for ResetFreeCells. 215 | ros::ServiceServer srv_reset_free_cells; 216 | 217 | // The service for ComputePath. 218 | ros::ServiceServer srv_compute_path; 219 | 220 | // Current width of the map. 221 | unsigned int width; 222 | 223 | // Current height of the map. 224 | unsigned int height; 225 | 226 | // Current resolution of the map. 227 | float resolution; 228 | 229 | // Current x origin offset of the map. 230 | float x_origin; 231 | 232 | // Current y origin offset of the map. 233 | float y_origin; 234 | 235 | // The Harmonic object. 236 | Harmonic harmonic; 237 | 238 | // The number of GPU threads for the Harmonic function. 239 | unsigned int num_gpu_threads; 240 | 241 | // If this is paused (i.e., basically do not run update), or not. 242 | bool paused; 243 | 244 | // If this is GPU-capable, or not. 245 | bool gpu; 246 | 247 | // If this class' services and subscriptions have been properly initialized or not. 248 | bool init_msgs; 249 | 250 | // If this class' algorithm variables have been properly initialized or not. 251 | bool init_alg; 252 | 253 | }; 254 | 255 | }; 256 | 257 | 258 | #endif // EPIC_NAVIGATION_NODE_HARMONIC_H 259 | 260 | -------------------------------------------------------------------------------- /include/epic/epic_navigation_node_harmonic_rviz.h: -------------------------------------------------------------------------------- 1 | /** 2 | * The MIT License (MIT) 3 | * 4 | * Copyright (c) 2015 Kyle Hollins Wray, University of Massachusetts 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | * this software and associated documentation files (the "Software"), to deal in 8 | * the Software without restriction, including without limitation the rights to 9 | * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | * the Software, and to permit persons to whom the Software is furnished to do so, 11 | * subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in all 14 | * copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | 25 | #ifndef EPIC_NAVIGATION_NODE_HARMONIC_RVIZ_H 26 | #define EPIC_NAVIGATION_NODE_HARMONIC_RVIZ_H 27 | 28 | 29 | #include 30 | 31 | #include 32 | #include 33 | #include 34 | 35 | #include 36 | 37 | #include 38 | 39 | // Generated by having the .srv files defined. 40 | #include 41 | #include 42 | #include 43 | 44 | namespace epic { 45 | 46 | class EpicNavigationNodeHarmonicRviz : public EpicNavigationNodeHarmonic { 47 | public: 48 | /** 49 | * The default constructor for the EpicNavigationNodeHarmonicRviz. 50 | * @param nh The node handle from main. 51 | */ 52 | EpicNavigationNodeHarmonicRviz(ros::NodeHandle &nh); 53 | 54 | /** 55 | * The deconstructor for the EpicNavigationNodeHarmonicRviz. 56 | */ 57 | virtual ~EpicNavigationNodeHarmonicRviz(); 58 | 59 | /** 60 | * Initialize the services, messages, and algorithm variables. Overridden from EpicNavigationNodeHarmonic. 61 | * @return True if successful in registering, subscribing, etc.; false otherwise. 62 | */ 63 | bool initialize(); 64 | 65 | protected: 66 | /** 67 | * Handler for receiving PoseWithConvarianceStamped messages for the "/initialpose" topic, 68 | * published by rviz by clicking the "2D Nav Goal" button. 69 | * @param msg The PoseWithConvarianceStamped message. 70 | */ 71 | void subMapPoseEstimate(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg); 72 | 73 | /** 74 | * Handler for receiving PostStamped messages for the "/move_base_simple/goal" topic, published 75 | * by rviz by clicking the "2D Nav Goal" button. 76 | * @param msg The PoseStamped message. 77 | */ 78 | void subMapNavGoal(const geometry_msgs::PoseStamped::ConstPtr &msg); 79 | 80 | // The subscriber for the "/initialpose" topic, published by rviz 81 | // by clicking the "2D Nav Goal" button. 82 | ros::Subscriber sub_map_pose_estimate; 83 | 84 | // The subscriber for the "/move_base_simple/goal" topic, published by rviz 85 | // by clicking the "2D Nav Goal" button. 86 | ros::Subscriber sub_map_nav_goal; 87 | 88 | // The publisher for the "path" topic, published to rviz. 89 | ros::Publisher pub_map_path; 90 | 91 | // If a goal has ever been added, however, only with the "subMapGoal" function. 92 | bool goal_added; 93 | 94 | // The last goal location of the robot. Used by "subMapGoal" function. 95 | // Assigned in rviz via "2D Nav Goal" button. 96 | geometry_msgs::PoseStamped last_goal; 97 | 98 | // The current pose of the robot. Used by "subMapGoal" function. 99 | // Assigned in rviz via "2D Pose Estimate" button. 100 | geometry_msgs::PoseStamped current_pose; 101 | 102 | }; 103 | 104 | }; 105 | 106 | 107 | #endif // EPIC_NAVIGATION_NODE_HARMONIC_RVIZ_H 108 | 109 | -------------------------------------------------------------------------------- /include/epic/epic_navigation_node_ompl.h: -------------------------------------------------------------------------------- 1 | /** 2 | * The MIT License (MIT) 3 | * 4 | * Copyright (c) 2015 Kyle Hollins Wray, University of Massachusetts 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | * this software and associated documentation files (the "Software"), to deal in 8 | * the Software without restriction, including without limitation the rights to 9 | * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | * the Software, and to permit persons to whom the Software is furnished to do so, 11 | * subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in all 14 | * copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | 25 | #ifndef EPIC_NAVIGATION_NODE_OMPL_H 26 | #define EPIC_NAVIGATION_NODE_OMPL_H 27 | 28 | 29 | #include 30 | 31 | #include 32 | #include 33 | #include 34 | 35 | #include 36 | #include 37 | 38 | #include 39 | 40 | // Generated by having the .srv files defined. 41 | #include 42 | #include 43 | #include 44 | 45 | namespace epic { 46 | 47 | #define EPIC_ALGORITHM_RRT_CONNECT 0 48 | #define EPIC_ALGORITHM_RRT_STAR 1 49 | #define EPIC_ALGORITHM_LAZY_RRT 2 50 | #define EPIC_ALGORITHM_LAZY_PRM 3 51 | #define EPIC_ALGORITHM_PRM_STAR 4 52 | #define EPIC_ALGORITHM_LAZY_PRM_STAR 5 53 | #define NUM_EPIC_ALGORITHMS 6 54 | 55 | class EpicNavigationNodeOMPL { 56 | public: 57 | /** 58 | * The constructor for the EpicNavigationNodeOMPL, enabling the selection of the planner. 59 | * @param nh The node handle from main. 60 | * @param alg The algorithm to use (see defines above). 61 | */ 62 | EpicNavigationNodeOMPL(ros::NodeHandle &nh, unsigned int alg); 63 | 64 | /** 65 | * The deconstructor for the EpicNavigationNodeOMPL. 66 | */ 67 | virtual ~EpicNavigationNodeOMPL(); 68 | 69 | /** 70 | * Initialize the services and messages. 71 | * @return True if successful in registering and subscribing, false otherwise. 72 | */ 73 | bool initMsgs(); 74 | 75 | /** 76 | * Update the harmonic function or other planner one step. 77 | * @param t The time to spend updating the planner. 78 | */ 79 | void update(float t); 80 | 81 | private: 82 | /** 83 | * Initialize the algorithm. 84 | */ 85 | void initAlg(); 86 | 87 | /** 88 | * Uninitialize the algorithm. 89 | */ 90 | void uninitAlg(); 91 | 92 | /** 93 | * Set the boundaries as obstacles for the occupancy grid. 94 | */ 95 | void setBoundariesAsObstacles(); 96 | 97 | /** 98 | * Convert a "float pixel" map coordinate to a world coordinate. 99 | * @param mx The "float pixel" map x coordinate. 100 | * @param my The "float pixel" map y coordinate. 101 | * @param wx The world x coordinate. This will be modified. 102 | * @param wy The world y coordinate. This will be modified. 103 | */ 104 | void mapToWorld(float mx, float my, float &wx, float &wy) const; 105 | 106 | /** 107 | * Convert a "float pixel" map coordinate to a world coordinate. 108 | * @param wx The world x coordinate. 109 | * @param wy The world y coordinate. 110 | * @param mx The "float pixel" map x coordinate. This will be modified. 111 | * @param my The "float pixel" map y coordinate. This will be modified. 112 | */ 113 | bool worldToMap(float wx, float wy, float &mx, float &my) const; 114 | 115 | /** 116 | * Check if a cell is an obstacle or not. 117 | * @param x The x cell index. 118 | * @param y The y cell index. 119 | * @return True if it is an obstacle, false otherwise. 120 | */ 121 | bool isCellObstacle(unsigned int x, unsigned int y); 122 | 123 | /** 124 | * Check if a cell is a goal or not. 125 | * @param x The x cell index. 126 | * @param y The y cell index. 127 | * @return True if it is a goal, false otherwise. 128 | */ 129 | bool isCellGoal(unsigned int x, unsigned int y); 130 | 131 | /** 132 | * Handler for receiving OccupancyGrid messages (see defines above; values differ here). 133 | * @param msg The OccupancyGrid message. 134 | */ 135 | void subOccupancyGrid(const nav_msgs::OccupancyGrid::ConstPtr &msg); 136 | 137 | /** 138 | * Handler for receiving add goal(s) service request. Assumption: These are not initially obstacles. 139 | * @param req The AddGoal service request containing the goal location(s) to add. 140 | * @param res The AddGoal service response containing if it was successful or not. 141 | * @return Returns true if successful, false otherwise. 142 | */ 143 | bool srvAddGoals(epic::ModifyGoals::Request &req, epic::ModifyGoals::Response &res); 144 | 145 | /** 146 | * Handler for receiving remove goal(s) messages. Assumption: Removed goals become free space. 147 | * @param req The RemoveGoal service request containing the goal location(s) to remove. 148 | * @param res The RemoveGoal service response containing if it was successful or not. 149 | * @return Returns true if successful, false otherwise. 150 | */ 151 | bool srvRemoveGoals(epic::ModifyGoals::Request &req, epic::ModifyGoals::Response &res); 152 | 153 | /** 154 | * Handler for receiving set cells messages. This allows for quick updates to the occupancy grid. 155 | * @param req The RemoveGoal service request containing the cells to set and resulting desired type. 156 | * @param res The RemoveGoal service response containing if it was successful or not. 157 | * @return Returns true if successful, false otherwise. 158 | */ 159 | bool srvSetCells(epic::SetCells::Request &req, epic::SetCells::Response &res); 160 | 161 | /** 162 | * Handler for service requests for generating Path messages. 163 | * @param req The ComputePath service request containing the start location, max length, precision, etc. 164 | * @param res The ComputePath service response containing the resulting path. 165 | * @return Returns true if successful, false otherwise. 166 | */ 167 | bool srvComputePath(epic::ComputePath::Request &req, epic::ComputePath::Response &res); 168 | 169 | /** 170 | * Handler for receiving PoseWithConvarianceStamped messages for the "/initialpose" topic, 171 | * published by rviz by clicking the "2D Nav Goal" button. 172 | * @param msg The PoseWithConvarianceStamped message. 173 | */ 174 | void subMapPoseEstimate(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg); 175 | 176 | /** 177 | * Handler for receiving PostStamped messages for the "/move_base_simple/goal" topic, published 178 | * by rviz by clicking the "2D Nav Goal" button. 179 | * @param msg The PoseStamped message. 180 | */ 181 | void subMapNavGoal(const geometry_msgs::PoseStamped::ConstPtr &msg); 182 | 183 | // A private node handle; usually a reference to the one created in the node's "main" function. 184 | ros::NodeHandle private_node_handle; 185 | 186 | // The subscriber for the OccupancyGrid message. 187 | ros::Subscriber sub_occupancy_grid; 188 | 189 | // The subscriber for the "/initialpose" topic, published by rviz 190 | // by clicking the "2D Nav Goal" button. 191 | ros::Subscriber sub_map_pose_estimate; 192 | 193 | // The subscriber for the "/move_base_simple/goal" topic, published by rviz 194 | // by clicking the "2D Nav Goal" button. 195 | ros::Subscriber sub_map_nav_goal; 196 | 197 | // The publisher for the " 198 | ros::Publisher pub_map_path; 199 | 200 | // If a goal has ever been added, however, only with the "subMapGoal" function. 201 | bool goal_added; 202 | 203 | // The last goal location of the robot. Used by "subMapGoal" function. 204 | // Assigned in rviz via "2D Nav Goal" button. 205 | geometry_msgs::PoseStamped last_goal; 206 | 207 | // The current pose of the robot. Used by "subMapGoal" function. 208 | // Assigned in rviz via "2D Pose Estimate" button. 209 | geometry_msgs::PoseStamped current_pose; 210 | 211 | // The service for AddGoal. 212 | ros::ServiceServer srv_add_goals; 213 | 214 | // The service for RemoveGoal. 215 | ros::ServiceServer srv_remove_goals; 216 | 217 | // The service for SetCells. 218 | ros::ServiceServer srv_set_cells; 219 | 220 | // The service for ComputePath. 221 | ros::ServiceServer srv_compute_path; 222 | 223 | // Current width of the map. 224 | unsigned int width; 225 | 226 | // Current height of the map. 227 | unsigned int height; 228 | 229 | // Current occupancy grid, following EPIC_CELL_TYPE conventions. 230 | int *occupancy_grid; 231 | 232 | // Current resolution of the map. 233 | float resolution; 234 | 235 | // Current x origin offset of the map. 236 | float x_origin; 237 | 238 | // Current y origin offset of the map. 239 | float y_origin; 240 | 241 | // The algorithm to use. 242 | unsigned int algorithm; 243 | 244 | // The planning algorithm for any algorithm. 245 | ompl::base::PlannerPtr ompl_planner; 246 | 247 | // The planner's status for any algorithm. 248 | ompl::base::PlannerStatus ompl_planner_status; 249 | 250 | // The start location for any algorithhm. 251 | std::pair start_location; 252 | 253 | // If the start location is assigned for any algorithm. 254 | bool start_assigned; 255 | 256 | // The goal location for any algorithm. 257 | std::pair goal_location; 258 | 259 | // If the goal location is assigned for any algorithm. 260 | bool goal_assigned; 261 | 262 | // If this class' services and subscriptions have been properly initialized or not. 263 | bool init_msgs; 264 | 265 | // If this class' algorithm variables have been properly initialized or not. 266 | bool init_alg; 267 | 268 | }; 269 | 270 | }; 271 | 272 | 273 | #endif // EPIC_NAVIGATION_NODE_OMPL_H 274 | 275 | -------------------------------------------------------------------------------- /launch/epic_nav_core_plugin_maze.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /launch/epic_nav_core_plugin_umass.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /launch/epic_navigation_node_maze.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /launch/epic_navigation_node_umass.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /launch/epic_rviz_nav.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /libepic/Makefile: -------------------------------------------------------------------------------- 1 | COMMAND = nvcc 2 | FLAGS = -std=c++11 -shared -O3 -use_fast_math -Xcompiler -fPIC -Iinclude 3 | 4 | all: epic 5 | 6 | epic: harmonic_gpu.o harmonic_cpu.o 7 | mkdir -p lib 8 | $(COMMAND) $(FLAGS) obj/*.o -o libepic.so 9 | mv libepic.so lib 10 | 11 | harmonic_gpu.o: src/harmonic/*.cu 12 | mkdir -p obj 13 | $(COMMAND) $(FLAGS) -c src/harmonic/*.cu 14 | mv *.o obj 15 | 16 | harmonic_cpu.o: src/harmonic/*.cpp 17 | mkdir -p obj 18 | $(COMMAND) $(FLAGS) -c src/harmonic/*.cpp 19 | mv *.o obj 20 | 21 | clean: 22 | rm -rf lib 23 | rm -rf obj 24 | 25 | -------------------------------------------------------------------------------- /libepic/include/epic/constants.h: -------------------------------------------------------------------------------- 1 | /** 2 | * The MIT License (MIT) 3 | * 4 | * Copyright (c) 2014 Kyle Hollins Wray, University of Massachusetts 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | * this software and associated documentation files (the "Software"), to deal in 8 | * the Software without restriction, including without limitation the rights to 9 | * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | * the Software, and to permit persons to whom the Software is furnished to do so, 11 | * subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in all 14 | * copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | 25 | #ifndef CONSTANTS_H 26 | #define CONSTANTS_H 27 | 28 | 29 | namespace epic { 30 | 31 | // This is determined by hardware, so what is below is a 'safe' guess. If this is 32 | // off, the program might return 'nan' or 'inf'. 33 | //#define FLT_MAX 1e+35 34 | #define EPIC_FLT_MAX 1e+300 35 | #define EPIC_FLT_MIN (-EPIC_FLT_MAX) 36 | 37 | #define EPIC_CELL_TYPE_GOAL 0 38 | #define EPIC_CELL_TYPE_OBSTACLE 1 39 | #define EPIC_CELL_TYPE_FREE 2 40 | 41 | #define EPIC_LOG_SPACE_GOAL 0.0 42 | #define EPIC_LOG_SPACE_OBSTACLE -1e6 43 | #define EPIC_LOG_SPACE_FREE -1e6 44 | 45 | }; 46 | 47 | 48 | #endif // CONSTANTS_H 49 | 50 | -------------------------------------------------------------------------------- /libepic/include/epic/error_codes.h: -------------------------------------------------------------------------------- 1 | /** 2 | * The MIT License (MIT) 3 | * 4 | * Copyright (c) 2015 Kyle Hollins Wray, University of Massachusetts 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | * this software and associated documentation files (the "Software"), to deal in 8 | * the Software without restriction, including without limitation the rights to 9 | * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | * the Software, and to permit persons to whom the Software is furnished to do so, 11 | * subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in all 14 | * copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | 25 | #ifndef ERROR_CODES_H 26 | #define ERROR_CODES_H 27 | 28 | 29 | namespace epic { 30 | 31 | #define EPIC_SUCCESS 0 32 | #define EPIC_SUCCESS_AND_CONVERGED 1 33 | 34 | #define EPIC_ERROR_INVALID_DATA 2 35 | #define EPIC_ERROR_INVALID_CUDA_PARAM 3 36 | #define EPIC_ERROR_DEVICE_MALLOC 4 37 | #define EPIC_ERROR_MEMCPY_TO_DEVICE 5 38 | #define EPIC_ERROR_MEMCPY_TO_HOST 6 39 | #define EPIC_ERROR_DEVICE_FREE 7 40 | #define EPIC_ERROR_KERNEL_EXECUTION 8 41 | #define EPIC_ERROR_DEVICE_SYNCHRONIZE 9 42 | 43 | #define EPIC_ERROR_INVALID_LOCATION 10 44 | #define EPIC_ERROR_INVALID_CELL_TYPE 11 45 | #define EPIC_ERROR_INVALID_GRADIENT 12 46 | #define EPIC_ERROR_INVALID_PATH 13 47 | 48 | }; 49 | 50 | 51 | #endif // ERROR_CODES_H 52 | 53 | -------------------------------------------------------------------------------- /libepic/include/epic/harmonic/harmonic.h: -------------------------------------------------------------------------------- 1 | /** 2 | * The MIT License (MIT) 3 | * 4 | * Copyright (c) 2015 Kyle Hollins Wray, University of Massachusetts 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | * this software and associated documentation files (the "Software"), to deal in 8 | * the Software without restriction, including without limitation the rights to 9 | * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | * the Software, and to permit persons to whom the Software is furnished to do so, 11 | * subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in all 14 | * copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | 25 | #ifndef HARMONIC_H 26 | #define HARMONIC_H 27 | 28 | 29 | namespace epic { 30 | 31 | /** 32 | * A structure for a Harmonic object within the inertia library. 33 | * @param n The number of dimensions. 34 | * @param m The size of each dimension. 35 | * @param u The log values; assumes boarder values are locked. 36 | * @param locked The locked cells in the grid; assumes boarder 37 | * values are locked. 38 | * @param epsilon The convergence criterion value. 39 | * @param d_m Device-side pointer of m. 40 | * @param d_u Device-side pointer of u. 41 | * @param d_locked Device-side pointer of locked. 42 | * @param d_delta The delta values (size of numBlocks). 43 | */ 44 | typedef struct Harmonic { 45 | // Core Variables (User-Defined). 46 | unsigned int n; 47 | unsigned int *m; 48 | 49 | float *u; 50 | unsigned int *locked; 51 | 52 | float epsilon; 53 | float delta; 54 | unsigned int numIterationsToStaggerCheck; 55 | 56 | // Computation Variables (Utilized by Processes Only). 57 | unsigned int currentIteration; 58 | 59 | unsigned int *d_m; 60 | float *d_u; 61 | unsigned int *d_locked; 62 | 63 | float *d_delta; 64 | } Harmonic; 65 | 66 | }; 67 | 68 | 69 | #endif // HARMONIC_H 70 | 71 | -------------------------------------------------------------------------------- /libepic/include/epic/harmonic/harmonic_cpu.h: -------------------------------------------------------------------------------- 1 | /** 2 | * The MIT License (MIT) 3 | * 4 | * Copyright (c) 2015 Kyle Hollins Wray, University of Massachusetts 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | * this software and associated documentation files (the "Software"), to deal in 8 | * the Software without restriction, including without limitation the rights to 9 | * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | * the Software, and to permit persons to whom the Software is furnished to do so, 11 | * subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in all 14 | * copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | 25 | #ifndef HARMONIC_CPU_H 26 | #define HARMONIC_CPU_H 27 | 28 | 29 | #include "harmonic.h" 30 | 31 | namespace epic { 32 | 33 | /** 34 | * Execute a Gauss-Seidel harmonic function solver until convergence. Uses the CPU. 35 | * Note: There is no 'initialize', 'execute', or 'uninitialize' function for 36 | * the CPU version. This is essentially equivalent to 'execute' for the GPU version. 37 | * @param harmonic The Harmonic object. 38 | * @return Returns zero upon success, non-zero otherwise. 39 | */ 40 | extern "C" int harmonic_complete_cpu(Harmonic *harmonic); 41 | 42 | /** 43 | * Perform a single update step of the Gauss-Seidel CPU implementation. 44 | * @param harmonic The Harmonic object. 45 | * @return Returns zero upon success, non-zero otherwise. 46 | */ 47 | extern "C" int harmonic_update_cpu(Harmonic *harmonic); 48 | 49 | /** 50 | * Perform a single update step of the Gauss-Seidel CPU implementation, updating delta, 51 | * and return convergence or not. 52 | * @param harmonic The Harmonic object. 53 | * @return Returns zero (EPIC_SUCCESS) upon success, EPIC_SUCCESS_AND_CONVERGED on 54 | * convergence, or non-zero otherwise. 55 | */ 56 | extern "C" int harmonic_update_and_check_cpu(Harmonic *harmonic); 57 | 58 | }; 59 | 60 | 61 | #endif // HARMONIC_CPU_H 62 | 63 | -------------------------------------------------------------------------------- /libepic/include/epic/harmonic/harmonic_gpu.h: -------------------------------------------------------------------------------- 1 | /** 2 | * The MIT License (MIT) 3 | * 4 | * Copyright (c) 2015 Kyle Hollins Wray, University of Massachusetts 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | * this software and associated documentation files (the "Software"), to deal in 8 | * the Software without restriction, including without limitation the rights to 9 | * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | * the Software, and to permit persons to whom the Software is furnished to do so, 11 | * subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in all 14 | * copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | 25 | #ifndef HARMONIC_GPU_H 26 | #define HARMONIC_GPU_H 27 | 28 | 29 | #include "harmonic.h" 30 | 31 | namespace epic { 32 | 33 | /** 34 | * Execute a Gauss-Seidel harmonic function solver until convergence. Uses the GPU (CUDA). 35 | * @param harmonic The Harmonic object. 36 | * @param numThreads The number of threads, as a multiple of 32 (e.g., 1024). 37 | * @return Returns zero upon success, non-zero otherwise. 38 | */ 39 | extern "C" int harmonic_complete_gpu(Harmonic *harmonic, unsigned int numThreads); 40 | 41 | /** 42 | * Step 1/3: Initialize the GPU-specific variables for the Gauss-Seidel GPU implementation. 43 | * @param harmonic The Harmonic object. 44 | * @param numThreads The number of threads, as a multiple of 32 (e.g., 1024). 45 | * @return Returns zero upon success, non-zero otherwise. 46 | */ 47 | extern "C" int harmonic_initialize_gpu(Harmonic *harmonic, unsigned int numThreads); 48 | 49 | /** 50 | * Step 2/3: Execute the Gauss-Seidel GPU update step until convergence. 51 | * @param harmonic The Harmonic object. 52 | * @param numThreads The number of threads, as a multiple of 32 (e.g., 1024). 53 | * @return Returns zero upon success, non-zero otherwise. 54 | */ 55 | extern "C" int harmonic_execute_gpu(Harmonic *harmonic, unsigned int numThreads); 56 | 57 | /** 58 | * Step 3/3: Uninitialize the GPU-specific variables for the Gauss-Seidel GPU implementation. 59 | * @param harmonic The Harmonic object. 60 | * @return Returns zero upon success, non-zero otherwise. 61 | */ 62 | extern "C" int harmonic_uninitialize_gpu(Harmonic *harmonic); 63 | 64 | /** 65 | * Perform a single update step of the Gauss-Seidel GPU implementation. 66 | * @param harmonic The Harmonic object. 67 | * @param numThreads The number of threads, as a multiple of 32 (e.g., 1024). 68 | * @return Returns zero upon success, non-zero otherwise. 69 | */ 70 | extern "C" int harmonic_update_gpu(Harmonic *harmonic, unsigned int numThreads); 71 | 72 | /** 73 | * Perform a single update step of the Gauss-Seidel GPU implementation, plus check 74 | * for convergence of the iteration. 75 | * @param harmonic The Harmonic object. 76 | * @param numThreads The number of threads, as a multiple of 32 (e.g., 1024). 77 | * @return Returns zero upon success, non-zero otherwise. 78 | */ 79 | extern "C" int harmonic_update_and_check_gpu(Harmonic *harmonic, unsigned int numThreads); 80 | 81 | /** 82 | * Copy the potential values from the GPU-side memory to the CPU-side memory. 83 | * @param harmonic The Harmonic object. 84 | * @return Returns zero upon success, non-zero otherwise. 85 | */ 86 | extern "C" int harmonic_get_potential_values_gpu(Harmonic *harmonic); 87 | 88 | }; 89 | 90 | 91 | #endif // HARMONIC_GPU_H 92 | 93 | -------------------------------------------------------------------------------- /libepic/include/epic/harmonic/harmonic_legacy_cpu.h: -------------------------------------------------------------------------------- 1 | /** 2 | * The MIT License (MIT) 3 | * 4 | * Copyright (c) 2016 Kyle Hollins Wray, University of Massachusetts 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | * this software and associated documentation files (the "Software"), to deal in 8 | * the Software without restriction, including without limitation the rights to 9 | * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | * the Software, and to permit persons to whom the Software is furnished to do so, 11 | * subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in all 14 | * copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | 25 | #ifndef HARMONIC_LEGACY_CPU_H 26 | #define HARMONIC_LEGACY_CPU_H 27 | 28 | 29 | namespace epic { 30 | 31 | /** 32 | * Execute a Successive Over-Relaxation (SOR) harmonic function solver until convergence. 33 | * Uses the CPU. Important: This legacy code assumes non-log-space for u! 34 | * @param w The width (x-axis). 35 | * @param h The height (y-axis). 36 | * @param epsilon The desired precision. Default is 0.1. 37 | * @param omega The relaxation parameter in [0, 2]. Default is 1.5. 38 | * @param locked The locked (xy array): 1 for locked, 0 for unlocked. 39 | * @param u The potentials (xy array): 0 for goal, 1 for obstacle, and in range (0, 1) 40 | * otherwise. Stored in row-major form (e.g., y * w + x). This will be modified. 41 | * @param iter The number of iterations until convergence. This will be modified. 42 | * @return Returns zero upon success, non-zero otherwise. 43 | */ 44 | extern "C" int harmonic_legacy_sor_2d_float_cpu(unsigned int w, unsigned int h, float epsilon, 45 | float omega, unsigned int *locked, float *u, unsigned int &iter); 46 | 47 | /** 48 | * Execute a Successive Over-Relaxation (SOR) harmonic function solver until convergence. 49 | * Uses the CPU. Important: This legacy code assumes non-log-space for u! 50 | * @param w The width (x-axis). 51 | * @param h The height (y-axis). 52 | * @param epsilon The desired precision. Default is 0.1. 53 | * @param omega The relaxation parameter in [0, 2]. Default is 1.5. 54 | * @param locked The locked (xy array): 1 for locked, 0 for unlocked. 55 | * @param u The potentials (xy array): 0 for goal, 1 for obstacle, and in range (0, 1) 56 | * otherwise. Stored in row-major form (e.g., y * w + x). This will be modified. 57 | * @param iter The number of iterations until convergence. This will be modified. 58 | * @return Returns zero upon success, non-zero otherwise. 59 | */ 60 | extern "C" int harmonic_legacy_sor_2d_double_cpu(unsigned int w, unsigned int h, double epsilon, 61 | double omega, unsigned int *locked, double *u, unsigned int &iter); 62 | 63 | /** 64 | * Execute a Successive Over-Relaxation (SOR) harmonic function solver until convergence. 65 | * Uses the CPU. Important: This legacy code assumes non-log-space for u! 66 | * @param w The width (x-axis). 67 | * @param h The height (y-axis). 68 | * @param epsilon The desired precision. Default is 0.1. 69 | * @param omega The relaxation parameter in [0, 2]. Default is 1.5. 70 | * @param locked The locked (xy array): 1 for locked, 0 for unlocked. 71 | * @param u The potentials (xy array): 0 for goal, 1 for obstacle, and in range (0, 1) 72 | * otherwise. Stored in row-major form (e.g., y * w + x). This will be modified. 73 | * @param iter The number of iterations until convergence. This will be modified. 74 | * @return Returns zero upon success, non-zero otherwise. 75 | */ 76 | extern "C" int harmonic_legacy_sor_2d_long_double_cpu(unsigned int w, unsigned int h, long double epsilon, 77 | long double omega, unsigned int *locked, long double *u, unsigned int &iter); 78 | 79 | }; 80 | 81 | 82 | #endif // HARMONIC_LEGACY_CPU_H 83 | 84 | -------------------------------------------------------------------------------- /libepic/include/epic/harmonic/harmonic_legacy_path_cpu.h: -------------------------------------------------------------------------------- 1 | /** 2 | * The MIT License (MIT) 3 | * 4 | * Copyright (c) 2015 Kyle Hollins Wray, University of Massachusetts 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | * this software and associated documentation files (the "Software"), to deal in 8 | * the Software without restriction, including without limitation the rights to 9 | * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | * the Software, and to permit persons to whom the Software is furnished to do so, 11 | * subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in all 14 | * copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | 25 | #ifndef HARMONIC_LEGACY_PATH_CPU_H 26 | #define HARMONIC_LEGACY_PATH_CPU_H 27 | 28 | 29 | namespace epic { 30 | 31 | /** 32 | * Compute the potential in two dimensions at an (x, y) location which is 33 | * in "double pixel units" -- meaning these are "blended cell indexes." 34 | * @param w The width dimension of u and locked. 35 | * @param h The height dimension of u and locked. 36 | * @param locked The locked cells of size w * h. 37 | * @param u The potential values of size w * h. 38 | * @param x The x "double pixel" or "blended cell index" location. 39 | * @param y The y "double pixel" or "blended cell index" location. 40 | * @param potential The potential at this location. This will be modified. 41 | * @return Returns zero upon success, non-zero otherwise. 42 | */ 43 | extern "C" int harmonic_legacy_compute_potential_2d_cpu(unsigned int w, unsigned int h, unsigned int *locked, double *u, 44 | double x, double y, double &potential); 45 | 46 | /** 47 | * Compute the gradient in two dimensions at an (x, y) location which is 48 | * in so-called "double pixel units." 49 | * @param w The width dimension of u and locked. 50 | * @param h The height dimension of u and locked. 51 | * @param locked The locked cells of size w * h. 52 | * @param u The potential values of size w * h. 53 | * @param x The x "double pixel" or "blended cell index" location. 54 | * @param y The y "double pixel" or "blended cell index" location. 55 | * @param cdPrecision The central difference (gradient) precision. Default is 0.5. 56 | * @param partialX The gradient in the x axis. This will be modified. 57 | * @param partialY The gradient in the y axis. This will be modified. 58 | * @return Returns zero upon success, non-zero otherwise. 59 | */ 60 | extern "C" int harmonic_legacy_compute_gradient_2d_cpu(unsigned int w, unsigned int h, unsigned int *locked, double *u, 61 | double x, double y, double cdPrecision, double &partialX, double &partialY); 62 | 63 | /** 64 | * Compute a path by following the gradient of the potential values. The starting 65 | * location is specified in "double pixel units" and the path is also in these units. 66 | * @param w The width dimension of u and locked. 67 | * @param h The height dimension of u and locked. 68 | * @param locked The locked cells of size w * h. 69 | * @param u The potential values of size w * h. 70 | * @param x The x "double pixel" start location. 71 | * @param y The y "double pixel" start location. 72 | * @param stepSize The step size in "double pixels." Default is 0.05. 73 | * @param cdPrecision The central difference (gradient) precision. Default is 0.5. 74 | * @param maxLength The maximum path length (in number of steps). 75 | * @param flipped Positive means to perform gradient ascent, otherwise descent. 76 | * @param k The number of elements in the path. This will be modified. 77 | * @param path The path (2k array) of the form [x1, y1, x2, y2, ..., xk, yk]. 78 | * This will be created and modified. 79 | * @return Returns zero upon success, non-zero otherwise. 80 | */ 81 | extern "C" int harmonic_legacy_compute_path_2d_cpu(unsigned int w, unsigned int h, unsigned int *locked, double *u, 82 | double x, double y, double stepSize, double cdPrecision, unsigned int maxLength, int flipped, 83 | unsigned int &k, double *&path); 84 | 85 | /** 86 | * Free the path provided. Useful for wrappers, such as Python. 87 | * @param path The path to free. This will be modified. 88 | * @return Returns zero upon success, non-zero otherwise. 89 | */ 90 | extern "C" int harmonic_legacy_free_path_cpu(double *&path); 91 | 92 | }; 93 | 94 | 95 | #endif // HARMONIC_LEGACY_PATH_CPU_H 96 | 97 | -------------------------------------------------------------------------------- /libepic/include/epic/harmonic/harmonic_model_gpu.h: -------------------------------------------------------------------------------- 1 | /** 2 | * The MIT License (MIT) 3 | * 4 | * Copyright (c) 2015 Kyle Hollins Wray, University of Massachusetts 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | * this software and associated documentation files (the "Software"), to deal in 8 | * the Software without restriction, including without limitation the rights to 9 | * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | * the Software, and to permit persons to whom the Software is furnished to do so, 11 | * subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in all 14 | * copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | 25 | #ifndef HARMONIC_MODEL_GPU_H 26 | #define HARMONIC_MODEL_GPU_H 27 | 28 | 29 | #include "harmonic.h" 30 | 31 | namespace epic { 32 | 33 | /** 34 | * Initialize CUDA by transferring dimension size information to the device. 35 | * @param harmonic The Harmonic object. 36 | * @return Returns zero upon success, non-zero otherwise. 37 | */ 38 | extern "C" int harmonic_initialize_dimension_size_gpu(Harmonic *harmonic); 39 | 40 | /** 41 | * Uninitialize CUDA by transferring dimension size information to the device. 42 | * @param harmonic The Harmonic object. 43 | * @return Returns zero upon success, non-zero otherwise. 44 | */ 45 | extern "C" int harmonic_uninitialize_dimension_size_gpu(Harmonic *harmonic); 46 | 47 | /** 48 | * Initialize CUDA by transferring potential value information to the device. 49 | * @param harmonic The Harmonic object. 50 | * @return Returns zero upon success, non-zero otherwise. 51 | */ 52 | extern "C" int harmonic_initialize_potential_values_gpu(Harmonic *harmonic); 53 | 54 | /** 55 | * Uninitialize CUDA by transferring potential value information to the device. 56 | * @param harmonic The Harmonic object. 57 | * @return Returns zero upon success, non-zero otherwise. 58 | */ 59 | extern "C" int harmonic_uninitialize_potential_values_gpu(Harmonic *harmonic); 60 | 61 | /** 62 | * Initialize CUDA by transferring locked information to the device. 63 | * @param harmonic The Harmonic object. 64 | * @return Returns zero upon success, non-zero otherwise. 65 | */ 66 | extern "C" int harmonic_initialize_locked_gpu(Harmonic *harmonic); 67 | 68 | /** 69 | * Uninitialize CUDA by transferring locked information to the device. 70 | * @param harmonic The Harmonic object. 71 | * @return Returns zero upon success, non-zero otherwise. 72 | */ 73 | extern "C" int harmonic_uninitialize_locked_gpu(Harmonic *harmonic); 74 | 75 | /** 76 | * Update the potential values and the locked values from host to device. 77 | * @param harmonic The Harmonic object. 78 | * @return Returns zero upon success, non-zero otherwise. 79 | */ 80 | extern "C" int harmonic_update_model_gpu(Harmonic *harmonic); 81 | 82 | }; 83 | 84 | 85 | #endif // HARMONIC_MODEL_GPU_H 86 | 87 | -------------------------------------------------------------------------------- /libepic/include/epic/harmonic/harmonic_path_cpu.h: -------------------------------------------------------------------------------- 1 | /** 2 | * The MIT License (MIT) 3 | * 4 | * Copyright (c) 2015 Kyle Hollins Wray, University of Massachusetts 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | * this software and associated documentation files (the "Software"), to deal in 8 | * the Software without restriction, including without limitation the rights to 9 | * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | * the Software, and to permit persons to whom the Software is furnished to do so, 11 | * subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in all 14 | * copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | 25 | #ifndef HARMONIC_PATH_CPU_H 26 | #define HARMONIC_PATH_CPU_H 27 | 28 | 29 | #include "harmonic.h" 30 | 31 | namespace epic { 32 | 33 | /** 34 | * Compute the potential in two dimensions at an (x, y) location which is 35 | * in "float pixel units" -- meaning these are "blended cell indexes." 36 | * @param harmonic The Harmonic object. 37 | * @param x The x "float pixel" or "blended cell index" location. 38 | * @param y The y "float pixel" or "blended cell index" location. 39 | * @param potential The potential at this location. This will be modified. 40 | * @return Returns zero upon success, non-zero otherwise. 41 | */ 42 | extern "C" int harmonic_compute_potential_2d_cpu(Harmonic *harmonic, 43 | float x, float y, float &potential); 44 | 45 | /** 46 | * Compute the gradient in two dimensions at an (x, y) location which is 47 | * in so-called "float pixel units." 48 | * @param harmonic The Harmonic object. 49 | * @param x The x "float pixel" or "blended cell index" location. 50 | * @param y The y "float pixel" or "blended cell index" location. 51 | * @param cdPrecision The central difference (gradient) precision. Default is 0.5. 52 | * @param partialX The gradient in the x axis. This will be modified. 53 | * @param partialY The gradient in the y axis. This will be modified. 54 | * @return Returns zero upon success, non-zero otherwise. 55 | */ 56 | extern "C" int harmonic_compute_gradient_2d_cpu(Harmonic *harmonic, float x, float y, 57 | float cdPrecision, float &partialX, float &partialY); 58 | 59 | /** 60 | * Compute a path by following the gradient of the potential values. The starting 61 | * location is specified in "float pixel units" and the path is also in these units. 62 | * @param harmonic The Harmonic object. 63 | * @param x The x "float pixel" start location. 64 | * @param y The y "float pixel" start location. 65 | * @param stepSize The step size in "float pixels." Default is 0.05. 66 | * @param cdPrecision The central difference (gradient) precision. Default is 0.5. 67 | * @param maxLength The maximum path length (in number of steps). 68 | * @param k The number of elements in the path. This will be modified. 69 | * @param path The path (2k array) of the form [x1, y1, x2, y2, ..., xk, yk]. 70 | * This will be created and modified. 71 | * @return Returns zero upon success, non-zero otherwise. 72 | */ 73 | extern "C" int harmonic_compute_path_2d_cpu(Harmonic *harmonic, float x, float y, 74 | float stepSize, float cdPrecision, unsigned int maxLength, 75 | unsigned int &k, float *&path); 76 | 77 | /** 78 | * Free the path provided. Useful for wrappers, such as Python. 79 | * @param path The path to free. This will be modified. 80 | * @return Returns zero upon success, non-zero otherwise. 81 | */ 82 | extern "C" int harmonic_free_path_cpu(float *&path); 83 | 84 | }; 85 | 86 | 87 | #endif // HARMONIC_PATH_CPU_H 88 | 89 | -------------------------------------------------------------------------------- /libepic/include/epic/harmonic/harmonic_utilities_cpu.h: -------------------------------------------------------------------------------- 1 | /** 2 | * The MIT License (MIT) 3 | * 4 | * Copyright (c) 2015 Kyle Hollins Wray, University of Massachusetts 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | * this software and associated documentation files (the "Software"), to deal in 8 | * the Software without restriction, including without limitation the rights to 9 | * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | * the Software, and to permit persons to whom the Software is furnished to do so, 11 | * subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in all 14 | * copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | 25 | #ifndef HARMONIC_UTILITIES_CPU_H 26 | #define HARMONIC_UTILITIES_CPU_H 27 | 28 | 29 | #include "harmonic.h" 30 | 31 | namespace epic { 32 | 33 | /** 34 | * Assign a cells to obstacle, goal, or free space in two dimensions. 35 | * @param harmonic The Harmonic object. 36 | * @param k The number of cell locations to update. 37 | * @param v The k 2-dimensional cell locations: [x1, y1, x2, y2, ..., xk, yk]. 38 | * @param types Sets 0 for goal, 1 for obstacle, and 2 for free (constants.h). 39 | * @return Returns zero upon success, non-zero otherwise. 40 | */ 41 | extern "C" int harmonic_utilities_set_cells_2d_cpu(Harmonic *harmonic, 42 | unsigned int k, unsigned int *v, unsigned int *types); 43 | 44 | }; 45 | 46 | 47 | #endif // HARMONIC_UTILITIES_CPU_H 48 | 49 | -------------------------------------------------------------------------------- /libepic/include/epic/harmonic/harmonic_utilities_gpu.h: -------------------------------------------------------------------------------- 1 | /** 2 | * The MIT License (MIT) 3 | * 4 | * Copyright (c) 2015 Kyle Hollins Wray, University of Massachusetts 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | * this software and associated documentation files (the "Software"), to deal in 8 | * the Software without restriction, including without limitation the rights to 9 | * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | * the Software, and to permit persons to whom the Software is furnished to do so, 11 | * subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in all 14 | * copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | 25 | #ifndef HARMONIC_UTILITIES_GPU_H 26 | #define HARMONIC_UTILITIES_GPU_H 27 | 28 | 29 | #include "harmonic.h" 30 | 31 | namespace epic { 32 | 33 | /** 34 | * Assign a cells to obstacle, goal, or free space in two dimensions on the GPU. 35 | * @param harmonic The Harmonic object. 36 | * @param numThreads The number of threads to use, a multiple of 32. Default is 1024. 37 | * @param k The number of cell locations to update. 38 | * @param v The k 2-dimensional cell locations: [x1, y1, x2, y2, ..., xk, yk]. 39 | * @param types Sets 0 for goal, 1 for obstacle, and 2 for free (constants.h). 40 | * @return Returns zero upon success, non-zero otherwise. 41 | */ 42 | extern "C" int harmonic_utilities_set_cells_2d_gpu(Harmonic *harmonic, unsigned int numThreads, 43 | unsigned int k, unsigned int *v, unsigned int *types); 44 | 45 | }; 46 | 47 | 48 | #endif // HARMONIC_UTILITIES_GPU_H 49 | 50 | 51 | -------------------------------------------------------------------------------- /libepic/lib/libepic.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kylewray/epic/4fc4e8f6ee51e7f800cd4db34836212995d5396e/libepic/lib/libepic.so -------------------------------------------------------------------------------- /libepic/python/__init__.py: -------------------------------------------------------------------------------- 1 | """ The MIT License (MIT) 2 | 3 | Copyright (c) 2015 Kyle Hollins Wray, University of Massachusetts 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy of 6 | this software and associated documentation files (the "Software"), to deal in 7 | the Software without restriction, including without limitation the rights to 8 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 9 | the Software, and to permit persons to whom the Software is furnished to do so, 10 | 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, FITNESS 17 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 18 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 19 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 20 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 21 | """ 22 | 23 | 24 | -------------------------------------------------------------------------------- /libepic/python/epic/__init__.py: -------------------------------------------------------------------------------- 1 | """ The MIT License (MIT) 2 | 3 | Copyright (c) 2015 Kyle Hollins Wray, University of Massachusetts 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy of 6 | this software and associated documentation files (the "Software"), to deal in 7 | the Software without restriction, including without limitation the rights to 8 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 9 | the Software, and to permit persons to whom the Software is furnished to do so, 10 | 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, FITNESS 17 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 18 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 19 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 20 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 21 | """ 22 | 23 | __all__ = ["epic_harmonic", "harmonic", "harmonic_map", "harmonic_legacy", "harmonic_legacy_map"] 24 | 25 | -------------------------------------------------------------------------------- /libepic/python/epic/epic_harmonic.py: -------------------------------------------------------------------------------- 1 | """ The MIT License (MIT) 2 | 3 | Copyright (c) 2015 Kyle Hollins Wray, University of Massachusetts 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy of 6 | this software and associated documentation files (the "Software"), to deal in 7 | the Software without restriction, including without limitation the rights to 8 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 9 | the Software, and to permit persons to whom the Software is furnished to do so, 10 | 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, FITNESS 17 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 18 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 19 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 20 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 21 | """ 22 | 23 | import ctypes as ct 24 | import platform 25 | import os.path 26 | 27 | 28 | # Check if we need to create the harmonic variable. If so, import the correct library 29 | # file depending on the platform. 30 | #try: 31 | # _epic 32 | #except NameError: 33 | _epic = None 34 | if platform.system() == "Windows": 35 | _ = ct.CDLL(os.path.join(os.path.dirname(os.path.realpath(__file__)), 36 | "..", "..", "lib", "libepic.dll")) 37 | else: 38 | _epic = ct.CDLL(os.path.join(os.path.dirname(os.path.realpath(__file__)), 39 | "..", "..", "lib", "libepic.so")) 40 | 41 | 42 | class EpicHarmonic(ct.Structure): 43 | """ The C struct Harmonic object. """ 44 | 45 | _fields_ = [("n", ct.c_uint), 46 | ("m", ct.POINTER(ct.c_uint)), 47 | ("u", ct.POINTER(ct.c_float)), 48 | ("locked", ct.POINTER(ct.c_uint)), 49 | ("epsilon", ct.c_float), 50 | ("delta", ct.c_float), 51 | ("numIterationsToStaggerCheck", ct.c_uint), 52 | ("currentIteration", ct.c_uint), 53 | ("d_m", ct.POINTER(ct.c_uint)), 54 | ("d_u", ct.POINTER(ct.c_float)), 55 | ("d_locked", ct.POINTER(ct.c_uint)), 56 | ("d_delta", ct.POINTER(ct.c_float)), 57 | ] 58 | 59 | 60 | # Functions from 'harmonic_cpu.h'. 61 | _epic.harmonic_complete_cpu.argtypes = tuple([ct.POINTER(EpicHarmonic)]) 62 | _epic.harmonic_update_cpu.argtypes = tuple([ct.POINTER(EpicHarmonic)]) 63 | _epic.harmonic_update_and_check_cpu.argtypes = tuple([ct.POINTER(EpicHarmonic)]) 64 | 65 | # Functions from 'harmonic_gpu.h'. 66 | _epic.harmonic_complete_gpu.argtypes = (ct.POINTER(EpicHarmonic), ct.c_uint) 67 | _epic.harmonic_initialize_gpu.argtypes = (ct.POINTER(EpicHarmonic), ct.c_uint) 68 | _epic.harmonic_execute_gpu.argtypes = (ct.POINTER(EpicHarmonic), ct.c_uint) 69 | _epic.harmonic_uninitialize_gpu.argtypes = tuple([ct.POINTER(EpicHarmonic)]) 70 | _epic.harmonic_update_gpu.argtypes = (ct.POINTER(EpicHarmonic), ct.c_uint) 71 | _epic.harmonic_update_and_check_gpu.argtypes = (ct.POINTER(EpicHarmonic), ct.c_uint) 72 | _epic.harmonic_get_potential_values_gpu.argtypes = tuple([ct.POINTER(EpicHarmonic)]) 73 | 74 | # Functions from 'harmonic_model_gpu.h'. 75 | _epic.harmonic_initialize_dimension_size_gpu.argtypes = tuple([ct.POINTER(EpicHarmonic)]) 76 | _epic.harmonic_uninitialize_dimension_size_gpu.argtypes = tuple([ct.POINTER(EpicHarmonic)]) 77 | 78 | _epic.harmonic_initialize_potential_values_gpu.argtypes = tuple([ct.POINTER(EpicHarmonic)]) 79 | _epic.harmonic_uninitialize_potential_values_gpu.argtypes = tuple([ct.POINTER(EpicHarmonic)]) 80 | 81 | _epic.harmonic_initialize_locked_gpu.argtypes = tuple([ct.POINTER(EpicHarmonic)]) 82 | _epic.harmonic_uninitialize_locked_gpu.argtypes = tuple([ct.POINTER(EpicHarmonic)]) 83 | 84 | _epic.harmonic_update_model_gpu.argtypes = tuple([ct.POINTER(EpicHarmonic)]) 85 | 86 | # Functions from 'harmonic_utilities_cpu.h'. 87 | _epic.harmonic_utilities_set_cells_2d_cpu.argtypes = (ct.POINTER(EpicHarmonic), ct.c_uint, 88 | ct.POINTER(ct.c_uint), ct.POINTER(ct.c_uint)) 89 | 90 | # Functions from 'harmonic_utilities_gpu.h'. 91 | _epic.harmonic_utilities_set_cells_2d_gpu.argtypes = (ct.POINTER(EpicHarmonic), ct.c_uint, ct.c_uint, 92 | ct.POINTER(ct.c_uint), ct.POINTER(ct.c_uint)) 93 | 94 | # Functions from 'harmonic_path_cpu.h'. 95 | _epic.harmonic_compute_potential_2d_cpu.argtypes = (ct.POINTER(EpicHarmonic), ct.c_float, ct.c_float, 96 | ct.POINTER(ct.c_float)) 97 | _epic.harmonic_compute_gradient_2d_cpu.argtypes = (ct.POINTER(EpicHarmonic), ct.c_float, ct.c_float, ct.c_float, 98 | ct.POINTER(ct.c_float), ct.POINTER(ct.c_float)) 99 | _epic.harmonic_compute_path_2d_cpu.argtypes = (ct.POINTER(EpicHarmonic), ct.c_float, ct.c_float, 100 | ct.c_float, ct.c_float, ct.c_uint, 101 | ct.POINTER(ct.c_uint), ct.POINTER(ct.POINTER(ct.c_float))) 102 | _epic.harmonic_free_path_cpu.argtypes = tuple([ct.POINTER(ct.POINTER(ct.c_float))]) 103 | 104 | # Functions from 'harmonic_legacy_cpu.h'. 105 | _epic.harmonic_legacy_sor_2d_float_cpu.argtypes = (ct.c_uint, ct.c_uint, ct.c_float, ct.c_float, 106 | ct.POINTER(ct.c_uint), ct.POINTER(ct.c_float), 107 | ct.POINTER(ct.c_uint)) 108 | _epic.harmonic_legacy_sor_2d_double_cpu.argtypes = (ct.c_uint, ct.c_uint, ct.c_double, ct.c_double, 109 | ct.POINTER(ct.c_uint), ct.POINTER(ct.c_double), 110 | ct.POINTER(ct.c_uint)) 111 | _epic.harmonic_legacy_sor_2d_long_double_cpu.argtypes = (ct.c_uint, ct.c_uint, ct.c_longdouble, ct.c_longdouble, 112 | ct.POINTER(ct.c_uint), ct.POINTER(ct.c_longdouble), 113 | ct.POINTER(ct.c_uint)) 114 | 115 | # Functions from 'harmonic_legacy_path_cpu.h'. 116 | _epic.harmonic_legacy_compute_potential_2d_cpu.argtypes = (ct.c_uint, ct.c_uint, ct.POINTER(ct.c_uint), ct.POINTER(ct.c_double), 117 | ct.c_double, ct.c_double, ct.POINTER(ct.c_double)) 118 | _epic.harmonic_legacy_compute_gradient_2d_cpu.argtypes = (ct.c_uint, ct.c_uint, ct.POINTER(ct.c_uint), ct.POINTER(ct.c_double), 119 | ct.c_double, ct.c_double, ct.c_double, 120 | ct.POINTER(ct.c_double), ct.POINTER(ct.c_double)) 121 | _epic.harmonic_legacy_compute_path_2d_cpu.argtypes = (ct.c_uint, ct.c_uint, ct.POINTER(ct.c_uint), ct.POINTER(ct.c_double), 122 | ct.c_double, ct.c_double, ct.c_double, ct.c_double, ct.c_uint, ct.c_int, 123 | ct.POINTER(ct.c_uint), ct.POINTER(ct.POINTER(ct.c_double))) 124 | _epic.harmonic_legacy_free_path_cpu.argtypes = tuple([ct.POINTER(ct.POINTER(ct.c_double))]) 125 | 126 | -------------------------------------------------------------------------------- /libepic/python/epic/harmonic.py: -------------------------------------------------------------------------------- 1 | """ The MIT License (MIT) 2 | 3 | Copyright (c) 2015 Kyle Hollins Wray, University of Massachusetts 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy of 6 | this software and associated documentation files (the "Software"), to deal in 7 | the Software without restriction, including without limitation the rights to 8 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 9 | the Software, and to permit persons to whom the Software is furnished to do so, 10 | 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, FITNESS 17 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 18 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 19 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 20 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 21 | """ 22 | 23 | import os 24 | import sys 25 | import time 26 | 27 | import ctypes as ct 28 | import numpy as np 29 | 30 | sys.path.append(os.path.join(os.path.dirname(os.path.realpath(__file__)))) 31 | import epic_harmonic as eh 32 | 33 | 34 | class Harmonic(eh.EpicHarmonic): 35 | """ A Harmonic object that can be used to easily solve harmonic functions. """ 36 | 37 | def __init__(self): 38 | """ The constructor for the Harmonic class. """ 39 | 40 | # Assign a nullptr for the device-side pointers. These will be set if the GPU is utilized. 41 | self.n = int(0) 42 | self.m = ct.POINTER(ct.c_uint)() 43 | self.u = ct.POINTER(ct.c_float)() 44 | self.locked = ct.POINTER(ct.c_uint)() 45 | self.epsilon = 1e-2 46 | self.delta = self.epsilon + 1.0 47 | self.numIterationsToStaggerCheck = int(100) 48 | self.currentIteration = int(0) 49 | self.d_m = ct.POINTER(ct.c_uint)() 50 | self.d_u = ct.POINTER(ct.c_float)() 51 | self.d_locked = ct.POINTER(ct.c_uint)() 52 | self.d_delta = ct.POINTER(ct.c_float)() 53 | 54 | def solve(self, algorithm='gauss-seidel', process='gpu', numThreads=1024, epsilon=1e-2): 55 | """ Solve the Harmonic function. 56 | 57 | Parameters: 58 | algorithm -- The algorithm to use. Default is 'gauss-seidel'. 59 | process -- Use the 'cpu' or 'gpu'. If 'gpu' fails, it tries 'cpu'. Default is 'gpu'. 60 | numThreads -- The number of CUDA threads to execute (multiple of 32). Default is 1024. 61 | epsilon -- The error from the true final value *in log space*. Default is 1e-2. 62 | 63 | Returns: 64 | A pair (wall-time, cpu-time) for the solver execution time, not including (un)initialization. 65 | """ 66 | 67 | self.epsilon = epsilon 68 | 69 | timing = None 70 | 71 | if algorithm == 'gauss-seidel': 72 | if process == 'gpu': 73 | result = eh._epic.harmonic_initialize_dimension_size_gpu(self) 74 | result += eh._epic.harmonic_initialize_potential_values_gpu(self) 75 | result += eh._epic.harmonic_initialize_locked_gpu(self) 76 | if result != 0: 77 | print("Failed to initialize the harmonic variables for the 'epic' library's GPU Gauss-Seidel solver.") 78 | process = 'cpu' 79 | 80 | timing = (time.time(), time.clock()) 81 | result = eh._epic.harmonic_complete_gpu(self, int(numThreads)) 82 | timing = (time.time() - timing[0], time.clock() - timing[1]) 83 | 84 | if result != 0: 85 | print("Failed to execute the 'epic' library's GPU Gauss-Seidel solver.") 86 | process = 'cpu' 87 | 88 | result = eh._epic.harmonic_uninitialize_dimension_size_gpu(self) 89 | result += eh._epic.harmonic_uninitialize_potential_values_gpu(self) 90 | result += eh._epic.harmonic_uninitialize_locked_gpu(self) 91 | if result != 0: 92 | # Note: Failing at uninitialization should not cause the CPU version to be executed. 93 | print("Failed to uninitialize the harmonic variables for the 'epic' library's GPU Gauss-Seidel solver.") 94 | 95 | if process == 'cpu': 96 | timing = (time.time(), time.clock()) 97 | result = eh._epic.harmonic_complete_cpu(self) 98 | timing = (time.time() - timing[0], time.clock() - timing[1]) 99 | 100 | if result != 0: 101 | print("Failed to execute the 'harmonic' library's CPU Gauss-Seidel solver.") 102 | raise Exception() 103 | 104 | else: 105 | print("Failed to solve since the algorithm '%' is undefined." % (algorithm)) 106 | 107 | return timing 108 | 109 | def __str__(self): 110 | """ Return the string of the Harmonic values. 111 | 112 | Returns: 113 | The string of the Harmonic values. 114 | """ 115 | 116 | result = "n: " + str(self.n) + "\n" 117 | result += "m: " + str([self.m[i] for i in range(self.n)]) + "\n" 118 | result += "epsilon: " + str(self.epsilon) + "\n" 119 | 120 | numCells = 1 121 | for i in range(self.n): 122 | numCells *= int(self.m[i]) 123 | 124 | result += "u:\n" + str(np.array([self.u[i] for i in range(numCells)]).reshape([self.m[i] for i in range(self.n)])) + "\n\n" 125 | 126 | result += "locked:\n" + str(np.array([self.locked[i] for i in range(numCells)]).reshape([self.m[i] for i in range(self.n)])) + "\n\n" 127 | 128 | return result 129 | 130 | -------------------------------------------------------------------------------- /libepic/python/epic/harmonic_legacy.py: -------------------------------------------------------------------------------- 1 | """ The MIT License (MIT) 2 | 3 | Copyright (c) 2015 Kyle Hollins Wray, University of Massachusetts 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy of 6 | this software and associated documentation files (the "Software"), to deal in 7 | the Software without restriction, including without limitation the rights to 8 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 9 | the Software, and to permit persons to whom the Software is furnished to do so, 10 | 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, FITNESS 17 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 18 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 19 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 20 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 21 | """ 22 | 23 | import os 24 | import sys 25 | import time 26 | 27 | import ctypes as ct 28 | import numpy as np 29 | 30 | sys.path.append(os.path.join(os.path.dirname(os.path.realpath(__file__)))) 31 | import epic_harmonic as eh 32 | 33 | 34 | class HarmonicLegacy(object): 35 | """ A Legacy Harmonic object that can be used to easily solve harmonic functions. """ 36 | 37 | def __init__(self): 38 | """ The constructor for the HarmonicLegacy class. """ 39 | 40 | # Assign a nullptr for the device-side pointers. 41 | self.w = int(0) 42 | self.h = int(0) 43 | self.epsilon = 1e-2 44 | self.omega = 1.0 45 | self.locked = ct.POINTER(ct.c_uint)() 46 | self.u = ct.POINTER(ct.c_double)() 47 | self.currentIteration = int(0) 48 | 49 | def solve(self, omega=1.0, epsilon=1e-2): 50 | """ Solve the Harmonic Legacy function. 51 | 52 | Parameters: 53 | omega -- The relaxation parameter. Default is 1.0. 54 | epsilon -- The error from the true final value *in log space*. Default is 1e-2. 55 | 56 | Returns: 57 | A pair (wall-time, cpu-time) for the solver execution time, not including (un)initialization. 58 | """ 59 | 60 | self.epsilon = epsilon 61 | self.omega = omega 62 | 63 | timing = None 64 | 65 | numIterations = ct.c_uint(int(0)) 66 | 67 | timing = (time.time(), time.clock()) 68 | result = eh._epic.harmonic_legacy_sor_2d_double_cpu(self.w, self.h, 69 | self.epsilon, self.omega, 70 | self.locked, self.u, 71 | ct.byref(numIterations)) 72 | timing = (time.time() - timing[0], time.clock() - timing[1]) 73 | 74 | self.currentIteration = numIterations.value 75 | 76 | print("DONE! NUM ITERATIONS: %i" % (self.currentIteration)) 77 | 78 | if result != 0: 79 | print("Failed to execute the 'harmonic' library's *legacy* CPU SOR solver.") 80 | 81 | return timing 82 | 83 | def __str__(self): 84 | """ Return the string of the Harmonic Legacy values. 85 | 86 | Returns: 87 | The string of the Harmonic Legacy values. 88 | """ 89 | 90 | result = "w: " + str(self.w) + "\n" 91 | result += "h: " + str(self.h) + "\n" 92 | result += "epsilon: " + str(self.epsilon) + "\n" 93 | result += "omega: " + str(self.omega) + "\n" 94 | result += "locked:\n" + str(np.array([self.locked[i] for i in range(self.w * self.h)]).reshape((w, h))) + "\n\n" 95 | result += "u:\n" + str(np.array([self.u[i] for i in range(self.w * self.h)]).reshape((w, h))) + "\n\n" 96 | 97 | return result 98 | 99 | -------------------------------------------------------------------------------- /libepic/python/epic/harmonic_legacy_map.py: -------------------------------------------------------------------------------- 1 | """ The MIT License (MIT) 2 | 3 | Copyright (c) 2015 Kyle Hollins Wray, University of Massachusetts 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy of 6 | this software and associated documentation files (the "Software"), to deal in 7 | the Software without restriction, including without limitation the rights to 8 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 9 | the Software, and to permit persons to whom the Software is furnished to do so, 10 | 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, FITNESS 17 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 18 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 19 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 20 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 21 | """ 22 | 23 | import os 24 | import sys 25 | 26 | import ctypes as ct 27 | import numpy as np 28 | 29 | import math 30 | 31 | import cv2 32 | 33 | sys.path.append(os.path.join(os.path.dirname(os.path.realpath(__file__)))) 34 | import harmonic_legacy as harm 35 | import epic_harmonic as eh 36 | 37 | 38 | class HarmonicLegacyMap(harm.HarmonicLegacy): 39 | """ A HarmonicLegacyMap object that can be used to easily solve harmonic functions in 2d maps. """ 40 | 41 | def __init__(self): 42 | """ The constructor for the HarmonicLegacyMap class. """ 43 | 44 | super().__init__() 45 | 46 | self.windowTitle = "Harmonic Legacy Map" 47 | 48 | self.originalImage = None 49 | self.image = None 50 | 51 | self.pxSize = 1.0 52 | self.hold = False 53 | 54 | self.flipped = False 55 | 56 | def load(self, filename): 57 | """ Load a map from a 2d grayscale image. 58 | 59 | Parameters: 60 | filename -- The 2d grayscale image file to load. 61 | """ 62 | 63 | # Load the image and handle errors. 64 | self.image = cv2.imread(filename, cv2.IMREAD_GRAYSCALE) 65 | self.originalImage = self.image.copy() 66 | 67 | if self.image is None: 68 | print("Failed to load image file '%s'." % (filename)) 69 | raise Exception() 70 | 71 | # Convert the image into a 2d potential and set other corresponding variables. 72 | self.w = int(self.image.shape[1]) 73 | self.h = int(self.image.shape[0]) 74 | 75 | array_type_u_double = ct.c_double * (self.image.size) 76 | self.u = array_type_u_double(*np.array([[1.0 - float(self.image[y, x] == 255) \ 77 | for x in range(self.image.shape[1])] \ 78 | for y in range(self.image.shape[0])]).flatten()) 79 | 80 | if self.flipped: 81 | self._flip_u_values() 82 | 83 | array_type_locked_uint = ct.c_uint * (self.image.size) 84 | self.locked = array_type_locked_uint(*np.array([[int(self.image[y, x] == 0 or self.image[y, x] == 255) \ 85 | for x in range(self.image.shape[1])] \ 86 | for y in range(self.image.shape[0])]).flatten()) 87 | 88 | def _flip_u_values(self): 89 | """ Flip u-values. """ 90 | 91 | for y in range(self.h): 92 | for x in range(self.w): 93 | self.u[y * self.w + x] = 1.0 - self.u[y * self.w + x] 94 | 95 | def _compute_streamline(self, x, y): 96 | """ Compute a streamline (series of points) starting from this initial (x, y) location. 97 | 98 | Parameters: 99 | x -- The x "double pixel" location to start. 100 | y -- The y "double pixel" location to start. 101 | 102 | Returns: 103 | The list of points from this starting location to a goal. 104 | """ 105 | 106 | k = ct.c_uint(0) 107 | rawPath = ct.POINTER(ct.c_double)() 108 | 109 | result = eh._epic.harmonic_legacy_compute_path_2d_cpu(self.w, self.h, self.locked, self.u, x, y, 110 | float(0.2), float(0.4), int(1e6), int(self.flipped), 111 | ct.byref(k), ct.byref(rawPath)) 112 | if result != 0: 113 | print("Failed to compute path using 'epic' library.") 114 | raise Exception() 115 | 116 | k = int(k.value) 117 | path = [(rawPath[2 * i + 0], rawPath[2 * i + 1]) for i in range(k)] 118 | 119 | result = eh._epic.harmonic_legacy_free_path_cpu(ct.byref(rawPath)) 120 | if result != 0: 121 | print("Failed to free path using 'epic' library.") 122 | 123 | return path 124 | 125 | def _draw_image(self): 126 | """ Draw the image given the updated u values. """ 127 | 128 | # Convert the 2d potential back into an image. 129 | self.image = self.originalImage.copy() 130 | 131 | def show(self): 132 | """ Render the current image to the screen; the escape key quits. """ 133 | 134 | def mouse_clicked(event, x, y, flags, param): 135 | """ Handle mouse clicks in the cv window. This draws a rough streamline from the 136 | clicked position to the goal. 137 | 138 | Parameters: 139 | event -- The event object (button up, down, etc.). 140 | x -- The x location clicked. 141 | y -- The y location clicked. 142 | flags -- Extra flags about the mouse. 143 | param -- Extra params. 144 | """ 145 | 146 | if event == cv2.EVENT_LBUTTONUP: 147 | # Repaint the image, but only if we are not holding. 148 | if not self.hold: 149 | self._draw_image() 150 | 151 | # Compute the streamline and draw the points 152 | for p in self._compute_streamline(x, y): 153 | self.image[p[1], p[0]] = 255 154 | 155 | # Update the image. 156 | cv2.imshow(self.windowTitle, self.image) 157 | 158 | self._draw_image() 159 | 160 | # Show the image and wait for the exit key. 161 | cv2.imshow(self.windowTitle, self.image) 162 | cv2.setMouseCallback(self.windowTitle, mouse_clicked) 163 | #cv2.ResizeWindow(self.windowTitle, max(100, self.image.shape[0]), max(100, self.image.shape[1])) 164 | 165 | key = None 166 | while key != 27: 167 | key = cv2.waitKey(0) 168 | cv2.destroyAllWindows() 169 | 170 | -------------------------------------------------------------------------------- /libepic/python/epic/harmonic_map.py: -------------------------------------------------------------------------------- 1 | """ The MIT License (MIT) 2 | 3 | Copyright (c) 2015 Kyle Hollins Wray, University of Massachusetts 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy of 6 | this software and associated documentation files (the "Software"), to deal in 7 | the Software without restriction, including without limitation the rights to 8 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 9 | the Software, and to permit persons to whom the Software is furnished to do so, 10 | 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, FITNESS 17 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 18 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 19 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 20 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 21 | """ 22 | 23 | import os 24 | import sys 25 | 26 | import ctypes as ct 27 | import numpy as np 28 | 29 | import math 30 | 31 | import cv2 32 | 33 | sys.path.append(os.path.join(os.path.dirname(os.path.realpath(__file__)))) 34 | import harmonic as harm 35 | import epic_harmonic as eh 36 | 37 | 38 | class HarmonicMap(harm.Harmonic): 39 | """ A HarmonicMap object that can be used to easily solve harmonic functions in 2d maps. """ 40 | 41 | def __init__(self): 42 | """ The constructor for the HarmonicMap class. """ 43 | 44 | super().__init__() 45 | 46 | self.windowTitle = "Harmonic Map" 47 | 48 | self.originalImage = None 49 | self.image = None 50 | 51 | self.pxSize = 1.0 52 | self.hold = False 53 | 54 | def load(self, filename): 55 | """ Load a map from a 2d grayscale image. 56 | 57 | Parameters: 58 | filename -- The 2d grayscale image file to load. 59 | """ 60 | 61 | # Load the image and handle errors. 62 | self.image = cv2.imread(filename, cv2.IMREAD_GRAYSCALE) 63 | self.originalImage = self.image.copy() 64 | 65 | if self.image is None: 66 | print("Failed to load image file '%s'." % (filename)) 67 | raise Exception() 68 | 69 | # Convert the image into a 2d potential and set other corresponding variables. 70 | self.n = 2 71 | 72 | array_type_m_uint = ct.c_uint * (self.n) 73 | self.m = array_type_m_uint(*np.array([self.image.shape[0], self.image.shape[1]])) 74 | 75 | array_type_u_float = ct.c_float * (self.image.size) 76 | self.u = array_type_u_float(*np.array([[1.0 - float(self.image[y, x] == 255) \ 77 | for x in range(self.image.shape[1])] \ 78 | for y in range(self.image.shape[0])]).flatten()) 79 | self._convert_log_scale() 80 | 81 | array_type_locked_uint = ct.c_uint * (self.image.size) 82 | self.locked = array_type_locked_uint(*np.array([[int(self.image[y, x] == 0 or self.image[y, x] == 255) \ 83 | for x in range(self.image.shape[1])] \ 84 | for y in range(self.image.shape[0])]).flatten()) 85 | 86 | def _convert_log_scale(self): 87 | """ Convert u to v. """ 88 | 89 | # Since this is supposed to get as close to 0 as possible, we have log(1-1e-13) ~= -4.3442952e-14 90 | # which is within double machine precision. 91 | #epsilon = 1e-130000 92 | 93 | for y in range(self.m[0]): 94 | for x in range(self.m[1]): 95 | # u[y * self.m[1] + x] = np.log((1.0 - self.u[y * self.m[1] + x]) * (1.0 - epsilon) + epsilon) 96 | if self.u[y * self.m[1] + x] == 1.0: 97 | # Note: This is within precision of floats, which has issues around +/- 1e6 98 | self.u[y * self.m[1] + x] = -1e6 99 | else: 100 | self.u[y * self.m[1] + x] = 0.0 101 | 102 | 103 | def _compute_streamline(self, x, y): 104 | """ Compute a streamline (series of points) starting from this initial (x, y) location. 105 | 106 | Parameters: 107 | x -- The x "float pixel" location to start. 108 | y -- The y "float pixel" location to start. 109 | 110 | Returns: 111 | The list of points from this starting location to a goal. 112 | """ 113 | 114 | k = ct.c_uint(0) 115 | rawPath = ct.POINTER(ct.c_float)() 116 | 117 | result = eh._epic.harmonic_compute_path_2d_cpu(self, x, y, 118 | float(0.2), float(0.4), int(1e6), 119 | ct.byref(k), ct.byref(rawPath)) 120 | if result != 0: 121 | print("Failed to compute path using 'epic' library.") 122 | raise Exception() 123 | 124 | k = int(k.value) 125 | path = [(rawPath[2 * i + 0], rawPath[2 * i + 1]) for i in range(k)] 126 | 127 | result = eh._epic.harmonic_free_path_cpu(ct.byref(rawPath)) 128 | if result != 0: 129 | print("Failed to free path using 'epic' library.") 130 | 131 | return path 132 | 133 | def _draw_image(self): 134 | """ Draw the image given the updated u values. """ 135 | 136 | # Convert the 2d potential back into an image. 137 | self.image = self.originalImage.copy() 138 | 139 | def show(self): 140 | """ Render the current image to the screen; the escape key quits. """ 141 | 142 | def mouse_clicked(event, x, y, flags, param): 143 | """ Handle mouse clicks in the cv window. This draws a rough streamline from the 144 | clicked position to the goal. 145 | 146 | Parameters: 147 | event -- The event object (button up, down, etc.). 148 | x -- The x location clicked. 149 | y -- The y location clicked. 150 | flags -- Extra flags about the mouse. 151 | param -- Extra params. 152 | """ 153 | 154 | if event == cv2.EVENT_LBUTTONUP: 155 | # Repaint the image, but only if we are not holding. 156 | if not self.hold: 157 | self._draw_image() 158 | 159 | # Compute the streamline and draw the points 160 | for p in self._compute_streamline(x, y): 161 | self.image[int(p[1]), int(p[0])] = 255 162 | 163 | # Update the image. 164 | cv2.imshow(self.windowTitle, self.image) 165 | 166 | self._draw_image() 167 | 168 | # Show the image and wait for the exit key. 169 | cv2.imshow(self.windowTitle, self.image) 170 | cv2.setMouseCallback(self.windowTitle, mouse_clicked) 171 | #cv2.ResizeWindow(self.windowTitle, max(100, self.image.shape[0]), max(100, self.image.shape[1])) 172 | 173 | key = None 174 | while key != 27: 175 | key = cv2.waitKey(0) 176 | cv2.destroyAllWindows() 177 | 178 | -------------------------------------------------------------------------------- /libepic/src/harmonic/harmonic_cpu.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * The MIT License (MIT) 3 | * 4 | * Copyright (c) 2014 Kyle Hollins Wray, University of Massachusetts 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | * this software and associated documentation files (the "Software"), to deal in 8 | * the Software without restriction, including without limitation the rights to 9 | * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | * the Software, and to permit persons to whom the Software is furnished to do so, 11 | * subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in all 14 | * copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | 36 | namespace epic { 37 | 38 | void harmonic_update_2d_cpu(Harmonic *harmonic, bool checkConvergence) 39 | { 40 | if (checkConvergence) { 41 | harmonic->delta = 0.0; 42 | } 43 | 44 | // Iterate over all non-boundary cells and update its value based on a red-black ordering. 45 | // Thus, for all rows, we either skip by evens or odds in 2-dimensions. 46 | for (unsigned int x0 = 1; x0 < harmonic->m[0] - 1; x0++) { 47 | // Determine if this rows starts with a red (even row) or black (odd row) cell, and 48 | // update the opposite depending on how many iterations there have been. 49 | unsigned int offset = (unsigned int)((harmonic->currentIteration % 2) != (x0 % 2)); 50 | 51 | for (unsigned int x1 = 1 + offset; x1 < harmonic->m[1] - 1; x1 += 2) { 52 | // If this is locked, then skip it. 53 | if (harmonic->locked[x0 * harmonic->m[1] + x1]) { 54 | continue; 55 | } 56 | 57 | float uPrevious = harmonic->u[x0 * harmonic->m[1] + x1]; 58 | 59 | // Update the value at this location with the log-sum-exp trick. 60 | float maxVal = EPIC_FLT_MIN; 61 | maxVal = std::max(harmonic->u[(x0 - 1) * harmonic->m[1] + x1], harmonic->u[(x0 + 1) * harmonic->m[1] + x1]); 62 | maxVal = std::max(maxVal, harmonic->u[x0 * harmonic->m[1] + (x1 - 1)]); 63 | maxVal = std::max(maxVal, harmonic->u[x0 * harmonic->m[1] + (x1 + 1)]); 64 | 65 | harmonic->u[x0 * harmonic->m[1] + x1] = maxVal + std::log( 66 | std::exp(harmonic->u[(x0 - 1) * harmonic->m[1] + x1] - maxVal) + 67 | std::exp(harmonic->u[(x0 + 1) * harmonic->m[1] + x1] - maxVal) + 68 | std::exp(harmonic->u[x0 * harmonic->m[1] + (x1 - 1)] - maxVal) + 69 | std::exp(harmonic->u[x0 * harmonic->m[1] + (x1 + 1)] - maxVal)) - 70 | std::log(2.0 * harmonic->n); 71 | 72 | // Compute the updated delta. 73 | if (checkConvergence) { 74 | harmonic->delta = std::max(harmonic->delta, (float)fabs(uPrevious - harmonic->u[x0 * harmonic->m[1] + x1])); 75 | } 76 | } 77 | } 78 | } 79 | 80 | 81 | void harmonic_update_3d_cpu(Harmonic *harmonic, bool checkConvergence) 82 | { 83 | if (checkConvergence) { 84 | harmonic->delta = 0.0; 85 | } 86 | 87 | // Iterate over all non-boundary cells and update its value based on a red-black ordering. 88 | // Thus, for all rows, we either skip by evens or odds in 2-dimensions. 89 | for (unsigned int x0 = 1; x0 < harmonic->m[0] - 1; x0++) { 90 | for (unsigned int x1 = 1; x1 < harmonic->m[1] - 1; x1++) { 91 | // Determine if this rows starts with a red (even row) or black (odd row) cell, and 92 | // update the opposite depending on how many iterations there have been. 93 | unsigned int offset = (unsigned int)((harmonic->currentIteration % 2) != (x0 % 2)); 94 | 95 | // We also negate the offset based on this dimension. 96 | if (x1 % 2 == 0) { 97 | offset = (unsigned int)(!(bool)offset); 98 | } 99 | 100 | for (unsigned int x2 = 1 + offset; x2 < harmonic->m[2] - 1; x2 += 2) { 101 | // If this is locked, then skip it. 102 | if (harmonic->locked[x0 * harmonic->m[1] * harmonic->m[2] + x1 * harmonic->m[2] + x2]) { 103 | continue; 104 | } 105 | 106 | float uPrevious = harmonic->u[x0 * harmonic->m[1] * harmonic->m[2] + x1 * harmonic->m[2] + x2]; 107 | 108 | // Update the value at this location with the log-sum-exp trick. 109 | float maxVal = EPIC_FLT_MIN; 110 | maxVal = std::max(harmonic->u[(x0 - 1) * harmonic->m[1] * harmonic->m[2] + x1 * harmonic->m[2] + x2], 111 | harmonic->u[(x0 + 1) * harmonic->m[1] * harmonic->m[2] + x1 * harmonic->m[2] + x2]); 112 | maxVal = std::max(maxVal, harmonic->u[x0 * harmonic->m[1] * harmonic->m[2] + (x1 - 1) * harmonic->m[2] + x2]); 113 | maxVal = std::max(maxVal, harmonic->u[x0 * harmonic->m[1] * harmonic->m[2] + (x1 + 1) * harmonic->m[2] + x2]); 114 | maxVal = std::max(maxVal, harmonic->u[x0 * harmonic->m[1] * harmonic->m[2] + x1 * harmonic->m[2] + (x2 - 1)]); 115 | maxVal = std::max(maxVal, harmonic->u[x0 * harmonic->m[1] * harmonic->m[2] + x1 * harmonic->m[2] + (x2 + 1)]); 116 | 117 | harmonic->u[x0 * harmonic->m[1] * harmonic->m[2] + x1 * harmonic->m[2] + x2] = maxVal + std::log( 118 | std::exp(harmonic->u[(x0 - 1) * harmonic->m[1] * harmonic->m[2] + x1 * harmonic->m[2] + x2] - maxVal) + 119 | std::exp(harmonic->u[(x0 + 1) * harmonic->m[1] * harmonic->m[2] + x1 * harmonic->m[2] + x2] - maxVal) + 120 | std::exp(harmonic->u[x0 * harmonic->m[1] * harmonic->m[2] + (x1 - 1) * harmonic->m[2] + x2] - maxVal) + 121 | std::exp(harmonic->u[x0 * harmonic->m[1] * harmonic->m[2] + (x1 + 1) * harmonic->m[2] + x2] - maxVal) + 122 | std::exp(harmonic->u[x0 * harmonic->m[1] * harmonic->m[2] + x1 * harmonic->m[2] + (x2 - 1)] - maxVal) + 123 | std::exp(harmonic->u[x0 * harmonic->m[1] * harmonic->m[2] + x1 * harmonic->m[2] + (x2 + 1)] - maxVal)) - 124 | std::log(2.0 * harmonic->n); 125 | 126 | // Compute the updated delta. 127 | if (checkConvergence) { 128 | harmonic->delta = std::max(harmonic->delta, (float)fabs(uPrevious - harmonic->u[x0 * harmonic->m[1] * harmonic->m[2] + x1 * harmonic->m[2] + x2])); 129 | } 130 | } 131 | } 132 | } 133 | } 134 | 135 | 136 | int harmonic_complete_cpu(Harmonic *harmonic) 137 | { 138 | int result; 139 | 140 | // Ensure data is valid before we begin. 141 | if (harmonic == nullptr || harmonic->m == nullptr || harmonic->u == nullptr || 142 | harmonic->locked == nullptr || harmonic->epsilon <= 0.0) { 143 | fprintf(stderr, "Error[harmonic_complete_cpu]: %s\n", "Invalid data."); 144 | return EPIC_ERROR_INVALID_DATA; 145 | } 146 | 147 | // Make sure 'information' can at least be propagated throughout the entire grid. 148 | unsigned int mMax = 0; 149 | for (unsigned int i = 0; i < harmonic->n; i++) { 150 | mMax = std::max(mMax, harmonic->m[i]); 151 | } 152 | 153 | harmonic->currentIteration = 0; 154 | harmonic->delta = harmonic->epsilon + 1.0; 155 | 156 | result = EPIC_SUCCESS; 157 | 158 | while (result != EPIC_SUCCESS_AND_CONVERGED || harmonic->currentIteration < mMax) { 159 | if (harmonic->currentIteration % harmonic->numIterationsToStaggerCheck == 0) { 160 | result = harmonic_update_and_check_cpu(harmonic); 161 | if (result != EPIC_SUCCESS && result != EPIC_SUCCESS_AND_CONVERGED) { 162 | fprintf(stderr, "Error[harmonic_complete_cpu]: %s\n", 163 | "Failed to perform the Gauss-Seidel update (and check) step."); 164 | return result; 165 | } 166 | } else { 167 | result = harmonic_update_cpu(harmonic); 168 | if (result != EPIC_SUCCESS) { 169 | fprintf(stderr, "Error[harmonic_complete_cpu]: %s\n", 170 | "Failed to perform the Gauss-Seidel update step."); 171 | return result; 172 | } 173 | } 174 | 175 | /* *** DEBUG *** 176 | if (harmonic->currentIteration % 100 == 0) { 177 | printf("Iteration %i --- %e\n", harmonic->currentIteration, delta); 178 | fflush(stdout); 179 | } 180 | //*/ 181 | } 182 | 183 | return EPIC_SUCCESS; 184 | } 185 | 186 | 187 | int harmonic_update_cpu(Harmonic *harmonic) 188 | { 189 | if (harmonic->n == 2) { 190 | harmonic_update_2d_cpu(harmonic, false); 191 | } else if (harmonic->n == 3) { 192 | harmonic_update_3d_cpu(harmonic, false); 193 | } else if (harmonic->n == 4) { 194 | //harmonic_update_4d_cpu(harmonic, false); 195 | } 196 | 197 | harmonic->currentIteration++; 198 | 199 | return EPIC_SUCCESS; 200 | } 201 | 202 | 203 | int harmonic_update_and_check_cpu(Harmonic *harmonic) 204 | { 205 | if (harmonic->n == 2) { 206 | harmonic_update_2d_cpu(harmonic, true); 207 | } else if (harmonic->n == 3) { 208 | harmonic_update_3d_cpu(harmonic, true); 209 | } else if (harmonic->n == 4) { 210 | //harmonic_update_4d_cpu(harmonic, true); 211 | } 212 | 213 | harmonic->currentIteration++; 214 | 215 | if (harmonic->delta < harmonic->epsilon) { 216 | return EPIC_SUCCESS_AND_CONVERGED; 217 | } else { 218 | return EPIC_SUCCESS; 219 | } 220 | } 221 | 222 | }; // namespace epic 223 | 224 | -------------------------------------------------------------------------------- /libepic/src/harmonic/harmonic_legacy_cpu.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * The MIT License (MIT) 3 | * 4 | * Copyright (c) 2016 Kyle Hollins Wray, University of Massachusetts 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | * this software and associated documentation files (the "Software"), to deal in 8 | * the Software without restriction, including without limitation the rights to 9 | * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | * the Software, and to permit persons to whom the Software is furnished to do so, 11 | * subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in all 14 | * copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | #include 30 | #include 31 | 32 | namespace epic { 33 | 34 | #define MIN_ITERATIONS 10000 35 | 36 | int harmonic_legacy_sor_2d_float_cpu(unsigned int w, unsigned int h, float epsilon, 37 | float omega, unsigned int *locked, float *u, unsigned int &iter) 38 | { 39 | float delta = epsilon + 1.0f; 40 | iter = 0; 41 | 42 | while (delta >= epsilon || iter < MIN_ITERATIONS) { 43 | delta = 0.0f; 44 | 45 | for (unsigned int y = 1; y < h - 1; y++) { 46 | for (unsigned int x = 1; x < w - 1; x++) { 47 | if (locked[y * w + x] == 1) { 48 | continue; 49 | } 50 | 51 | float uPrev = u[y * w + x]; 52 | 53 | u[y * w + x] = (1.0f - omega) * u[y * w + x] + 54 | omega / 4.0f * (u[(y - 1) * w + x] + 55 | u[(y + 1) * w + x] + 56 | u[y * w + (x - 1)] + 57 | u[y * w + (x + 1)]); 58 | 59 | delta = std::fmax(delta, std::fabs(u[y * w + x] - uPrev)); 60 | } 61 | } 62 | 63 | //printf("Iteration: %i and Delta = %.3f vs Epsilon = %.3f\n", iter, delta, epsilon); 64 | 65 | iter++; 66 | } 67 | 68 | return EPIC_SUCCESS; 69 | } 70 | 71 | 72 | int harmonic_legacy_sor_2d_double_cpu(unsigned int w, unsigned int h, double epsilon, 73 | double omega, unsigned int *locked, double *u, unsigned int &iter) 74 | { 75 | double delta = epsilon + 1.0; 76 | iter = 0; 77 | 78 | while (delta >= epsilon || iter < MIN_ITERATIONS) { 79 | delta = 0.0; 80 | 81 | for (unsigned int y = 1; y < h - 1; y++) { 82 | for (unsigned int x = 1; x < w - 1; x++) { 83 | if (locked[y * w + x] == 1) { 84 | continue; 85 | } 86 | 87 | double uPrev = u[y * w + x]; 88 | 89 | u[y * w + x] = (1.0 - omega) * u[y * w + x] + 90 | omega / 4.0 * (u[(y - 1) * w + x] + 91 | u[(y + 1) * w + x] + 92 | u[y * w + (x - 1)] + 93 | u[y * w + (x + 1)]); 94 | 95 | delta = std::fmax(delta, std::fabs(u[y * w + x] - uPrev)); 96 | } 97 | } 98 | 99 | //printf("Iteration: %i and Delta = %.3f vs Epsilon = %.3f\n", iter, delta, epsilon); 100 | 101 | iter++; 102 | } 103 | 104 | return EPIC_SUCCESS; 105 | } 106 | 107 | 108 | int harmonic_legacy_sor_2d_long_double_cpu(unsigned int w, unsigned int h, long double epsilon, 109 | long double omega, unsigned int *locked, long double *u, unsigned int &iter) 110 | { 111 | long double delta = epsilon + 1.0; 112 | iter = 0; 113 | 114 | while (delta >= epsilon || iter < MIN_ITERATIONS) { 115 | delta = 0.0; 116 | 117 | for (unsigned int y = 1; y < h - 1; y++) { 118 | for (unsigned int x = 1; x < w - 1; x++) { 119 | if (locked[y * w + x] == 1) { 120 | continue; 121 | } 122 | 123 | long double uPrev = u[y * w + x]; 124 | 125 | u[y * w + x] = (1.0 - omega) * u[y * w + x] + 126 | omega / 4.0 * (u[(y - 1) * w + x] + 127 | u[(y + 1) * w + x] + 128 | u[y * w + (x - 1)] + 129 | u[y * w + (x + 1)]); 130 | 131 | delta = std::fmax(delta, std::fabs(u[y * w + x] - uPrev)); 132 | } 133 | } 134 | 135 | //printf("Iteration: %i and Delta = %.3f vs Epsilon = %.3f\n", iter, delta, epsilon); 136 | 137 | iter++; 138 | } 139 | 140 | return EPIC_SUCCESS; 141 | } 142 | 143 | }; // namespace epic 144 | 145 | -------------------------------------------------------------------------------- /libepic/src/harmonic/harmonic_legacy_path_cpu.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * The MIT License (MIT) 3 | * 4 | * Copyright (c) 2015 Kyle Hollins Wray, University of Massachusetts 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | * this software and associated documentation files (the "Software"), to deal in 8 | * the Software without restriction, including without limitation the rights to 9 | * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | * the Software, and to permit persons to whom the Software is furnished to do so, 11 | * subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in all 14 | * copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | 37 | namespace epic { 38 | 39 | #define PATH_STUCK_HISTORY_LENGTH 5 40 | 41 | int harmonic_legacy_compute_potential_2d_cpu(unsigned int w, unsigned int h, unsigned int *locked, double *u, double x, double y, double &potential) 42 | { 43 | if (w == 0 || h == 0 || locked == nullptr || u == nullptr) { 44 | fprintf(stderr, "Error[harmonic_legacy_compute_potential_2d_cpu]: %s\n", "Invalid data."); 45 | return EPIC_ERROR_INVALID_DATA; 46 | } 47 | 48 | unsigned int xCellIndex = (unsigned int)(x + 0.5); 49 | unsigned int yCellIndex = (unsigned int)(y + 0.5); 50 | 51 | if (xCellIndex < 0.0 || yCellIndex < 0.0 || 52 | xCellIndex >= w || yCellIndex >= h || 53 | (locked[yCellIndex * w + xCellIndex] == 1 && 54 | u[yCellIndex * w + xCellIndex] < 0.0)) { 55 | fprintf(stderr, "Error[harmonic_legacy_compute_potential_2d_cpu]: %s\n", "Invalid location."); 56 | return EPIC_ERROR_INVALID_LOCATION; 57 | } 58 | 59 | unsigned int xtl = (unsigned int)(x - 0.5); 60 | unsigned int ytl = (unsigned int)(y - 0.5); 61 | 62 | unsigned int xtr = (unsigned int)(x + 0.5); 63 | unsigned int ytr = (unsigned int)(y - 0.5); 64 | 65 | unsigned int xbl = (unsigned int)(x - 0.5); 66 | unsigned int ybl = (unsigned int)(y + 0.5); 67 | 68 | unsigned int xbr = (unsigned int)(x + 0.5); 69 | unsigned int ybr = (unsigned int)(y + 0.5); 70 | 71 | double alpha = (x - xtl); 72 | double beta = (y - ytl); 73 | 74 | double one = (1.0 - alpha) * u[ytl * w + xtl] + alpha * u[ytr * w + xtr]; 75 | double two = (1.0 - alpha) * u[ybl * w + xbl] + alpha * u[ybr * w + xbr]; 76 | potential = (1.0f - beta) * one + beta * two; 77 | 78 | return EPIC_SUCCESS; 79 | } 80 | 81 | 82 | int harmonic_legacy_compute_gradient_2d_cpu(unsigned int w, unsigned int h, unsigned int *locked, double *u, 83 | double x, double y, double cdPrecision, double &partialX, double &partialY) 84 | { 85 | if (w == 0 || h == 0 || u == nullptr || locked == nullptr) { 86 | fprintf(stderr, "Error[harmonic_legacy_compute_gradient_2d_cpu]: %s\n", "Invalid data."); 87 | return EPIC_ERROR_INVALID_DATA; 88 | } 89 | 90 | double value0 = 0.0; 91 | double value1 = 0.0; 92 | double value2 = 0.0; 93 | double value3 = 0.0; 94 | 95 | int result = harmonic_legacy_compute_potential_2d_cpu(w, h, locked, u, x - cdPrecision, y, value0); 96 | result += harmonic_legacy_compute_potential_2d_cpu(w, h, locked, u, x + cdPrecision, y, value1); 97 | result += harmonic_legacy_compute_potential_2d_cpu(w, h, locked, u, x, y - cdPrecision, value2); 98 | result += harmonic_legacy_compute_potential_2d_cpu(w, h, locked, u, x, y + cdPrecision, value3); 99 | 100 | if (result != EPIC_SUCCESS) { 101 | fprintf(stderr, "Error[harmonic_legacy_compute_gradient_2d_cpu]: %s\n", 102 | "Failed to compute potential values."); 103 | return EPIC_ERROR_INVALID_GRADIENT; 104 | } 105 | 106 | partialX = (value1 - value0) / (2.0 * cdPrecision); 107 | partialY = (value3 - value2) / (2.0 * cdPrecision); 108 | 109 | double denom = std::sqrt(std::pow(partialX, 2) + std::pow(partialY, 2)); 110 | partialX /= denom; 111 | partialY /= denom; 112 | 113 | return EPIC_SUCCESS; 114 | } 115 | 116 | 117 | bool harmonic_legacy_is_path_stuck_cpu(const std::vector &pathVector, double stepSize) 118 | { 119 | unsigned int n = pathVector.size(); 120 | 121 | // There is an error (it is stuck or something) if the path vector is odd. 122 | if (n % 2 == 1) { 123 | return true; 124 | } 125 | 126 | // Empty paths are fine. 127 | if (n == 0) { 128 | return false; 129 | } 130 | 131 | double x = pathVector[n - 2]; 132 | double y = pathVector[n - 1]; 133 | 134 | // If the path reached a point where it backtracked to a recently visited region, then there is a problem. 135 | for (unsigned int i = n - 2; i > std::max(0, (int)n - 2 * PATH_STUCK_HISTORY_LENGTH - 2); i -= 2) { 136 | double xi = pathVector[i - 2]; 137 | double yi = pathVector[i - 1]; 138 | 139 | double distance = std::sqrt(std::pow(x - xi, 2) + std::pow(y - yi, 2)); 140 | 141 | if (distance < stepSize / 2.0) { 142 | return true; 143 | } 144 | } 145 | 146 | return false; 147 | } 148 | 149 | 150 | int harmonic_legacy_compute_path_2d_cpu(unsigned int w, unsigned int h, unsigned int *locked, double *u, 151 | double x, double y, double stepSize, double cdPrecision, unsigned int maxLength, int flipped, 152 | unsigned int &k, double *&path) 153 | { 154 | if (w == 0 || h == 0 || locked == nullptr || u == nullptr || path != nullptr) { 155 | fprintf(stderr, "Error[harmonic_legacy_compute_path_2d_cpu]: %s\n", "Invalid data."); 156 | return EPIC_ERROR_INVALID_DATA; 157 | } 158 | 159 | unsigned int xCellIndex = (unsigned int)(x + 0.5); 160 | unsigned int yCellIndex = (unsigned int)(y + 0.5); 161 | 162 | if (xCellIndex < 0.0 || yCellIndex < 0.0 || 163 | xCellIndex >= w || yCellIndex >= h || 164 | (locked[yCellIndex * w + xCellIndex] == 1 && 165 | ((flipped == 0 && u[yCellIndex * w + xCellIndex] == 1.0) || 166 | (flipped == 1 && u[yCellIndex * w + xCellIndex] == 0.0)))) { 167 | fprintf(stderr, "Error[harmonic_legacy_compute_path_2d_cpu]: %s\n", "Invalid location."); 168 | return EPIC_ERROR_INVALID_LOCATION; 169 | } 170 | 171 | std::vector pathVector; 172 | 173 | pathVector.push_back(x); 174 | pathVector.push_back(y); 175 | //pathVector.push_back(theta); 176 | 177 | while (locked[yCellIndex * w + xCellIndex] != 1 && 178 | harmonic_legacy_is_path_stuck_cpu(pathVector, stepSize) == false && 179 | pathVector.size() < maxLength) { 180 | double partialX = 0.0; 181 | double partialY = 0.0; 182 | 183 | int result = harmonic_legacy_compute_gradient_2d_cpu(w, h, locked, u, x, y, cdPrecision, partialX, partialY); 184 | if (result != EPIC_SUCCESS) { 185 | fprintf(stderr, "Error[harmonic_legacy_compute_path_2d_cpu]: %s\n", "Could not compute gradient."); 186 | return EPIC_ERROR_INVALID_GRADIENT; 187 | } 188 | 189 | if (flipped == 1) { 190 | x += partialX * stepSize; 191 | y += partialY * stepSize; 192 | } else { 193 | x -= partialX * stepSize; 194 | y -= partialY * stepSize; 195 | } 196 | 197 | pathVector.push_back(x); 198 | pathVector.push_back(y); 199 | 200 | //double theta = std::atan2(partialY, partialX); 201 | //pathVector.push_back(theta); 202 | 203 | xCellIndex = (unsigned int)(x + 0.5); 204 | yCellIndex = (unsigned int)(y + 0.5); 205 | } 206 | 207 | // If there are only two points, then there's a 99.999% the gradient was invalid and it 208 | // is not done relaxing enough. Thus, return a special error. 209 | if (pathVector.size() / 2 <= 2) { 210 | fprintf(stderr, "Error[harmonic_legacy_compute_path_2d_cpu]: %s\n", "Could not compute a valid path."); 211 | return EPIC_ERROR_INVALID_PATH; 212 | } 213 | 214 | k = pathVector.size() / 2; 215 | path = new double[2 * k]; 216 | for (unsigned int i = 0; i < 2 * k; i++) { 217 | path[i] = pathVector[i]; 218 | } 219 | 220 | return EPIC_SUCCESS; 221 | } 222 | 223 | 224 | int harmonic_legacy_free_path_cpu(double *&path) 225 | { 226 | if (path != nullptr) { 227 | delete [] path; 228 | } 229 | path = nullptr; 230 | 231 | return EPIC_SUCCESS; 232 | } 233 | 234 | }; // namespace epic 235 | 236 | -------------------------------------------------------------------------------- /libepic/src/harmonic/harmonic_model_gpu.cu: -------------------------------------------------------------------------------- 1 | /** 2 | * The MIT License (MIT) 3 | * 4 | * Copyright (c) 2015 Kyle Hollins Wray, University of Massachusetts 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | * this software and associated documentation files (the "Software"), to deal in 8 | * the Software without restriction, including without limitation the rights to 9 | * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | * the Software, and to permit persons to whom the Software is furnished to do so, 11 | * subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in all 14 | * copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | #include 30 | 31 | 32 | namespace epic { 33 | 34 | int harmonic_initialize_dimension_size_gpu(Harmonic *harmonic) 35 | { 36 | // Ensure the data is valid. 37 | if (harmonic->n == 0 || harmonic->m == nullptr) { 38 | fprintf(stderr, "Error[harmonic_initialize_dimension_size_gpu]: %s\n", "Invalid input."); 39 | return EPIC_ERROR_INVALID_DATA; 40 | } 41 | 42 | // Allocate the memory on the device. 43 | if (cudaMalloc(&harmonic->d_m, harmonic->n * sizeof(unsigned int)) != cudaSuccess) { 44 | fprintf(stderr, "Error[harmonic_initialize_dimension_size_gpu]: %s\n", 45 | "Failed to allocate device-side memory for the dimension size."); 46 | return EPIC_ERROR_DEVICE_MALLOC; 47 | } 48 | 49 | // Copy the data from the host to the device. 50 | if (cudaMemcpy(harmonic->d_m, harmonic->m, harmonic->n * sizeof(unsigned int), 51 | cudaMemcpyHostToDevice) != cudaSuccess) { 52 | fprintf(stderr, "Error[harmonic_initialize_dimension_size_gpu]: %s\n", 53 | "Failed to copy memory from host to device for the dimension size."); 54 | return EPIC_ERROR_MEMCPY_TO_DEVICE; 55 | } 56 | 57 | return EPIC_SUCCESS; 58 | } 59 | 60 | 61 | int harmonic_uninitialize_dimension_size_gpu(Harmonic *harmonic) 62 | { 63 | if (harmonic->d_m != nullptr) { 64 | if (cudaFree(harmonic->d_m) != cudaSuccess) { 65 | fprintf(stderr, "Error[harmonic_uninitialize_dimension_size_gpu]: %s\n", 66 | "Failed to free device-side memory for the dimension size."); 67 | return EPIC_ERROR_DEVICE_FREE; 68 | } 69 | } 70 | harmonic->d_m = nullptr; 71 | 72 | return EPIC_SUCCESS; 73 | } 74 | 75 | 76 | int harmonic_initialize_potential_values_gpu(Harmonic *harmonic) 77 | { 78 | // Ensure the data is valid. 79 | if (harmonic->n == 0 || harmonic->m == nullptr || harmonic->u == nullptr) { 80 | fprintf(stderr, "Error[harmonic_initialize_potential_values_gpu]: %s\n", "Invalid input."); 81 | return EPIC_ERROR_INVALID_DATA; 82 | } 83 | 84 | // Compute the number of cells. 85 | unsigned int numCells = 1; 86 | for (unsigned int i = 0; i < harmonic->n; i++) { 87 | numCells *= harmonic->m[i]; 88 | } 89 | 90 | // Allocate the memory on the device. 91 | if (cudaMalloc(&harmonic->d_u, numCells * sizeof(float)) != cudaSuccess) { 92 | fprintf(stderr, "Error[harmonic_initialize_potential_values_gpu]: %s\n", 93 | "Failed to allocate device-side memory for the potential values."); 94 | return EPIC_ERROR_DEVICE_MALLOC; 95 | } 96 | 97 | // Copy the data from the host to the device. 98 | if (cudaMemcpy(harmonic->d_u, harmonic->u, numCells * sizeof(float), 99 | cudaMemcpyHostToDevice) != cudaSuccess) { 100 | fprintf(stderr, "Error[harmonic_initialize_potential_values_gpu]: %s\n", 101 | "Failed to copy memory from host to device for the potential values."); 102 | return EPIC_ERROR_MEMCPY_TO_DEVICE; 103 | } 104 | 105 | return EPIC_SUCCESS; 106 | } 107 | 108 | 109 | int harmonic_uninitialize_potential_values_gpu(Harmonic *harmonic) 110 | { 111 | if (harmonic->d_u != nullptr) { 112 | if (cudaFree(harmonic->d_u) != cudaSuccess) { 113 | fprintf(stderr, "Error[harmonic_uninitialize_potential_values_gpu]: %s\n", 114 | "Failed to free device-side memory for the potential values."); 115 | return EPIC_ERROR_DEVICE_FREE; 116 | } 117 | } 118 | harmonic->d_u = nullptr; 119 | 120 | return EPIC_SUCCESS; 121 | } 122 | 123 | 124 | int harmonic_initialize_locked_gpu(Harmonic *harmonic) 125 | { 126 | // Ensure the data is valid. 127 | if (harmonic->n == 0 || harmonic->m == nullptr || harmonic->locked == nullptr) { 128 | fprintf(stderr, "Error[harmonic_initialize_locked_gpu]: %s\n", "Invalid input."); 129 | return EPIC_ERROR_INVALID_DATA; 130 | } 131 | 132 | // Compute the number of cells. 133 | unsigned int numCells = 1; 134 | for (unsigned int i = 0; i < harmonic->n; i++) { 135 | numCells *= harmonic->m[i]; 136 | } 137 | 138 | // Allocate the memory on the device. 139 | if (cudaMalloc(&harmonic->d_locked, numCells * sizeof(unsigned int)) != cudaSuccess) { 140 | fprintf(stderr, "Error[harmonic_initialize_locked_gpu]: %s\n", 141 | "Failed to allocate device-side memory for the locked cells."); 142 | return EPIC_ERROR_DEVICE_MALLOC; 143 | } 144 | 145 | // Copy the data from the host to the device. 146 | if (cudaMemcpy(harmonic->d_locked, harmonic->locked, numCells * sizeof(unsigned int), 147 | cudaMemcpyHostToDevice) != cudaSuccess) { 148 | fprintf(stderr, "Error[harmonic_initialize_locked_gpu]: %s\n", 149 | "Failed to copy memory from host to device for the locked cells."); 150 | return EPIC_ERROR_MEMCPY_TO_DEVICE; 151 | } 152 | 153 | return EPIC_SUCCESS; 154 | } 155 | 156 | 157 | int harmonic_uninitialize_locked_gpu(Harmonic *harmonic) 158 | { 159 | if (harmonic->d_locked != nullptr) { 160 | if (cudaFree(harmonic->d_locked) != cudaSuccess) { 161 | fprintf(stderr, "Error[harmonic_uninitialize_locked_gpu]: %s\n", 162 | "Failed to free device-side memory for the locked cells."); 163 | return EPIC_ERROR_DEVICE_FREE; 164 | } 165 | } 166 | harmonic->d_locked = nullptr; 167 | 168 | return EPIC_SUCCESS; 169 | } 170 | 171 | 172 | int harmonic_update_model_gpu(Harmonic *harmonic) 173 | { 174 | if (harmonic->n == 0 || harmonic->m == nullptr || 175 | harmonic->u == nullptr || harmonic->d_u == nullptr || 176 | harmonic->locked == nullptr || harmonic->d_locked == nullptr) { 177 | fprintf(stderr, "Error[harmonic_update_model_gpu]: %s\n", "Invalid data."); 178 | return EPIC_ERROR_INVALID_DATA; 179 | } 180 | 181 | // Compute the number of cells. 182 | unsigned int numCells = 1; 183 | for (unsigned int i = 0; i < harmonic->n; i++) { 184 | numCells *= harmonic->m[i]; 185 | } 186 | 187 | // Copy the data from the host to the device. 188 | if (cudaMemcpy(harmonic->d_u, harmonic->u, numCells * sizeof(float), 189 | cudaMemcpyHostToDevice) != cudaSuccess) { 190 | fprintf(stderr, "Error[harmonic_update_model_gpu]: %s\n", 191 | "Failed to copy memory from host to device for the potential values."); 192 | return EPIC_ERROR_MEMCPY_TO_DEVICE; 193 | } 194 | 195 | // Copy the data from the host to the device. 196 | if (cudaMemcpy(harmonic->d_locked, harmonic->locked, numCells * sizeof(unsigned int), 197 | cudaMemcpyHostToDevice) != cudaSuccess) { 198 | fprintf(stderr, "Error[harmonic_update_model_gpu]: %s\n", 199 | "Failed to copy memory from host to device for the locked cells."); 200 | return EPIC_ERROR_MEMCPY_TO_DEVICE; 201 | } 202 | 203 | return EPIC_SUCCESS; 204 | } 205 | 206 | }; // namespace epic 207 | 208 | -------------------------------------------------------------------------------- /libepic/src/harmonic/harmonic_path_cpu.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * The MIT License (MIT) 3 | * 4 | * Copyright (c) 2015 Kyle Hollins Wray, University of Massachusetts 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | * this software and associated documentation files (the "Software"), to deal in 8 | * the Software without restriction, including without limitation the rights to 9 | * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | * the Software, and to permit persons to whom the Software is furnished to do so, 11 | * subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in all 14 | * copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | 37 | namespace epic { 38 | 39 | #define PATH_STUCK_HISTORY_LENGTH 5 40 | 41 | int harmonic_compute_potential_2d_cpu(Harmonic *harmonic, float x, float y, float &potential) 42 | { 43 | if (harmonic == nullptr || harmonic->m == nullptr || 44 | harmonic->u == nullptr || harmonic->locked == nullptr) { 45 | fprintf(stderr, "Error[harmonic_compute_potential_2d_cpu]: %s\n", "Invalid data."); 46 | return EPIC_ERROR_INVALID_DATA; 47 | } 48 | 49 | unsigned int xCellIndex = (unsigned int)(x + 0.5f); 50 | unsigned int yCellIndex = (unsigned int)(y + 0.5f); 51 | 52 | if (xCellIndex < 0.0f || yCellIndex < 0.0f || 53 | xCellIndex >= harmonic->m[1] || yCellIndex >= harmonic->m[0] || 54 | (harmonic->locked[yCellIndex * harmonic->m[1] + xCellIndex] == 1 && 55 | harmonic->u[yCellIndex * harmonic->m[1] + xCellIndex] < 0.0f)) { 56 | fprintf(stderr, "Error[harmonic_compute_potential_2d_cpu]: %s\n", "Invalid location."); 57 | return EPIC_ERROR_INVALID_LOCATION; 58 | } 59 | 60 | unsigned int xtl = (unsigned int)(x - 0.5f); 61 | unsigned int ytl = (unsigned int)(y - 0.5f); 62 | 63 | unsigned int xtr = (unsigned int)(x + 0.5f); 64 | unsigned int ytr = (unsigned int)(y - 0.5f); 65 | 66 | unsigned int xbl = (unsigned int)(x - 0.5f); 67 | unsigned int ybl = (unsigned int)(y + 0.5f); 68 | 69 | unsigned int xbr = (unsigned int)(x + 0.5f); 70 | unsigned int ybr = (unsigned int)(y + 0.5f); 71 | 72 | float alpha = (x - xtl); 73 | float beta = (y - ytl); 74 | 75 | float one = (1.0f - alpha) * harmonic->u[ytl * harmonic->m[1] + xtl] + 76 | alpha * harmonic->u[ytr * harmonic->m[1] + xtr]; 77 | float two = (1.0f - alpha) * harmonic->u[ybl * harmonic->m[1] + xbl] + 78 | alpha * harmonic->u[ybr * harmonic->m[1] + xbr]; 79 | potential = (1.0f - beta) * one + beta * two; 80 | 81 | return EPIC_SUCCESS; 82 | } 83 | 84 | 85 | int harmonic_compute_gradient_2d_cpu(Harmonic *harmonic, float x, float y, float cdPrecision, 86 | float &partialX, float &partialY) 87 | { 88 | if (harmonic == nullptr || harmonic->m == nullptr || 89 | harmonic->u == nullptr || harmonic->locked == nullptr) { 90 | fprintf(stderr, "Error[harmonic_compute_gradient_2d_cpu]: %s\n", "Invalid data."); 91 | return EPIC_ERROR_INVALID_DATA; 92 | } 93 | 94 | float value0 = 0.0f; 95 | float value1 = 0.0f; 96 | float value2 = 0.0f; 97 | float value3 = 0.0f; 98 | 99 | int result = harmonic_compute_potential_2d_cpu(harmonic, x - cdPrecision, y, value0); 100 | result += harmonic_compute_potential_2d_cpu(harmonic, x + cdPrecision, y, value1); 101 | result += harmonic_compute_potential_2d_cpu(harmonic, x, y - cdPrecision, value2); 102 | result += harmonic_compute_potential_2d_cpu(harmonic, x, y + cdPrecision, value3); 103 | 104 | if (result != EPIC_SUCCESS) { 105 | fprintf(stderr, "Error[harmonic_compute_gradient_2d_cpu]: %s\n", 106 | "Failed to compute potential values."); 107 | return EPIC_ERROR_INVALID_GRADIENT; 108 | } 109 | 110 | partialX = (value1 - value0) / (2.0f * cdPrecision); 111 | partialY = (value3 - value2) / (2.0f * cdPrecision); 112 | 113 | float denom = std::sqrt(std::pow(partialX, 2) + std::pow(partialY, 2)); 114 | partialX /= denom; 115 | partialY /= denom; 116 | 117 | return EPIC_SUCCESS; 118 | } 119 | 120 | 121 | bool harmonic_is_path_stuck_cpu(const std::vector &pathVector, float stepSize) 122 | { 123 | unsigned int n = pathVector.size(); 124 | 125 | // There is an error (it is stuck or something) if the path vector is odd. 126 | if (n % 2 == 1) { 127 | return true; 128 | } 129 | 130 | // Empty paths are fine. 131 | if (n == 0) { 132 | return false; 133 | } 134 | 135 | float x = pathVector[n - 2]; 136 | float y = pathVector[n - 1]; 137 | 138 | // If the path reached a point where it backtracked to a recently visited region, then there is a problem. 139 | for (unsigned int i = n - 2; i > std::max(0, (int)n - 2 * PATH_STUCK_HISTORY_LENGTH - 2); i -= 2) { 140 | float xi = pathVector[i - 2]; 141 | float yi = pathVector[i - 1]; 142 | 143 | float distance = std::sqrt(std::pow(x - xi, 2) + std::pow(y - yi, 2)); 144 | 145 | if (distance < stepSize / 2.0f) { 146 | return true; 147 | } 148 | } 149 | 150 | return false; 151 | } 152 | 153 | 154 | int harmonic_compute_path_2d_cpu(Harmonic *harmonic, float x, float y, 155 | float stepSize, float cdPrecision, unsigned int maxLength, 156 | unsigned int &k, float *&path) 157 | { 158 | if (harmonic == nullptr || harmonic->m == nullptr || 159 | harmonic->u == nullptr || harmonic->locked == nullptr || 160 | path != nullptr) { 161 | fprintf(stderr, "Error[harmonic_compute_path_2d_cpu]: %s\n", "Invalid data."); 162 | return EPIC_ERROR_INVALID_DATA; 163 | } 164 | 165 | unsigned int xCellIndex = (unsigned int)(x + 0.5f); 166 | unsigned int yCellIndex = (unsigned int)(y + 0.5f); 167 | 168 | if (xCellIndex < 0.0f || yCellIndex < 0.0f || 169 | xCellIndex >= harmonic->m[1] || yCellIndex >= harmonic->m[0] || 170 | (harmonic->locked[yCellIndex * harmonic->m[1] + xCellIndex] == 1 && 171 | harmonic->u[yCellIndex * harmonic->m[1] + xCellIndex] < 0.0f)) { 172 | fprintf(stderr, "Error[harmonic_compute_path_2d_cpu]: %s\n", "Invalid location."); 173 | return EPIC_ERROR_INVALID_LOCATION; 174 | } 175 | 176 | std::vector pathVector; 177 | 178 | pathVector.push_back(x); 179 | pathVector.push_back(y); 180 | //pathVector.push_back(theta); 181 | 182 | while (harmonic->locked[yCellIndex * harmonic->m[1] + xCellIndex] != 1 && 183 | harmonic_is_path_stuck_cpu(pathVector, stepSize) == false && 184 | pathVector.size() < 2 * maxLength) { 185 | float partialX = 0.0f; 186 | float partialY = 0.0f; 187 | 188 | int result = harmonic_compute_gradient_2d_cpu(harmonic, x, y, cdPrecision, partialX, partialY); 189 | if (result != EPIC_SUCCESS) { 190 | fprintf(stderr, "Error[harmonic_compute_path_2d_cpu]: %s\n", "Could not compute gradient."); 191 | return EPIC_ERROR_INVALID_GRADIENT; 192 | } 193 | 194 | x += partialX * stepSize; 195 | y += partialY * stepSize; 196 | 197 | pathVector.push_back(x); 198 | pathVector.push_back(y); 199 | 200 | //float theta = std::atan2(partialY, partialX); 201 | //pathVector.push_back(theta); 202 | 203 | xCellIndex = (unsigned int)(x + 0.5f); 204 | yCellIndex = (unsigned int)(y + 0.5f); 205 | } 206 | 207 | // If there are only two points, then there's a 99.999% the gradient was invalid and it 208 | // is not done relaxing enough. Thus, return a special error. 209 | if (pathVector.size() / 2 <= 2) { 210 | fprintf(stderr, "Error[harmonic_compute_path_2d_cpu]: %s\n", "Could not compute a valid path."); 211 | return EPIC_ERROR_INVALID_PATH; 212 | } 213 | 214 | k = pathVector.size() / 2; 215 | path = new float[2 * k]; 216 | for (unsigned int i = 0; i < 2 * k; i++) { 217 | path[i] = pathVector[i]; 218 | } 219 | 220 | return EPIC_SUCCESS; 221 | } 222 | 223 | 224 | int harmonic_free_path_cpu(float *&path) 225 | { 226 | if (path != nullptr) { 227 | delete [] path; 228 | } 229 | path = nullptr; 230 | 231 | return EPIC_SUCCESS; 232 | } 233 | 234 | }; // namespace epic 235 | 236 | -------------------------------------------------------------------------------- /libepic/src/harmonic/harmonic_utilities_cpu.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * The MIT License (MIT) 3 | * 4 | * Copyright (c) 2015 Kyle Hollins Wray, University of Massachusetts 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | * this software and associated documentation files (the "Software"), to deal in 8 | * the Software without restriction, including without limitation the rights to 9 | * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | * the Software, and to permit persons to whom the Software is furnished to do so, 11 | * subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in all 14 | * copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | 36 | namespace epic { 37 | 38 | int harmonic_utilities_set_cells_2d_cpu(Harmonic *harmonic, unsigned int k, unsigned int *v, unsigned int *types) 39 | { 40 | if (harmonic == nullptr || harmonic->n == 0 || harmonic->m == nullptr || 41 | harmonic->u == nullptr || harmonic->locked == nullptr || 42 | k == 0 || v == nullptr || types == nullptr) { 43 | fprintf(stderr, "Error[harmonic_utilities_set_cells_2d_cpu]: %s\n", "Invalid data."); 44 | return EPIC_ERROR_INVALID_DATA; 45 | } 46 | 47 | for (unsigned int i = 0; i < k; i++) { 48 | unsigned int x = v[i * 2 + 0]; 49 | unsigned int y = v[i * 2 + 1]; 50 | 51 | if (y >= harmonic->m[0] || x >= harmonic->m[1]) { 52 | fprintf(stderr, "Warning[harmonic_utilities_set_cells_2d_cpu]: %s\n", 53 | "Provided vector has invalid values outside area."); 54 | //return EPIC_ERROR_INVALID_LOCATION; 55 | continue; 56 | } 57 | 58 | if (types[i] == EPIC_CELL_TYPE_GOAL) { 59 | harmonic->u[y * harmonic->m[1] + x] = EPIC_LOG_SPACE_GOAL; 60 | harmonic->locked[y * harmonic->m[1] + x] = 1; 61 | } else if (types[i] == EPIC_CELL_TYPE_OBSTACLE) { 62 | harmonic->u[y * harmonic->m[1] + x] = EPIC_LOG_SPACE_OBSTACLE; 63 | harmonic->locked[y * harmonic->m[1] + x] = 1; 64 | } else if (types[i] == EPIC_CELL_TYPE_FREE) { 65 | harmonic->u[y * harmonic->m[1] + x] = EPIC_LOG_SPACE_FREE; 66 | harmonic->locked[y * harmonic->m[1] + x] = 0; 67 | } else { 68 | fprintf(stderr, "Warning[harmonic_utilities_set_cells_2d_cpu]: %s\n", 69 | "Type is invalid. No change made."); 70 | //return EPIC_ERROR_INVALID_CELL_TYPE; 71 | continue; 72 | } 73 | } 74 | 75 | return EPIC_SUCCESS; 76 | } 77 | 78 | }; // namespace epic 79 | 80 | -------------------------------------------------------------------------------- /libepic/src/harmonic/harmonic_utilities_gpu.cu: -------------------------------------------------------------------------------- 1 | /** 2 | * The MIT License (MIT) 3 | * 4 | * Copyright (c) 2015 Kyle Hollins Wray, University of Massachusetts 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | * this software and associated documentation files (the "Software"), to deal in 8 | * the Software without restriction, including without limitation the rights to 9 | * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | * the Software, and to permit persons to whom the Software is furnished to do so, 11 | * subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in all 14 | * copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | 36 | namespace epic { 37 | 38 | __global__ void harmonic_utilities_set_cells_2d_gpu(unsigned int *m, float *u, unsigned int *locked, 39 | unsigned int k, unsigned int *v, unsigned int *types) 40 | { 41 | unsigned int i = blockIdx.x * blockDim.x + threadIdx.x; 42 | if (i >= k) { 43 | return; 44 | } 45 | 46 | unsigned int x = v[i * 2 + 0]; 47 | unsigned int y = v[i * 2 + 1]; 48 | 49 | if (y >= m[0] || x >= m[1]) { 50 | return; 51 | } 52 | 53 | if (types[i] == EPIC_CELL_TYPE_GOAL) { 54 | u[y * m[1] + x] = EPIC_LOG_SPACE_GOAL; 55 | locked[y * m[1] + x] = 1; 56 | } else if (types[i] == EPIC_CELL_TYPE_OBSTACLE) { 57 | u[y * m[1] + x] = EPIC_LOG_SPACE_OBSTACLE; 58 | locked[y * m[1] + x] = 1; 59 | } else if (types[i] == EPIC_CELL_TYPE_FREE) { 60 | u[y * m[1] + x] = EPIC_LOG_SPACE_FREE; 61 | locked[y * m[1] + x] = 0; 62 | } 63 | } 64 | 65 | 66 | int harmonic_utilities_set_cells_2d_gpu(Harmonic *harmonic, unsigned int numThreads, 67 | unsigned int k, unsigned int *v, unsigned int *types) 68 | { 69 | if (harmonic == nullptr || harmonic->n == 0 || harmonic->m == nullptr || 70 | harmonic->u == nullptr || harmonic->locked == nullptr || 71 | k == 0 || v == nullptr || types == nullptr) { 72 | fprintf(stderr, "Error[harmonic_utilities_set_cells_2d_gpu]: Invalid data."); 73 | return EPIC_ERROR_INVALID_DATA; 74 | } 75 | 76 | unsigned int *d_v = nullptr; 77 | unsigned int *d_types = nullptr; 78 | unsigned int numBlocks = (unsigned int)(k / numThreads) + 1; 79 | 80 | // Allocate memory and copy the data for the parameters we will pass to the device. 81 | if (cudaMalloc(&d_v, 2 * k * sizeof(unsigned int)) != cudaSuccess) { 82 | fprintf(stderr, "Error[harmonic_utilities_set_cells_2d_gpu]: %s\n", 83 | "Failed to allocate device-side memory for the vector of cell locations."); 84 | return EPIC_ERROR_DEVICE_MALLOC; 85 | } 86 | if (cudaMemcpy(d_v, v, 2 * k * sizeof(unsigned int), cudaMemcpyHostToDevice) != cudaSuccess) { 87 | fprintf(stderr, "Error[harmonic_utilities_set_cells_2d_gpu]: %s\n", 88 | "Failed to allocate device-side memory for the vector of cell locations."); 89 | return EPIC_ERROR_MEMCPY_TO_DEVICE; 90 | } 91 | 92 | if (cudaMalloc(&d_types, k * sizeof(unsigned int)) != cudaSuccess) { 93 | fprintf(stderr, "Error[harmonic_utilities_set_cells_2d_gpu]: %s\n", 94 | "Failed to allocate device-side memory for the vector of types."); 95 | return EPIC_ERROR_DEVICE_MALLOC; 96 | } 97 | if (cudaMemcpy(d_types, types, k * sizeof(unsigned int), cudaMemcpyHostToDevice) != cudaSuccess) { 98 | fprintf(stderr, "Error[harmonic_utilities_set_cells_2d_gpu]: %s\n", 99 | "Failed to allocate device-side memory for the vector of types."); 100 | return EPIC_ERROR_MEMCPY_TO_DEVICE; 101 | } 102 | 103 | // Call the function to update the values on the device's memory. 104 | harmonic_utilities_set_cells_2d_gpu<<< numBlocks, numThreads >>>(harmonic->d_m, 105 | harmonic->d_u, harmonic->d_locked, 106 | k, d_v, d_types); 107 | 108 | // Check if there was an error executing the kernel. 109 | if (cudaGetLastError() != cudaSuccess) { 110 | fprintf(stderr, "Error[harmonic_utilities_set_cells_2d_gpu]: %s\n", 111 | "Failed to execute the 'set cells' kernel."); 112 | return EPIC_ERROR_KERNEL_EXECUTION; 113 | } 114 | 115 | // Wait for the kernel to finish before looping more. 116 | if (cudaDeviceSynchronize() != cudaSuccess) { 117 | fprintf(stderr, "Error[harmonic_utilities_set_cells_2d_gpu]: %s\n", 118 | "Failed to synchronize the device after 'set cells' kernel."); 119 | return EPIC_ERROR_DEVICE_SYNCHRONIZE; 120 | } 121 | 122 | // Free memory for the parameters we allocated on the device. 123 | if (cudaFree(d_v) != cudaSuccess) { 124 | fprintf(stderr, "Error[harmonic_utilities_set_cells_2d_gpu]: %s\n", 125 | "Failed to free device-side memory for the vector of cell locations."); 126 | return EPIC_ERROR_DEVICE_FREE; 127 | } 128 | d_v = nullptr; 129 | 130 | if (cudaFree(d_types) != cudaSuccess) { 131 | fprintf(stderr, "Error[harmonic_utilities_set_cells_2d_gpu]: %s\n", 132 | "Failed to free device-side memory for the vector of types."); 133 | return EPIC_ERROR_DEVICE_FREE; 134 | } 135 | d_types = nullptr; 136 | 137 | return EPIC_SUCCESS; 138 | } 139 | 140 | }; // namespace epic 141 | 142 | -------------------------------------------------------------------------------- /libepic/tests/batch/batch.py: -------------------------------------------------------------------------------- 1 | """ The MIT License (MIT) 2 | 3 | Copyright (c) 2015 Kyle Hollins Wray, University of Massachusetts 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy of 6 | this software and associated documentation files (the "Software"), to deal in 7 | the Software without restriction, including without limitation the rights to 8 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 9 | the Software, and to permit persons to whom the Software is furnished to do so, 10 | 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, FITNESS 17 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 18 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 19 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 20 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 21 | """ 22 | 23 | import os 24 | import sys 25 | 26 | import math 27 | 28 | thisFilePath = os.path.dirname(os.path.realpath(__file__)) 29 | 30 | sys.path.append(os.path.join(thisFilePath, "..", "..", "python")) 31 | from epic.harmonic import * 32 | from epic.harmonic_map import * 33 | 34 | import epic.epic_harmonic as eh 35 | 36 | import compare_precision as cp 37 | 38 | 39 | precision = 1e-3 40 | 41 | domains = [ 42 | #{'name': "UMass", 'filename': os.path.join(thisFilePath, "umass.png")}, 43 | #{'name': "Willow", 'filename': os.path.join(thisFilePath, "willow_garage.png")}, 44 | #{'name': "Mine S", 'filename': os.path.join(thisFilePath, "small_mine.png")}, 45 | #{'name': "Mine L", 'filename': os.path.join(thisFilePath, "large_mine.png")}, 46 | #{'name': "C-Space", 'filename': os.path.join(thisFilePath, "c_space.png")}, 47 | #{'name': "Maze S", 'filename': os.path.join(thisFilePath, "small_maze.png")}, 48 | {'name': "Maze L", 'filename': os.path.join(thisFilePath, "large_maze.png")}, 49 | ] 50 | 51 | 52 | def cpu_sor(harmonicMap): 53 | """ Run the CPU version of SOR. 54 | 55 | Parameters: 56 | harmonicMap -- The HarmonicMap object. 57 | 58 | Returns: 59 | percentValid -- The percentage of valid cells. 60 | timePerUpdate -- The time per update step. 61 | timeToConverge -- The time until it converged. 62 | """ 63 | 64 | omega = 1.5 65 | 66 | numIterations = ct.c_uint(int(0)) 67 | array_type_m_uint = ct.c_uint * (harmonicMap.originalImage.size) 68 | locked = array_type_m_uint(*np.array([[int(harmonicMap.originalImage[y, x] == 0 or \ 69 | harmonicMap.originalImage[y, x] == 255) \ 70 | for x in range(harmonicMap.originalImage.shape[1])] \ 71 | for y in range(harmonicMap.originalImage.shape[0])]).flatten()) 72 | array_type_m_double = ct.c_double * (harmonicMap.originalImage.size) 73 | uDouble = array_type_m_double(*np.array([[1.0 - float(harmonicMap.originalImage[y, x] == 255) \ 74 | for x in range(harmonicMap.originalImage.shape[1])] \ 75 | for y in range(harmonicMap.originalImage.shape[0])]).flatten()) 76 | 77 | timing = (time.time(), time.clock()) 78 | result = eh._epic.harmonic_legacy_sor_2d_double_cpu(harmonicMap.originalImage.shape[1], 79 | harmonicMap.originalImage.shape[0], 80 | precision, omega, locked, uDouble, 81 | ct.byref(numIterations)) 82 | timing = (time.time() - timing[0], time.clock() - timing[1]) 83 | 84 | harmonicMap.solve(process='gpu', epsilon=1e-4) 85 | 86 | cp.color_valid_gradient_in_image(harmonicMap, uDouble, cp.DOUBLE_COLOR) 87 | 88 | numValidStreamlines = 0 89 | numStreamlineChecks = 0 90 | 91 | for y in range(1, harmonicMap.originalImage.shape[0] - 1): 92 | for x in range(1, harmonicMap.originalImage.shape[1] - 1): 93 | if harmonicMap.originalImage[y, x] == 0: 94 | continue 95 | 96 | numStreamlineChecks += 1 97 | 98 | if harmonicMap.originalImage[y, x] == cp.DOUBLE_COLOR or \ 99 | harmonicMap.originalImage[y, x] == 255: 100 | numValidStreamlines += 1 101 | 102 | return numValidStreamlines / numStreamlineChecks, timing[0] / int(numIterations.value), timing[0] 103 | 104 | 105 | if __name__ == "__main__": 106 | if len(sys.argv) != 2: 107 | print("Please specify an output filename.") 108 | sys.exit(0) 109 | 110 | print("Starting Batch Experiments...") 111 | 112 | with open(sys.argv[1], 'w') as f: 113 | f.write(",,,CPU SOR,,CPU log-GS,,GPU log-GS,\n") 114 | f.write("Domain,Size") 115 | f.write(",Percent Valid,Time per Update,Time to Converge") 116 | f.write(",Time per Update,Time to Converge") 117 | f.write(",Time per Update,Time to Converge\n") 118 | 119 | for img in domains: 120 | # ----- CPU SOR ----- 121 | harmonicMap = HarmonicMap() 122 | harmonicMap.load(img['filename']) 123 | 124 | f.write("%s,%i," % (img['name'], 125 | harmonicMap.originalImage.shape[0] * harmonicMap.originalImage.shape[1])) 126 | f.flush() 127 | 128 | percentValid, timePerUpdate, timeToConverge = cpu_sor(harmonicMap) 129 | f.write("%.5f," % (percentValid)) 130 | f.write("%.5f," % (timePerUpdate)) 131 | f.write("%.5f," % (timeToConverge)) 132 | f.flush() 133 | 134 | print(".", end='') 135 | sys.stdout.flush() 136 | 137 | # ----- CPU log-GS ----- 138 | harmonicMap = HarmonicMap() 139 | harmonicMap.load(img['filename']) 140 | timing = harmonicMap.solve(process='cpu', epsilon=precision) 141 | 142 | f.write("%.5f," % (timing[0] / int(harmonicMap.currentIteration))) 143 | f.write("%.5f," % (timing[0])) 144 | f.flush() 145 | 146 | print(".", end='') 147 | sys.stdout.flush() 148 | 149 | # ----- GPU log-GS ----- 150 | harmonicMap = HarmonicMap() 151 | harmonicMap.load(img['filename']) 152 | timing = harmonicMap.solve(process='gpu', epsilon=precision) 153 | 154 | f.write("%.5f," % (timing[0] / int(harmonicMap.currentIteration))) 155 | f.write("%.5f" % (timing[0])) 156 | f.write("\n") 157 | f.flush() 158 | 159 | print(".") 160 | sys.stdout.flush() 161 | 162 | f.close() 163 | 164 | print("Done.") 165 | 166 | -------------------------------------------------------------------------------- /libepic/tests/batch/c_space.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kylewray/epic/4fc4e8f6ee51e7f800cd4db34836212995d5396e/libepic/tests/batch/c_space.png -------------------------------------------------------------------------------- /libepic/tests/batch/compare_precision.py: -------------------------------------------------------------------------------- 1 | """ The MIT License (MIT) 2 | 3 | Copyright (c) 2015 Kyle Hollins Wray, University of Massachusetts 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy of 6 | this software and associated documentation files (the "Software"), to deal in 7 | the Software without restriction, including without limitation the rights to 8 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 9 | the Software, and to permit persons to whom the Software is furnished to do so, 10 | 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, FITNESS 17 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 18 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 19 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 20 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 21 | """ 22 | 23 | import os 24 | import sys 25 | 26 | import math 27 | import itertools as it 28 | 29 | thisFilePath = os.path.dirname(os.path.realpath(__file__)) 30 | 31 | sys.path.append(os.path.join(thisFilePath, "..", "..", "python")) 32 | from epic.harmonic import * 33 | from epic.harmonic_map import * 34 | 35 | import epic.epic_harmonic as eh 36 | 37 | FLOAT_COLOR = 60 38 | DOUBLE_COLOR = 90 39 | LONG_DOUBLE_COLOR = 120 40 | FREE_SPACE_COLOR = 150 41 | 42 | 43 | def compute_potential(harmonicMap, u, x, y): 44 | """ Compute the potential at a location. 45 | 46 | Parameters: 47 | harmonicMap -- The HarmonicMap object. 48 | u -- The potentials (row-major 1-d array). 49 | x -- The x-axis location. 50 | y -- The y-axis location. 51 | 52 | Returns: 53 | The potential at the location. 54 | """ 55 | 56 | xtl = int(x - 0.5) 57 | ytl = int(y - 0.5) 58 | xtr = int(x + 0.5) 59 | ytr = int(y - 0.5) 60 | xbl = int(x - 0.5) 61 | ybl = int(y + 0.5) 62 | xbr = int(x + 0.5) 63 | ybr = int(y + 0.5) 64 | 65 | alpha = (x - xtl) 66 | beta = (y - ytl) 67 | 68 | one = (1.0 - alpha) * u[ytl * harmonicMap.originalImage.shape[1] + xtl] + \ 69 | alpha * u[ytr * harmonicMap.originalImage.shape[1] + xtr] 70 | two = (1.0 - alpha) * u[ybl * harmonicMap.originalImage.shape[1] + xbl] + \ 71 | alpha * u[ybr * harmonicMap.originalImage.shape[1] + xbr] 72 | return (1.0 - beta) * one + beta * two 73 | 74 | 75 | def color_valid_gradient_in_image(harmonicMap, u, c): 76 | """ Color the image on all cells with a valid gradient. 77 | 78 | Parameters: 79 | harmonicMap -- The HarmonicMap object. 80 | u -- The potentials (row-major 1-d array). 81 | c -- The grayscale color (0 to 255) to paint on the image. 82 | """ 83 | 84 | originalColorMapping = dict() 85 | 86 | for y in range(1, harmonicMap.originalImage.shape[0] - 1): 87 | for x in range(1, harmonicMap.originalImage.shape[1] - 1): 88 | # Obstacles and goals are skipped. 89 | if harmonicMap.originalImage[y, x] == 255 or harmonicMap.originalImage[y, x] == 0: 90 | continue 91 | 92 | # Check if this pixel has a valid gradient; it is not valid without one. 93 | valid = True 94 | 95 | precision = 0.25 96 | 97 | x += 0.5 98 | y += 0.5 99 | 100 | value0 = compute_potential(harmonicMap, u, x - precision, y) 101 | value1 = compute_potential(harmonicMap, u, x + precision, y) 102 | value2 = compute_potential(harmonicMap, u, x, y - precision) 103 | value3 = compute_potential(harmonicMap, u, x, y + precision) 104 | 105 | x = int(x) 106 | y = int(y) 107 | 108 | partialX = (value1 - value0) / (2.0 * precision) 109 | partialY = (value3 - value2) / (2.0 * precision) 110 | 111 | denom = math.sqrt(pow(partialX, 2) + pow(partialY, 2)) 112 | 113 | if denom <= 1e-10: 114 | valid = False 115 | 116 | # Draw color where precision is valid. 117 | originalColorMapping[(x, y)] = harmonicMap.originalImage[y, x] 118 | if valid: 119 | harmonicMap.originalImage[y, x] = c 120 | 121 | # Uncomment to instead draw the potential. 122 | #harmonicMap.originalImage[y, x] = int(255.0 * 123 | # (1.0 - u[y * harmonicMap.originalImage.shape[1] + x])) 124 | 125 | # Compute all pixels that can reach a goal over these valid colored pixels. 126 | closedset = set() 127 | openset = [(xi, yj) 128 | for xi, yj in it.product(range(1, harmonicMap.originalImage.shape[1] - 1), 129 | range(1, harmonicMap.originalImage.shape[0] - 1)) 130 | if harmonicMap.originalImage[yj, xi] == 255] 131 | 132 | while len(openset) > 0: 133 | node = openset.pop() 134 | closedset |= {node} 135 | 136 | for x, y in [(node[0] - 1, node[1]), (node[0] + 1, node[1]), (node[0], node[1] - 1), (node[0], node[1] + 1)]: 137 | if x < 0 or y < 0 or \ 138 | x >= harmonicMap.originalImage.shape[1] or y >= harmonicMap.originalImage.shape[0] or \ 139 | (x, y) in closedset or harmonicMap.originalImage[y, x] != c: 140 | continue 141 | 142 | openset += [(x, y)] 143 | 144 | # Finally, re-color everything to only render the valid pixels. 145 | for y in range(1, harmonicMap.originalImage.shape[0] - 1): 146 | for x in range(1, harmonicMap.originalImage.shape[1] - 1): 147 | # Obstacles and goals are skipped. 148 | if harmonicMap.originalImage[y, x] == 255 or harmonicMap.originalImage[y, x] == 0: 149 | continue 150 | 151 | # Only re-color this specific color to freespace. 152 | if harmonicMap.originalImage[y, x] == c and (x, y) not in closedset: 153 | harmonicMap.originalImage[y, x] = originalColorMapping[(x, y)] 154 | 155 | # Lastly, go over all these pixels one last time and compute a streamline from them. If the streamline passes 156 | # through a pixel that is not this color, then reset this initial starting pixel. 157 | invalidLocations = list() 158 | 159 | for y in range(1, harmonicMap.originalImage.shape[0] - 1): 160 | for x in range(1, harmonicMap.originalImage.shape[1] - 1): 161 | # Obstacles and goals are skipped. Also, skip non-c-pixels. 162 | if harmonicMap.originalImage[y, x] != c: 163 | continue 164 | 165 | # Skip cells that border on obstacles or goals. 166 | if harmonicMap.locked[(y - 1) * harmonicMap.m[1] + x] == 1 or \ 167 | harmonicMap.locked[(y + 1) * harmonicMap.m[1] + x] == 1 or \ 168 | harmonicMap.locked[y * harmonicMap.m[1] + (x - 1)] == 1 or \ 169 | harmonicMap.locked[y * harmonicMap.m[1] + (x + 1)] == 1: 170 | continue 171 | 172 | validStreamline = True 173 | try: 174 | path = harmonicMap._compute_streamline(x, y) 175 | for xi, yj in path: 176 | if not (harmonicMap.originalImage[int(yj), int(xi)] == c or harmonicMap.originalImage[int(yj), int(xi)] == 255): 177 | validStreamline = False 178 | break 179 | except: 180 | #print((y, x), (harmonicMap.m[0], harmonicMap.m[1]), harmonicMap.originalImage[y, x]) 181 | validStreamline = False 182 | 183 | # If not a valid streamline, then reset color. 184 | if not validStreamline: 185 | #harmonicMap.originalImage[y, x] = originalColorMapping[(x, y)] 186 | invalidLocations += [(x, y)] 187 | 188 | for x, y in invalidLocations: 189 | harmonicMap.originalImage[y, x] = originalColorMapping[(x, y)] 190 | 191 | 192 | if __name__ == "__main__": 193 | #mapFilename = os.path.join(thisFilePath, "umass.png") 194 | mapFilename = os.path.join(thisFilePath, "willow_garage.png") 195 | #mapFilename = os.path.join(thisFilePath, "c_space.png") 196 | #mapFilename = os.path.join(thisFilePath, "small_maze.png") 197 | #mapFilename = os.path.join(thisFilePath, "large_maze.png") 198 | #mapFilename = os.path.join(thisFilePath, "..", "maps", "basic.png") 199 | 200 | harmonicMap = HarmonicMap() 201 | harmonicMap.load(mapFilename) 202 | #harmonicMap.show() 203 | 204 | numIterations = ct.c_uint(int(0)) 205 | 206 | array_type_m_uint = ct.c_uint * (harmonicMap.originalImage.size) 207 | locked = array_type_m_uint(*np.array([[int(harmonicMap.originalImage[y, x] == 0 or \ 208 | harmonicMap.originalImage[y, x] == 255) \ 209 | for x in range(harmonicMap.originalImage.shape[1])] \ 210 | for y in range(harmonicMap.originalImage.shape[0])]).flatten()) 211 | 212 | print("Computing SOR float...") 213 | array_type_m_float = ct.c_float * (harmonicMap.originalImage.size) 214 | uFloat = array_type_m_float(*np.array([[1.0 - float(harmonicMap.originalImage[y, x] == 255) \ 215 | for x in range(harmonicMap.originalImage.shape[1])] \ 216 | for y in range(harmonicMap.originalImage.shape[0])]).flatten()) 217 | result = eh._epic.harmonic_legacy_sor_2d_float_cpu(harmonicMap.originalImage.shape[1], 218 | harmonicMap.originalImage.shape[0], 219 | 1e-4, 1.5, locked, uFloat, ct.byref(numIterations)) 220 | 221 | print("Computing SOR double...") 222 | array_type_m_double = ct.c_double * (harmonicMap.originalImage.size) 223 | uDouble = array_type_m_double(*np.array([[1.0 - float(harmonicMap.originalImage[y, x] == 255) \ 224 | for x in range(harmonicMap.originalImage.shape[1])] \ 225 | for y in range(harmonicMap.originalImage.shape[0])]).flatten()) 226 | result = eh._epic.harmonic_legacy_sor_2d_double_cpu(harmonicMap.originalImage.shape[1], 227 | harmonicMap.originalImage.shape[0], 228 | 1e-4, 1.5, locked, uDouble, ct.byref(numIterations)) 229 | 230 | #print("Computing SOR long double...") 231 | #array_type_m_longdouble = ct.c_longdouble * (harmonicMap.originalImage.size) 232 | #uLongDouble = array_type_m_longdouble(*np.array([[1.0 - float(harmonicMap.originalImage[y, x] == 255) \ 233 | # for x in range(harmonicMap.originalImage.shape[1])] \ 234 | # for y in range(harmonicMap.originalImage.shape[0])]).flatten()) 235 | #result = eh._epic.harmonic_legacy_sor_2d_long_double_cpu(harmonicMap.originalImage.shape[1], 236 | # harmonicMap.originalImage.shape[0], 237 | # 1e-4, 1.5, locked, uLongDouble, ct.byref(numIterations)) 238 | 239 | print("Computing Log-Space GS GPU...") 240 | timing = harmonicMap.solve(process='gpu', epsilon=1e-4) 241 | 242 | #print("Coloring map with long double colors...") 243 | #color_valid_gradient_in_image(uLongDouble, LONG_DOUBLE_COLOR) 244 | 245 | print("Coloring map with double colors...") 246 | color_valid_gradient_in_image(harmonicMap, uDouble, DOUBLE_COLOR) 247 | 248 | print("Coloring map with float colors...") 249 | color_valid_gradient_in_image(harmonicMap, uFloat, FLOAT_COLOR) 250 | 251 | harmonicMap.hold = True 252 | harmonicMap.show() 253 | 254 | print("Done.") 255 | 256 | -------------------------------------------------------------------------------- /libepic/tests/batch/large_maze.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kylewray/epic/4fc4e8f6ee51e7f800cd4db34836212995d5396e/libepic/tests/batch/large_maze.png -------------------------------------------------------------------------------- /libepic/tests/batch/large_mine.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kylewray/epic/4fc4e8f6ee51e7f800cd4db34836212995d5396e/libepic/tests/batch/large_mine.png -------------------------------------------------------------------------------- /libepic/tests/batch/small_maze.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kylewray/epic/4fc4e8f6ee51e7f800cd4db34836212995d5396e/libepic/tests/batch/small_maze.png -------------------------------------------------------------------------------- /libepic/tests/batch/small_mine.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kylewray/epic/4fc4e8f6ee51e7f800cd4db34836212995d5396e/libepic/tests/batch/small_mine.png -------------------------------------------------------------------------------- /libepic/tests/batch/umass.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kylewray/epic/4fc4e8f6ee51e7f800cd4db34836212995d5396e/libepic/tests/batch/umass.png -------------------------------------------------------------------------------- /libepic/tests/batch/willow_garage.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kylewray/epic/4fc4e8f6ee51e7f800cd4db34836212995d5396e/libepic/tests/batch/willow_garage.png -------------------------------------------------------------------------------- /libepic/tests/maps/basic.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kylewray/epic/4fc4e8f6ee51e7f800cd4db34836212995d5396e/libepic/tests/maps/basic.png -------------------------------------------------------------------------------- /libepic/tests/maps/c_space.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kylewray/epic/4fc4e8f6ee51e7f800cd4db34836212995d5396e/libepic/tests/maps/c_space.png -------------------------------------------------------------------------------- /libepic/tests/maps/maps.py: -------------------------------------------------------------------------------- 1 | """ The MIT License (MIT) 2 | 3 | Copyright (c) 2015 Kyle Hollins Wray, University of Massachusetts 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy of 6 | this software and associated documentation files (the "Software"), to deal in 7 | the Software without restriction, including without limitation the rights to 8 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 9 | the Software, and to permit persons to whom the Software is furnished to do so, 10 | 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, FITNESS 17 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 18 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 19 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 20 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 21 | """ 22 | 23 | import os 24 | import sys 25 | 26 | import math 27 | 28 | thisFilePath = os.path.dirname(os.path.realpath(__file__)) 29 | 30 | sys.path.append(os.path.join(thisFilePath, "..", "..", "python")) 31 | from epic.harmonic import * 32 | from epic.harmonic_map import * 33 | from epic.harmonic_legacy import * 34 | from epic.harmonic_legacy_map import * 35 | 36 | import epic.epic_harmonic as eh 37 | 38 | 39 | images = [ 40 | #{'name': "Trvial", 'filename': os.path.join(thisFilePath, "trivial.png")}, 41 | #{'name': "Basic", 'filename': os.path.join(thisFilePath, "basic.png")}, 42 | #{'name': "C-Space", 'filename': os.path.join(thisFilePath, "c_space.png")}, 43 | #{'name': "Maze 1", 'filename': os.path.join(thisFilePath, "maze_1.png")}, 44 | #{'name': "Maze 2", 'filename': os.path.join(thisFilePath, "maze_2.png")}, 45 | #{'name': "Maze 3", 'filename': os.path.join(thisFilePath, "maze_3.png")}, 46 | {'name': "Maze 4", 'filename': os.path.join(thisFilePath, "maze_4.png")}, 47 | #{'name': "Mine 1", 'filename': os.path.join(thisFilePath, "mine_1.png")}, 48 | #{'name': "Mine 2", 'filename': os.path.join(thisFilePath, "mine_2.png")}, 49 | ] 50 | 51 | precisions = [1e-1, 1e-2, 1e-3] 52 | processes = ['gpu', 'cpu'] 53 | 54 | 55 | if __name__ == "__main__": 56 | if len(sys.argv) != 2 or sys.argv[1] not in ['visual', 'visual_legacy', 'batch']: 57 | print("Please specify either 'visual', 'visual_legacy', or 'batch' as an argument.") 58 | sys.exit(0) 59 | 60 | for img in images: 61 | print("Image: %s." % (img['name'])) 62 | 63 | if sys.argv[1] == 'visual': 64 | harmonicMap = HarmonicMap() 65 | harmonicMap.load(img['filename']) 66 | 67 | timing = harmonicMap.solve(process='gpu', epsilon=1e-3) 68 | harmonicMap.show() 69 | 70 | elif sys.argv[1] == 'visual_legacy': 71 | harmonicLegacyMap = HarmonicLegacyMap() 72 | 73 | harmonicLegacyMap.flipped = True 74 | #harmonicLegacyMap.flipped = False 75 | 76 | harmonicLegacyMap.load(img['filename']) 77 | 78 | timing = harmonicLegacyMap.solve(epsilon=1e-10) 79 | harmonicLegacyMap.show() 80 | 81 | elif sys.argv[1] == 'batch': 82 | for prec in precisions: 83 | for proc in processes: 84 | print("%s %s " % (prec, proc), end='') 85 | sys.stdout.flush() 86 | 87 | harmonicMap = HarmonicMap() 88 | harmonicMap.load(img['filename']) 89 | timing = harmonicMap.solve(process=proc, epsilon=prec) 90 | 91 | print("%.2f" % (timing[0])) 92 | 93 | print("Done.") 94 | 95 | -------------------------------------------------------------------------------- /libepic/tests/maps/maze_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kylewray/epic/4fc4e8f6ee51e7f800cd4db34836212995d5396e/libepic/tests/maps/maze_1.png -------------------------------------------------------------------------------- /libepic/tests/maps/maze_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kylewray/epic/4fc4e8f6ee51e7f800cd4db34836212995d5396e/libepic/tests/maps/maze_2.png -------------------------------------------------------------------------------- /libepic/tests/maps/maze_3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kylewray/epic/4fc4e8f6ee51e7f800cd4db34836212995d5396e/libepic/tests/maps/maze_3.png -------------------------------------------------------------------------------- /libepic/tests/maps/maze_4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kylewray/epic/4fc4e8f6ee51e7f800cd4db34836212995d5396e/libepic/tests/maps/maze_4.png -------------------------------------------------------------------------------- /libepic/tests/maps/mine_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kylewray/epic/4fc4e8f6ee51e7f800cd4db34836212995d5396e/libepic/tests/maps/mine_1.png -------------------------------------------------------------------------------- /libepic/tests/maps/mine_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kylewray/epic/4fc4e8f6ee51e7f800cd4db34836212995d5396e/libepic/tests/maps/mine_2.png -------------------------------------------------------------------------------- /libepic/tests/maps/trivial.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kylewray/epic/4fc4e8f6ee51e7f800cd4db34836212995d5396e/libepic/tests/maps/trivial.png -------------------------------------------------------------------------------- /libepic/tests/maps/umass_lpr.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kylewray/epic/4fc4e8f6ee51e7f800cd4db34836212995d5396e/libepic/tests/maps/umass_lpr.png -------------------------------------------------------------------------------- /maps/maze.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kylewray/epic/4fc4e8f6ee51e7f800cd4db34836212995d5396e/maps/maze.png -------------------------------------------------------------------------------- /maps/maze.yaml: -------------------------------------------------------------------------------- 1 | image: maze.png 2 | resolution: 0.1 3 | origin: [-10.0, -10.0, 0.0] 4 | occupied_thresh: 0.65 5 | free_thresh: 0.196 6 | negate: 0 7 | -------------------------------------------------------------------------------- /maps/umass.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kylewray/epic/4fc4e8f6ee51e7f800cd4db34836212995d5396e/maps/umass.png -------------------------------------------------------------------------------- /maps/umass.yaml: -------------------------------------------------------------------------------- 1 | image: umass.png 2 | resolution: 0.03048 3 | origin: [-2.5, -2.5, 0.0] 4 | occupied_thresh: 0.65 5 | free_thresh: 0.196 6 | negate: 0 7 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | epic 4 | 0.1.0 5 | The epic package for anytime harmonic function path planning. 6 | Kyle Hollins Wray 7 | MIT 8 | 9 | catkin 10 | 11 | roscpp 12 | std_msgs 13 | geometry_msgs 14 | nav_msgs 15 | nav_core 16 | costmap_2d 17 | 18 | message_generation 19 | message_runtime 20 | 21 | ompl 22 | 23 | libepic 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /plugins/epic_nav_core_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | This is a global planner plugin using harmonic functions as part of the 'epic' library. 4 | 5 | 6 | -------------------------------------------------------------------------------- /rviz/default.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Map1 10 | - /Path1 11 | - /Path2 12 | - /Path3 13 | - /Path4 14 | Splitter Ratio: 0.5 15 | Tree Height: 719 16 | - Class: rviz/Selection 17 | Name: Selection 18 | - Class: rviz/Tool Properties 19 | Expanded: 20 | - /2D Pose Estimate1 21 | - /2D Nav Goal1 22 | - /Publish Point1 23 | Name: Tool Properties 24 | Splitter Ratio: 0.588679 25 | - Class: rviz/Views 26 | Expanded: 27 | - /Current View1 28 | Name: Views 29 | Splitter Ratio: 0.5 30 | - Class: rviz/Time 31 | Experimental: false 32 | Name: Time 33 | SyncMode: 0 34 | SyncSource: "" 35 | Visualization Manager: 36 | Class: "" 37 | Displays: 38 | - Alpha: 0.5 39 | Cell Size: 1 40 | Class: rviz/Grid 41 | Color: 160; 160; 164 42 | Enabled: false 43 | Line Style: 44 | Line Width: 0.03 45 | Value: Lines 46 | Name: Grid 47 | Normal Cell Count: 0 48 | Offset: 49 | X: 0 50 | Y: 0 51 | Z: 0 52 | Plane: XY 53 | Plane Cell Count: 10 54 | Reference Frame: 55 | Value: false 56 | - Class: rviz/TF 57 | Enabled: false 58 | Frame Timeout: 15 59 | Frames: 60 | All Enabled: true 61 | Marker Scale: 1 62 | Name: TF 63 | Show Arrows: true 64 | Show Axes: true 65 | Show Names: true 66 | Tree: 67 | {} 68 | Update Interval: 0 69 | Value: false 70 | - Alpha: 1 71 | Class: rviz/RobotModel 72 | Collision Enabled: false 73 | Enabled: true 74 | Links: 75 | All Links Enabled: true 76 | Expand Joint Details: false 77 | Expand Link Details: false 78 | Expand Tree: false 79 | Link Tree Style: Links in Alphabetic Order 80 | base_footprint: 81 | Alpha: 1 82 | Show Axes: false 83 | Show Trail: false 84 | base_link: 85 | Alpha: 1 86 | Show Axes: false 87 | Show Trail: false 88 | Value: true 89 | caster_back_link: 90 | Alpha: 1 91 | Show Axes: false 92 | Show Trail: false 93 | Value: true 94 | caster_front_link: 95 | Alpha: 1 96 | Show Axes: false 97 | Show Trail: false 98 | Value: true 99 | cliff_sensor_front_link: 100 | Alpha: 1 101 | Show Axes: false 102 | Show Trail: false 103 | cliff_sensor_left_link: 104 | Alpha: 1 105 | Show Axes: false 106 | Show Trail: false 107 | cliff_sensor_right_link: 108 | Alpha: 1 109 | Show Axes: false 110 | Show Trail: false 111 | gyro_link: 112 | Alpha: 1 113 | Show Axes: false 114 | Show Trail: false 115 | wheel_left_link: 116 | Alpha: 1 117 | Show Axes: false 118 | Show Trail: false 119 | Value: true 120 | wheel_right_link: 121 | Alpha: 1 122 | Show Axes: false 123 | Show Trail: false 124 | Value: true 125 | Name: RobotModel 126 | Robot Description: robot_description 127 | TF Prefix: "" 128 | Update Interval: 0 129 | Value: true 130 | Visual Enabled: true 131 | - Alpha: 0.7 132 | Class: rviz/Map 133 | Color Scheme: map 134 | Draw Behind: false 135 | Enabled: true 136 | Name: Map 137 | Topic: /map 138 | Value: true 139 | - Alpha: 1 140 | Buffer Length: 1 141 | Class: rviz/Path 142 | Color: 25; 255; 0 143 | Enabled: true 144 | Line Style: Lines 145 | Line Width: 0.03 146 | Name: Path 147 | Offset: 148 | X: 0 149 | Y: 0 150 | Z: 0 151 | Topic: /move_base/TrajectoryPlannerROS/global_plan 152 | Value: true 153 | - Alpha: 1 154 | Buffer Length: 1 155 | Class: rviz/Path 156 | Color: 135; 137; 255 157 | Enabled: true 158 | Line Style: Lines 159 | Line Width: 0.03 160 | Name: Path 161 | Offset: 162 | X: 0 163 | Y: 0 164 | Z: 0 165 | Topic: /move_base/NavfnROS/plan 166 | Value: true 167 | - Alpha: 0.7 168 | Class: rviz/Map 169 | Color Scheme: costmap 170 | Draw Behind: true 171 | Enabled: false 172 | Name: Global Costmap 173 | Topic: /move_base/global_costmap/costmap 174 | Value: false 175 | - Alpha: 0.7 176 | Class: rviz/Map 177 | Color Scheme: costmap 178 | Draw Behind: false 179 | Enabled: false 180 | Name: Localcostmap 181 | Topic: /move_base/local_costmap/costmap 182 | Value: false 183 | - Alpha: 1 184 | Buffer Length: 1 185 | Class: rviz/Path 186 | Color: 255; 0; 0 187 | Enabled: true 188 | Line Style: Lines 189 | Line Width: 0.03 190 | Name: Path 191 | Offset: 192 | X: 0 193 | Y: 0 194 | Z: 0 195 | Topic: /move_base/EpicNavCorePlugin/plan 196 | Value: true 197 | - Alpha: 1 198 | Buffer Length: 1 199 | Class: rviz/Path 200 | Color: 25; 255; 0 201 | Enabled: true 202 | Line Style: Lines 203 | Line Width: 0.03 204 | Name: Path 205 | Offset: 206 | X: 0 207 | Y: 0 208 | Z: 0 209 | Topic: /epic_navigation_node/path 210 | Value: true 211 | Enabled: true 212 | Global Options: 213 | Background Color: 48; 48; 48 214 | Fixed Frame: map 215 | Frame Rate: 30 216 | Name: root 217 | Tools: 218 | - Class: rviz/Interact 219 | Hide Inactive Objects: true 220 | - Class: rviz/MoveCamera 221 | - Class: rviz/Select 222 | - Class: rviz/FocusCamera 223 | - Class: rviz/Measure 224 | - Class: rviz/SetInitialPose 225 | Topic: /initialpose 226 | - Class: rviz/SetGoal 227 | Topic: /move_base_simple/goal 228 | - Class: rviz/PublishPoint 229 | Single click: true 230 | Topic: /clicked_point 231 | Value: true 232 | Views: 233 | Current: 234 | Angle: 0 235 | Class: rviz/TopDownOrtho 236 | Enable Stereo Rendering: 237 | Stereo Eye Separation: 0.06 238 | Stereo Focal Distance: 1 239 | Swap Stereo Eyes: false 240 | Value: false 241 | Name: Current View 242 | Near Clip Distance: 0.01 243 | Scale: 15.6653 244 | Target Frame: 245 | Value: TopDownOrtho (rviz) 246 | X: 15.7852 247 | Y: 13.8392 248 | Saved: ~ 249 | Window Geometry: 250 | Displays: 251 | collapsed: false 252 | Height: 1025 253 | Hide Left Dock: false 254 | Hide Right Dock: false 255 | QMainWindow State: 000000ff00000000fd00000004000000000000013c00000360fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f00000360000000bb00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000360fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003f00000360000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000021a00fffffffb0000000800540069006d00650100000000000004500000000000000000000005290000036000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 256 | Selection: 257 | collapsed: false 258 | Time: 259 | collapsed: false 260 | Tool Properties: 261 | collapsed: false 262 | Views: 263 | collapsed: false 264 | Width: 1920 265 | X: 0 266 | Y: 30 267 | -------------------------------------------------------------------------------- /src/epic_navigation_node.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * The MIT License (MIT) 3 | * 4 | * Copyright (c) 2016 Kyle Hollins Wray, University of Massachusetts 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | * this software and associated documentation files (the "Software"), to deal in 8 | * the Software without restriction, including without limitation the rights to 9 | * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | * the Software, and to permit persons to whom the Software is furnished to do so, 11 | * subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in all 14 | * copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | 25 | #include 26 | 27 | namespace epic { 28 | 29 | EpicNavigationNode::EpicNavigationNode(ros::NodeHandle &nh) : private_node_handle(nh) 30 | { } 31 | 32 | EpicNavigationNode::~EpicNavigationNode() 33 | { } 34 | 35 | }; // namespace epic 36 | 37 | -------------------------------------------------------------------------------- /src/epic_navigation_node_harmonic_rviz.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * The MIT License (MIT) 3 | * 4 | * Copyright (c) 2015 Kyle Hollins Wray, University of Massachusetts 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | * this software and associated documentation files (the "Software"), to deal in 8 | * the Software without restriction, including without limitation the rights to 9 | * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | * the Software, and to permit persons to whom the Software is furnished to do so, 11 | * subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in all 14 | * copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | 25 | #include 26 | 27 | #include 28 | #include 29 | 30 | #include 31 | #include 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | namespace epic { 43 | 44 | EpicNavigationNodeHarmonicRviz::EpicNavigationNodeHarmonicRviz(ros::NodeHandle &nh) : 45 | EpicNavigationNodeHarmonic(nh) 46 | //private_node_handle(nh) 47 | { 48 | goal_added = false; 49 | } 50 | 51 | 52 | EpicNavigationNodeHarmonicRviz::~EpicNavigationNodeHarmonicRviz() 53 | { } 54 | 55 | 56 | bool EpicNavigationNodeHarmonicRviz::initialize() 57 | { 58 | if (init_msgs) { 59 | return false; 60 | } 61 | 62 | EpicNavigationNodeHarmonic::initialize(); 63 | 64 | // The follow subscribers/publishers are exclusively for simplified interaction with rviz. 65 | std::string sub_map_pose_estimate_topic; 66 | private_node_handle.param("sub_map_pose_estimate", 67 | sub_map_pose_estimate_topic, 68 | "/initialpose"); 69 | sub_map_pose_estimate = private_node_handle.subscribe(sub_map_pose_estimate_topic, 70 | 10, 71 | &EpicNavigationNodeHarmonicRviz::subMapPoseEstimate, 72 | this); 73 | 74 | std::string sub_map_nav_goal_topic; 75 | private_node_handle.param("sub_map_nav_goal", 76 | sub_map_nav_goal_topic, 77 | "/move_base_simple/goal"); 78 | sub_map_nav_goal = private_node_handle.subscribe(sub_map_nav_goal_topic, 79 | 10, 80 | &EpicNavigationNodeHarmonicRviz::subMapNavGoal, 81 | this); 82 | 83 | std::string pub_map_path_topic; 84 | private_node_handle.param("pub_map_path", 85 | pub_map_path_topic, 86 | "path"); 87 | pub_map_path = private_node_handle.advertise(pub_map_path_topic, 1); 88 | 89 | init_msgs = true; 90 | 91 | return true; 92 | } 93 | 94 | 95 | void EpicNavigationNodeHarmonicRviz::subMapPoseEstimate(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg) 96 | { 97 | current_pose.header.frame_id = msg->header.frame_id; 98 | current_pose.header.stamp = msg->header.stamp; 99 | 100 | current_pose.pose.position.x = msg->pose.pose.position.x; 101 | current_pose.pose.position.y = msg->pose.pose.position.y; 102 | current_pose.pose.position.z = msg->pose.pose.position.z; 103 | 104 | current_pose.pose.orientation.x = msg->pose.pose.orientation.x; 105 | current_pose.pose.orientation.y = msg->pose.pose.orientation.y; 106 | current_pose.pose.orientation.z = msg->pose.pose.orientation.z; 107 | current_pose.pose.orientation.w = msg->pose.pose.orientation.w; 108 | 109 | epic::ComputePath::Request req; 110 | epic::ComputePath::Response res; 111 | 112 | req.start = current_pose; 113 | 114 | req.step_size = 0.05; 115 | req.precision = 0.5; 116 | req.max_length = width * height / req.step_size; 117 | 118 | EpicNavigationNodeHarmonic::srvComputePath(req, res); 119 | 120 | pub_map_path.publish(res.path); 121 | } 122 | 123 | 124 | void EpicNavigationNodeHarmonicRviz::subMapNavGoal(const geometry_msgs::PoseStamped::ConstPtr &msg) 125 | { 126 | epic::ModifyGoals::Request req; 127 | epic::ModifyGoals::Response res; 128 | 129 | if (goal_added) { 130 | req.goals.push_back(last_goal); 131 | EpicNavigationNodeHarmonic::srvRemoveGoals(req, res); 132 | } 133 | 134 | last_goal.header.frame_id = msg->header.frame_id; 135 | last_goal.header.stamp = msg->header.stamp; 136 | 137 | last_goal.pose.position.x = msg->pose.position.x; 138 | last_goal.pose.position.y = msg->pose.position.y; 139 | last_goal.pose.position.z = msg->pose.position.z; 140 | 141 | last_goal.pose.orientation.x = msg->pose.orientation.x; 142 | last_goal.pose.orientation.y = msg->pose.orientation.y; 143 | last_goal.pose.orientation.z = msg->pose.orientation.z; 144 | last_goal.pose.orientation.w = msg->pose.orientation.w; 145 | 146 | req.goals.clear(); 147 | req.goals.push_back(last_goal); 148 | EpicNavigationNodeHarmonic::srvAddGoals(req, res); 149 | 150 | goal_added = true; 151 | } 152 | 153 | }; // namespace epic 154 | 155 | 156 | -------------------------------------------------------------------------------- /src/epic_navigation_node_main.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * The MIT License (MIT) 3 | * 4 | * Copyright (c) 2015 Kyle Hollins Wray, University of Massachusetts 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | * this software and associated documentation files (the "Software"), to deal in 8 | * the Software without restriction, including without limitation the rights to 9 | * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | * the Software, and to permit persons to whom the Software is furnished to do so, 11 | * subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in all 14 | * copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | 25 | #include 26 | 27 | #include 28 | #include 29 | #include 30 | //#include 31 | 32 | int main(int argc, char **argv) 33 | { 34 | ros::init(argc, argv, "epic_navigation_node"); 35 | 36 | ros::NodeHandle node_handle("~"); 37 | 38 | // Note: The extra "~" is not required in here because private_node_handle is initialized 39 | // with this relative namespace already. Basically, it is already in the string. This 40 | // differs from Python, which does not use a private node handle. 41 | 42 | // Read a ROS parameter for this node to assign the desired algorithm for EpicNavigationNode. 43 | std::string algorithm; 44 | node_handle.param("algorithm", algorithm, "harmonic"); 45 | 46 | // Read a ROS parameter for if Rviz message support should be included. 47 | bool rviz_support = false; 48 | node_handle.param("rviz_support", rviz_support, false); 49 | 50 | epic::EpicNavigationNode *epic_navigation_node; 51 | 52 | if (algorithm == "harmonic") { 53 | if (!rviz_support) { 54 | epic_navigation_node = new epic::EpicNavigationNodeHarmonic(node_handle); 55 | } else { 56 | epic_navigation_node = new epic::EpicNavigationNodeHarmonicRviz(node_handle); 57 | } 58 | } 59 | 60 | epic_navigation_node->initialize(); 61 | 62 | // Read a ROS parameter for the number of update steps of the algorithm at a time. 63 | int steps_per_update = 50; 64 | node_handle.param("steps_per_update", steps_per_update, 50); 65 | 66 | // Read a ROS parameter for how many updates should be called per second. (Default is 10Hz.) 67 | int update_rate = 10; 68 | node_handle.param("update_rate", update_rate, 10); 69 | 70 | ros::Rate rate(update_rate); 71 | 72 | while (ros::ok()) { 73 | // Check for service calls. 74 | ros::spinOnce(); 75 | 76 | // Perform an update of the harmonic function or other navigation planning algorithm. 77 | epic_navigation_node->update(steps_per_update); 78 | 79 | // Sleep and let other processes think. 80 | rate.sleep(); 81 | } 82 | 83 | delete epic_navigation_node; 84 | 85 | return 0; 86 | } 87 | 88 | -------------------------------------------------------------------------------- /srv/ComputePath.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/PoseStamped start 2 | float32 step_size 3 | float32 precision 4 | uint32 max_length 5 | --- 6 | nav_msgs/Path path 7 | -------------------------------------------------------------------------------- /srv/GetCell.srv: -------------------------------------------------------------------------------- 1 | uint32 x 2 | uint32 y 3 | --- 4 | bool success 5 | float32 value 6 | -------------------------------------------------------------------------------- /srv/ModifyGoals.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/PoseStamped[] goals 2 | --- 3 | bool success 4 | -------------------------------------------------------------------------------- /srv/ResetFreeCells.srv: -------------------------------------------------------------------------------- 1 | --- 2 | bool success 3 | -------------------------------------------------------------------------------- /srv/SetCells.srv: -------------------------------------------------------------------------------- 1 | uint32[] v 2 | uint32[] types 3 | --- 4 | bool success 5 | -------------------------------------------------------------------------------- /srv/SetStatus.srv: -------------------------------------------------------------------------------- 1 | bool paused 2 | --- 3 | bool success 4 | --------------------------------------------------------------------------------