├── mpl_benchmark
    ├── CMakeLists.txt
    └── mpl_benchmark.cpp
├── tests
    ├── jps_test.cpp
    ├── graph_fixture.h
    ├── CMakeLists.txt
    ├── graph_test.cpp
    ├── planner_fixture.h
    └── planner_test.cpp
├── .gitignore
├── include
    ├── CMakeLists.txt
    └── mpl
    │   ├── planner.h
    │   ├── dfs.h
    │   ├── bfs.h
    │   ├── astar.h
    │   └── jps.h
├── src
    ├── CMakeLists.txt
    └── mpl
    │   ├── common_utility.h
    │   ├── grid_map_generator
    │       └── random_grid_map_generator.h
    │   ├── eigen_utility.h
    │   └── visualization_utility.h
├── examples
    ├── grid_map_generation.cpp
    ├── CMakeLists.txt
    ├── dfs.cpp
    ├── bfs.cpp
    ├── jps.cpp
    └── astar.cpp
├── README.md
└── CMakeLists.txt


/mpl_benchmark/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | add_executable(mpl_benchmark mpl_benchmark.cpp)


--------------------------------------------------------------------------------
/tests/jps_test.cpp:
--------------------------------------------------------------------------------
1 | //
2 | // Created by yash on 9/17/19.
3 | //
4 | 
5 | 
6 | 
7 | 


--------------------------------------------------------------------------------
/.gitignore:
--------------------------------------------------------------------------------
1 | /cmake-build-debug
2 | /cmake-build-release
3 | /.idea
4 | /include/benchmark


--------------------------------------------------------------------------------
/include/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | add_library(planner mpl/planner.h mpl/dfs.h mpl/bfs.h mpl/astar.h mpl/jps.h)
2 | 
3 | set_target_properties(planner PROPERTIES LINKER_LANGUAGE CXX)


--------------------------------------------------------------------------------
/src/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | add_library(utility STATIC mpl/eigen_utility.h mpl/common_utility.h mpl/grid_map_generator/random_grid_map_generator.h
2 |         mpl/visualization_utility.h)
3 | 
4 | set_target_properties(utility PROPERTIES LINKER_LANGUAGE CXX)
5 | 
6 | target_link_libraries(utility ${OpenCV_LIBS})


--------------------------------------------------------------------------------
/src/mpl/common_utility.h:
--------------------------------------------------------------------------------
 1 | //
 2 | // Created by yash on 9/14/19.
 3 | //
 4 | 
 5 | #pragma once
 6 | 
 7 | template <typename T>
 8 | struct crtp
 9 | {
10 |     T& underlying() { return static_cast<T&>(*this); }
11 |     T const& underlying() const { return static_cast<T const&>(*this); }
12 | };
13 | 
14 | 


--------------------------------------------------------------------------------
/examples/grid_map_generation.cpp:
--------------------------------------------------------------------------------
 1 | //
 2 | // Created by yash on 9/14/19.
 3 | //
 4 | 
 5 | #include "mpl/grid_map_generator/random_grid_map_generator.h"
 6 | #include "mpl/visualization_utility.h"
 7 | 
 8 | int main()
 9 | {
10 |     const auto grid_map = mpl::generate_map(400, 400, 10);
11 | 
12 |     mpl::display(grid_map);
13 | }
14 | 
15 | 


--------------------------------------------------------------------------------
/examples/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | add_executable(examples grid_map_generation.cpp)
 2 | target_link_libraries(examples utility)
 3 | 
 4 | add_executable(dfs dfs.cpp)
 5 | target_link_libraries(dfs utility)
 6 | 
 7 | add_executable(astar astar.cpp)
 8 | target_link_libraries(astar utility)
 9 | 
10 | add_executable(bfs bfs.cpp)
11 | target_link_libraries(bfs utility)
12 | 
13 | add_executable(jps jps.cpp)
14 | target_link_libraries(jps utility)


--------------------------------------------------------------------------------
/tests/graph_fixture.h:
--------------------------------------------------------------------------------
 1 | //
 2 | // Created by yash on 9/14/19.
 3 | //
 4 | 
 5 | #pragma once
 6 | 
 7 | #include "mpl/planner.h"
 8 | 
 9 | #include <gtest/gtest.h>
10 | 
11 | class GraphFixture : public ::testing::Test
12 | {
13 | protected:
14 |     GraphFixture()
15 |     {
16 |         standard_graph_ <<  0, 1, 0,
17 |                             1, 0, 1,
18 |                             0, 1, 0;
19 |     }
20 | 
21 |     Eigen::Matrix3d standard_graph_;
22 | };
23 | 


--------------------------------------------------------------------------------
/tests/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | add_library(graph_fixture graph_fixture.h)
 2 | set_target_properties(graph_fixture PROPERTIES LINKER_LANGUAGE CXX)
 3 | 
 4 | add_library(planner_fixture planner_fixture.h)
 5 | set_target_properties(planner_fixture PROPERTIES LINKER_LANGUAGE CXX)
 6 | 
 7 | add_executable(planner_test planner_test.cpp jps_test.cpp)
 8 | target_link_libraries(planner_test ${GTEST_LIBRARIES} pthread)
 9 | 
10 | add_executable(graph_test graph_test.cpp)
11 | target_link_libraries(graph_test ${GTEST_LIBRARIES} pthread)
12 | 
13 | 
14 | 
15 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # mpl
 2 | Motion Planning Library for Grid Maps 
 3 | 
 4 | This library aims to provide pathfinding algorithms for grid based maps which are thoroughly unit tested and benchmarked for use in Robot Path Planning and Game development.
 5 | 
 6 | Currently the library provides the following path planning algorithms to the users:
 7 | 1. Breadth First Search 
 8 | 2. Depth First Search 
 9 | 3. A-Star Heuristic Search 
10 | 4. Jump Point Search 
11 | 
12 | The following features are planned to be added in the future:
13 | 1. Faster and Flexible versions of A-star 
14 | 2. Sampling Based Algorithms for Larger Search Spaces
15 | 3. Support for 3 Dimensional Planning
16 | 


--------------------------------------------------------------------------------
/examples/dfs.cpp:
--------------------------------------------------------------------------------
 1 | //
 2 | // Created by yash on 9/14/19.
 3 | //
 4 | 
 5 | #include <mpl/visualization_utility.h>
 6 | #include "mpl/dfs.h"
 7 | #include "mpl/planner.h"
 8 | #include "mpl/grid_map_generator/random_grid_map_generator.h"
 9 | 
10 | int main()
11 | {
12 |     auto grid_map = mpl::generate_map(600, 600, 15);
13 | 
14 |     const auto planner = mpl::planner<mpl::dfs>(grid_map);
15 | 
16 |     const mpl::location_2d start(0, 0);
17 |     const mpl::location_2d goal(300, 300);
18 | 
19 |     const auto plan = planner.get_plan(start, goal);
20 | 
21 |     if(plan)
22 |     {
23 |         std::cout << "Path exist between Start and Goal";
24 |     }
25 | }
26 | 
27 | 
28 | 


--------------------------------------------------------------------------------
/examples/bfs.cpp:
--------------------------------------------------------------------------------
 1 | //
 2 | // Created by yash on 9/14/19.
 3 | //
 4 | 
 5 | #include "mpl/bfs.h"
 6 | #include "mpl/grid_map_generator/random_grid_map_generator.h"
 7 | #include "mpl/planner.h"
 8 | #include "mpl/visualization_utility.h"
 9 | 
10 | int main()
11 | {
12 |     auto grid_map = mpl::generate_map(600, 600, 10);
13 | 
14 |     const auto planner = mpl::planner<mpl::bfs>(grid_map);
15 | 
16 |     const mpl::location_2d start(0, 0);
17 |     const mpl::location_2d goal(100, 300);
18 | 
19 |     const auto plan = planner.get_plan(start, goal);
20 | 
21 |     if(plan)
22 |     {
23 |         std::cout << "Path exist between Start and Goal";
24 |         mpl::display(grid_map, *plan);
25 |     }
26 | }
27 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 3.14)
 2 | project(mpl)
 3 | 
 4 | set(CMAKE_CXX_STANDARD 17)
 5 | 
 6 | find_package(Boost 1.65 COMPONENTS program_options REQUIRED )
 7 | find_package(Eigen3 REQUIRED)
 8 | find_package(GTest REQUIRED)
 9 | find_package(OpenCV REQUIRED)
10 | find_package(PythonLibs 2.7)
11 | 
12 | include_directories("${googlebenchmark_SOURCE_DIR}/include")
13 | include_directories(${EIGEN3_INCLUDE_DIR})
14 | include_directories( ${Boost_INCLUDE_DIR} )
15 | include_directories(${GTEST_INCLUDE_DIRS})
16 | include_directories(${OpenCV_INCLUDE_DIRS})
17 | include_directories(../random_grid_map_generator)
18 | 
19 | include_directories(${PYTHON_INCLUDE_DIRS})
20 | 
21 | include_directories(./include ./src)
22 | 
23 | link_directories(${OpenCV_LIBRARY_DIRS})
24 | link_libraries(${PYTHON_LIBRARIES})
25 | 
26 | add_subdirectory(mpl_benchmark)
27 | add_subdirectory(examples)
28 | add_subdirectory(include)
29 | add_subdirectory(src)
30 | add_subdirectory(tests)


--------------------------------------------------------------------------------
/examples/jps.cpp:
--------------------------------------------------------------------------------
 1 | //
 2 | // Created by yash on 9/19/19.
 3 | //
 4 | 
 5 | #include "mpl/jps.h"
 6 | #include "mpl/grid_map_generator/random_grid_map_generator.h"
 7 | #include "mpl/planner.h"
 8 | #include "mpl/visualization_utility.h"
 9 | 
10 | static constexpr bool visualization = false;
11 | 
12 | int main()
13 | {
14 |     auto grid_map = mpl::generate_map(510, 500, 2);
15 | 
16 |     auto planner = mpl::planner<mpl::jps>(grid_map);
17 | 
18 |     const mpl::location_2d start(2, 2);
19 |     const mpl::location_2d goal(500, 499);
20 | 
21 |     const auto plan = planner.get_plan(start, goal);
22 | 
23 |     if constexpr (visualization)
24 |     {
25 |         if(plan)
26 |         {
27 |             const auto visualization_info = planner.get_visualization_info();
28 |             std::cout << "Path exist between Start and Goal";
29 |             mpl::display(grid_map, visualization_info);
30 |             return 0;
31 |         }
32 |     }
33 | 
34 |     if(plan)
35 |     {
36 |         std::cout << "Path exist between Start and Goal";
37 |         mpl::display(grid_map, *plan);
38 |     }
39 | }


--------------------------------------------------------------------------------
/examples/astar.cpp:
--------------------------------------------------------------------------------
 1 | //
 2 | // Created by yash on 9/14/19.
 3 | //
 4 | 
 5 | #include "mpl/astar.h"
 6 | #include "mpl/grid_map_generator/random_grid_map_generator.h"
 7 | #include "mpl/planner.h"
 8 | #include "mpl/visualization_utility.h"
 9 | 
10 | static constexpr bool visualization = false;
11 | 
12 | int main()
13 | {
14 |     auto grid_map = mpl::generate_map(4500, 4500, 2);
15 | 
16 |     auto planner = mpl::planner<mpl::astar>(grid_map);
17 | 
18 |     const mpl::location_2d start(2, 2);
19 |     const mpl::location_2d goal(4100, 4400);
20 | 
21 |     const auto plan = planner.get_plan(start, goal);
22 | 
23 |     if constexpr (visualization)
24 |     {
25 |         if(plan)
26 |         {
27 |             const auto visualization_info = planner.get_visualization_info();
28 |             std::cout << "Path exist between Start and Goal";
29 |             mpl::display(grid_map, visualization_info);
30 |             return 0;
31 |         }
32 |     }
33 | 
34 |     if(plan)
35 |     {
36 |         std::cout << "Path exist between Start and Goal";
37 |         mpl::display(grid_map, *plan);
38 |     }
39 | }
40 | 
41 | 
42 | 


--------------------------------------------------------------------------------
/src/mpl/grid_map_generator/random_grid_map_generator.h:
--------------------------------------------------------------------------------
 1 | //
 2 | // Created by yash on 9/14/19.
 3 | //
 4 | 
 5 | #pragma once
 6 | 
 7 | #include <Eigen/Dense>
 8 | #include <random>
 9 | #include <iostream>
10 | 
11 | namespace mpl
12 | {
13 | 
14 | Eigen::MatrixXd generate_map(size_t rows, size_t cols, size_t n_obstacles)
15 | {
16 |     Eigen::MatrixXd grid_map{Eigen::MatrixXd::Zero(rows, cols)};
17 | 
18 |     std::random_device r;
19 |     std::default_random_engine engine(r());
20 |     std::uniform_int_distribution<int> random_row(1, rows);
21 |     std::uniform_int_distribution<int> random_column(1, cols);
22 |     std::uniform_int_distribution<int> random_height(1, rows / 3);
23 |     std::uniform_int_distribution<int> random_width(1, cols / 3);
24 | 
25 |     while (n_obstacles > 0)
26 |     {
27 |         const auto upper_left_obstacle_row = random_row(engine);
28 |         const auto upper_left_obstacle_col = random_column(engine);
29 |         const auto obstacle_height = random_height(engine);
30 |         const auto obstacle_width = random_width(engine);
31 | 
32 |         for (size_t row_index = upper_left_obstacle_row, height = 0;
33 |              row_index < rows && height < obstacle_height;
34 |              row_index++, height++)
35 |             for (size_t col_index = upper_left_obstacle_col, width = 0; col_index < cols &&
36 |                                                                         width <
37 |                                                                         obstacle_width; col_index++, width++)
38 |             {
39 |                 grid_map(row_index, col_index) = 1;
40 |             }
41 | 
42 |         n_obstacles--;
43 |     }
44 | 
45 |     return grid_map;
46 | }
47 | 
48 | 
49 | }


--------------------------------------------------------------------------------
/include/mpl/planner.h:
--------------------------------------------------------------------------------
 1 | //
 2 | // Created by yash on 9/14/19.
 3 | //
 4 | 
 5 | #pragma once
 6 | 
 7 | #include "mpl/common_utility.h"
 8 | #include "mpl/eigen_utility.h"
 9 | #include "mpl/visualization_utility.h"
10 | 
11 | #include <Eigen/Dense>
12 | #include <iostream>
13 | 
14 | namespace mpl
15 | {
16 | 
17 | /// Use this flag to show visualization info
18 | /// You need to call
19 | constexpr bool mpl_visualization = false;
20 | 
21 | /// Planner Interface
22 | /// @tparam PlannerType
23 | template <typename PlannerType>
24 | class planner : public crtp<PlannerType>
25 | {
26 | public:
27 | 
28 |     explicit planner(Eigen::MatrixXd graph) : graph_(std::move(graph)),
29 |                                               n_rows(graph_.rows()),
30 |                                               n_cols(graph_.cols())
31 |     {
32 |     }
33 | 
34 |     /// Get the path from start to goal (Optimality depends on the PlannerType)
35 |     /// @param start
36 |     /// @param goal
37 |     /// @return std::vector<location_2d> if a path is found or std::null_opt
38 |     std::optional<std::vector<location_2d>> get_plan(const location_2d& start, const location_2d& goal)
39 |     {
40 |         if constexpr (!mpl_visualization)
41 |         {
42 |             return this->underlying().get_plan(start, goal);
43 |         }
44 |         else
45 |         {
46 |             return this->underlying().get_plan_with_visualization(start, goal);
47 |         }
48 |     }
49 | 
50 |     std::vector<viz_location_2d> get_visualization_info()
51 |     {
52 |         if constexpr (!mpl_visualization)
53 |         {
54 |             std::__throw_logic_error("Set mpl_visualization to true to call this function.");
55 |         }
56 |         else
57 |         {
58 |             if(explored_locations_.empty())
59 |             {
60 |                 std::cout << "No Path available yet. Make sure you call get_plan.";
61 |             }
62 |             return explored_locations_;
63 |         }
64 |     }
65 | 
66 |     const Eigen::MatrixXd graph_;
67 |     const int n_rows;
68 |     const int n_cols;
69 | 
70 |     std::vector<viz_location_2d> explored_locations_;
71 | };
72 | 
73 | } // namespace mpl
74 | 


--------------------------------------------------------------------------------
/include/mpl/dfs.h:
--------------------------------------------------------------------------------
 1 | //
 2 | // Created by yash on 9/14/19.
 3 | //
 4 | 
 5 | #pragma once
 6 | 
 7 | #include "mpl/eigen_utility.h"
 8 | #include "planner.h"
 9 | 
10 | #include <unordered_set>
11 | #include <stack>
12 | #include <iostream>
13 | 
14 | namespace mpl
15 | {
16 | 
17 | /// DFS Planner - Not a Good Planner for Grid Space! (Not Optimal)
18 | class dfs : public planner<dfs>
19 | {
20 | public:
21 |     std::optional<std::vector<location_2d>> get_plan(const location_2d& start, const location_2d& goal) const
22 |     {
23 |         if(graph_(start.row, start.col) == 1 )
24 |         {
25 |             std::cout << "Cannot Find Path. Start Position is Occupied.";
26 |             return std::nullopt;
27 |         }
28 |         if(graph_(goal.row, goal.col) == 1)
29 |         {
30 |             std::cout << "Cannot Find Path. End Position is Occupied.";
31 |             return std::nullopt;
32 |         }
33 | 
34 |         std::vector<location_2d> path;
35 |         std::unordered_set<location_2d> closed_set;
36 |         std::stack<location_2d> open_stack;
37 |         std::unordered_map<location_2d, location_2d> parent_from_node;
38 | 
39 |         open_stack.push(start);
40 | 
41 |         while(!open_stack.empty())
42 |         {
43 |             const auto current_location = open_stack.top();
44 |             open_stack.pop();
45 | 
46 |             if(current_location == goal)
47 |             {
48 |                 auto path_node = goal;
49 |                 while (path_node != start)
50 |                 {
51 |                     path.emplace_back(path_node);
52 |                     if(parent_from_node.find(path_node) == parent_from_node.end())
53 |                     {
54 |                         std::__throw_logic_error("No Parent for Discovered Node! Check Logic");
55 |                     }
56 |                     path_node = parent_from_node[path_node];
57 |                 }
58 |                 path.emplace_back(start);
59 |                 return path;
60 |             }
61 | 
62 |             for_all_adjacent_nodes(current_location, &graph_, [&](location_2d&& adjacent_loc){
63 |                 if(graph_(adjacent_loc.row, adjacent_loc.col) == 0 &&closed_set.find(adjacent_loc) == closed_set.end())
64 |                 {
65 |                     parent_from_node[adjacent_loc] = current_location;
66 |                     open_stack.push(adjacent_loc);
67 |                 }
68 |             });
69 | 
70 |             closed_set.insert(current_location);
71 |         }
72 |     }
73 | };
74 | 
75 | }
76 | 


--------------------------------------------------------------------------------
/include/mpl/bfs.h:
--------------------------------------------------------------------------------
 1 | //
 2 | // Created by yash on 9/14/19.
 3 | //
 4 | 
 5 | #pragma once
 6 | 
 7 | #include "planner.h"
 8 | 
 9 | #include <iostream>
10 | #include <queue>
11 | #include <unordered_set>
12 | 
13 | namespace mpl
14 | {
15 | 
16 | /// BFS Planner
17 | class bfs : public planner<bfs>
18 | {
19 | public:
20 |     std::optional<std::vector<location_2d>> get_plan(const location_2d& start, const location_2d& goal) const
21 |     {
22 |         if(graph_(start.row, start.col) == 1 )
23 |         {
24 |             std::cout << "Cannot Find Path. Start Position is Occupied. \n";
25 |             return std::nullopt;
26 |         }
27 |         if(graph_(goal.row, goal.col) == 1)
28 |         {
29 |             std::cout << "Cannot Find Path. End Position is Occupied. \n";
30 |             return std::nullopt;
31 |         }
32 | 
33 |         std::vector<location_2d> path;
34 |         std::unordered_set<location_2d> closed_set;
35 | 
36 |         std::queue<location_2d> open_queue;
37 |         std::unordered_set<location_2d> open_set;
38 | 
39 |         std::unordered_map<location_2d, location_2d> parent_from_node;
40 | 
41 |         open_queue.push(start);
42 |         open_set.insert(start);
43 | 
44 |         while(!open_queue.empty())
45 |         {
46 |             const auto current_location = open_queue.front();
47 |             open_queue.pop();
48 | 
49 |             if(current_location == goal)
50 |             {
51 |                 auto path_node = goal;
52 |                 while (path_node != start)
53 |                 {
54 |                     path.emplace_back(path_node);
55 |                     if(parent_from_node.find(path_node) == parent_from_node.end())
56 |                     {
57 |                         std::__throw_logic_error("No Parent for Discovered Node! Check Logic");
58 |                     }
59 |                     path_node = parent_from_node[path_node];
60 |                 }
61 |                 path.emplace_back(start);
62 |                 return path;
63 |             }
64 | 
65 |             for_all_adjacent_nodes(current_location, &graph_, [&](location_2d&& adjacent_loc){
66 | 
67 |                 if(graph_(adjacent_loc.row, adjacent_loc.col) == 0 && closed_set.find(adjacent_loc) == closed_set.end())
68 |                 {
69 |                     // If child not in open list
70 |                     if(open_set.find(adjacent_loc) == open_set.end())
71 |                     {
72 |                         parent_from_node[adjacent_loc] = current_location;
73 | 
74 |                         open_set.insert(adjacent_loc);
75 |                         open_queue.push(adjacent_loc);
76 |                     }
77 |                 }
78 |             });
79 | 
80 |             closed_set.insert(current_location);
81 |         }
82 | 
83 |         std::cout << "Path Not Found! \n";
84 |         return std::nullopt;
85 |     }
86 | };
87 | 
88 | }


--------------------------------------------------------------------------------
/mpl_benchmark/mpl_benchmark.cpp:
--------------------------------------------------------------------------------
 1 | #include "mpl/planner.h"
 2 | #include "mpl/jps.h"
 3 | #include "mpl/astar.h"
 4 | #include "mpl/grid_map_generator/random_grid_map_generator.h"
 5 | 
 6 | #include <iostream>
 7 | #include <chrono>
 8 | #include <random>
 9 | 
10 | static constexpr int n_iterations = 100;
11 | static constexpr int dim_x = 2000;
12 | static constexpr int dim_y = 2000;
13 | static constexpr int n_obstacles = 4;
14 | 
15 | static constexpr bool multi_map = false;
16 | static constexpr int n_map = 1;
17 | 
18 | int main()
19 | {
20 |     std::random_device dev;
21 |     std::mt19937 rng(dev());
22 |     std::uniform_int_distribution<std::mt19937::result_type> x_index_gen(0,dim_x-1);
23 |     std::uniform_int_distribution<std::mt19937::result_type> y_index_gen(0,dim_y-1);
24 | 
25 |     int astar_iters = 0;
26 |     int jps_iters = 0;
27 |     std::chrono::milliseconds astar_time;
28 |     std::chrono::milliseconds jps_time;
29 | 
30 |     std::vector<mpl::location_2d> start_indices;
31 |     for(int i=0; i<1000; ++i)
32 |     {
33 |         start_indices.emplace_back(mpl::location_2d{x_index_gen(rng), y_index_gen(rng)});
34 |     }
35 | 
36 |     std::vector<mpl::location_2d> end_indices;
37 |     for(int i=0; i<1000; ++i)
38 |     {
39 |         end_indices.emplace_back(mpl::location_2d{x_index_gen(rng), y_index_gen(rng)});
40 |     }
41 | 
42 |     if constexpr (!multi_map)
43 |     {
44 |         const auto grid_map = mpl::generate_map(dim_x, dim_y, n_obstacles);
45 | 
46 |         for(int i=0; i<n_iterations; i++)
47 |         {
48 |             auto planner1 = mpl::planner<mpl::astar>(grid_map);
49 | 
50 |             auto start = std::chrono::system_clock::now();
51 |             const auto plan1 = planner1.get_plan(start_indices[i], end_indices[i]);
52 |             auto end = std::chrono::system_clock::now();
53 | 
54 |             if(plan1 && plan1->size() > 50)
55 |             {
56 |                 astar_time = astar_time + std::chrono::duration_cast<std::chrono::milliseconds>(end-start);
57 |                 astar_iters++;
58 |             }
59 | 
60 |             auto planner2 = mpl::planner<mpl::jps>(grid_map);
61 | 
62 |             start = std::chrono::system_clock::now();
63 |             const auto plan2 = planner2.get_plan(start_indices[i], end_indices[i]);
64 |             end = std::chrono::system_clock::now();
65 | 
66 |             if(plan2 && plan2->size() > 50)
67 |             {
68 |                 jps_time = jps_time + std::chrono::duration_cast<std::chrono::milliseconds>(end-start);
69 |                 jps_iters++;
70 |             }
71 |         }
72 | 
73 |         const std::chrono::milliseconds average_astar_time = astar_time/astar_iters;
74 |         const std::chrono::milliseconds average_jps_time = jps_time/jps_iters;
75 | 
76 |         std::cout << "Average Time for A-Star in ms: "  << average_astar_time.count() << "\n";
77 |         std::cout << "Average Time for JPS in ms: " << average_jps_time.count() << "\n";
78 |     }
79 | 
80 | }


--------------------------------------------------------------------------------
/src/mpl/eigen_utility.h:
--------------------------------------------------------------------------------
  1 | //
  2 | // Created by yash on 9/14/19.
  3 | //
  4 | 
  5 | #pragma once
  6 | 
  7 | #include <boost/functional/hash.hpp>
  8 | #include <Eigen/Dense>
  9 | 
 10 | namespace mpl
 11 | {
 12 | 
 13 | /// Used to represent the location of an element in the eigen matrix
 14 | struct location_2d
 15 | {
 16 |     location_2d() = default;
 17 |     location_2d(int row, int col) : row(row), col(col) {}
 18 |     int row;
 19 |     int col;
 20 | 
 21 |     bool is_within_boundary(int max_rows, int max_cols) const
 22 |     {
 23 |         return !(row < 0 || row > max_rows - 1 || col < 0 || col > max_cols - 1);
 24 |     }
 25 | 
 26 |     friend std::size_t hash_value(location_2d const& location)
 27 |     {
 28 |         std::size_t seed = 0;
 29 |         boost::hash_combine(seed, location.row);
 30 |         boost::hash_combine(seed, location.col);
 31 |         return seed;
 32 |     }
 33 | 
 34 |     friend std::ostream& operator<<(std::ostream& os, const location_2d& location)
 35 |     {
 36 |         os << "(" << location.row << ", " << location.col <<")";
 37 |     }
 38 | };
 39 | 
 40 | inline bool operator==(const location_2d& lhs, const location_2d& rhs)
 41 | {
 42 |     return (lhs.row == rhs.row) && (lhs.col == rhs.col);
 43 | }
 44 | 
 45 | inline bool operator!=(const location_2d& lhs, const location_2d& rhs)
 46 | {
 47 |     return lhs.row != rhs.row || lhs.col != rhs.col;
 48 | }
 49 | 
 50 | inline bool operator==(const std::pair<location_2d, location_2d>& lhs, const std::pair<location_2d, location_2d>& rhs)
 51 | {
 52 |     return (lhs.first == rhs.first) && (lhs.second == rhs.second);
 53 | }
 54 | 
 55 | inline location_2d operator+(location_2d const& location1, location_2d const& location2)
 56 | {
 57 |     return location_2d(location1.row + location2.row, location1.col + location2.col);
 58 | }
 59 | 
 60 | inline location_2d operator-(location_2d const& location1, location_2d const& location2)
 61 | {
 62 |     return location_2d(location1.row - location2.row, location1.col - location2.col);
 63 | }
 64 | 
 65 | inline bool is_within_boundary(const location_2d& location, const int& rows, const int& cols)
 66 | {
 67 |     return !(location.row < 0 || location.row > rows - 1 || location.col < 0 || location.col > cols - 1);
 68 | }
 69 | 
 70 | constexpr std::array<std::pair<int, int>, 8> neighbors_2d{{ {-1, 0}, {0, -1}, {1, 0}, {0, 1}, {-1, -1},{-1, 1},
 71 |                                                            {1, -1},  {1, 1}}};
 72 | 
 73 | /// Gives Access to all the Adjacent Nodes of a point (Does Boundary Condition Checking)
 74 | /// @tparam Graph
 75 | /// @tparam Func
 76 | /// @param row
 77 | /// @param col
 78 | /// @param graph
 79 | /// @param func
 80 | template <typename Graph, typename Func>
 81 | void for_all_adjacent_nodes(const location_2d& loc, const Graph& graph, Func&& func)
 82 | {
 83 |     for(const auto& neighbor: neighbors_2d)
 84 |     {
 85 |         const auto new_row = loc.row + neighbor.first;
 86 |         const auto new_col = loc.col + neighbor.second;
 87 |         if(new_row < 0 || new_row > graph.rows()-1 || new_col < 0 || new_col > graph.cols()-1 || graph(new_row, new_col) == 1)
 88 |         {
 89 |             continue;
 90 |         }
 91 |         func(location_2d(new_row, new_col));
 92 |     }
 93 | }
 94 | 
 95 | /// Get L2 distance
 96 | /// @param start - location_2d*
 97 | /// @param goal - location_2d*
 98 | /// @return l2 distance (double)
 99 | inline double get_distance(const location_2d* start, const location_2d* goal)
100 | {
101 |     return sqrt(pow(start->row - goal->row, 2) + pow(start->col - goal->col, 2));
102 | }
103 | 
104 | } // namespace mpl
105 | 
106 | /// hash for location_2d
107 | namespace std
108 | {
109 |     template<> struct hash<mpl::location_2d> : boost::hash<mpl::location_2d> {};
110 | }
111 | 


--------------------------------------------------------------------------------
/tests/graph_test.cpp:
--------------------------------------------------------------------------------
 1 | //
 2 | // Created by yash on 9/14/19.
 3 | //
 4 | 
 5 | #include "graph_fixture.h"
 6 | 
 7 | #include <gtest/gtest.h>
 8 | 
 9 | TEST_F(GraphFixture, check_node_adjacency)
10 | {
11 |     auto create_adjacent_node_vectors = [&](mpl::location_2d&& location, auto graph_ptr)
12 |     {
13 |         std::vector<int> actual_rows_indices;
14 |         std::vector<int> actual_cols_indices;
15 |         mpl::for_all_adjacent_nodes(location, graph_ptr, [&](const auto adjacent_node){
16 |             actual_rows_indices.emplace_back(adjacent_node.row);
17 |             actual_cols_indices.emplace_back(adjacent_node.col);
18 |         });
19 |         return std::pair{actual_rows_indices, actual_cols_indices};
20 |     };
21 | 
22 |     std::vector<int> ground_truth_row_indices = {0, 1, 1};
23 |     std::vector<int> ground_truth_cols_indices = {1, 0, 1};
24 |     auto actual_node_indices_pair = create_adjacent_node_vectors(mpl::location_2d(0, 0), &standard_graph_);
25 |     EXPECT_EQ(ground_truth_row_indices, actual_node_indices_pair.first);
26 |     EXPECT_EQ(ground_truth_cols_indices, actual_node_indices_pair.second);
27 | 
28 |     ground_truth_row_indices = {0, 0, 1, 1, 1};
29 |     ground_truth_cols_indices = {0, 2, 0, 1, 2};
30 |     actual_node_indices_pair = create_adjacent_node_vectors(mpl::location_2d(0, 1), &standard_graph_);
31 |     EXPECT_EQ(ground_truth_row_indices, actual_node_indices_pair.first);
32 |     EXPECT_EQ(ground_truth_cols_indices, actual_node_indices_pair.second);
33 | 
34 |     ground_truth_row_indices = {0, 1, 1};
35 |     ground_truth_cols_indices = {1, 1, 2};
36 |     actual_node_indices_pair = create_adjacent_node_vectors(mpl::location_2d(0, 2), &standard_graph_);
37 |     EXPECT_EQ(ground_truth_row_indices, actual_node_indices_pair.first);
38 |     EXPECT_EQ(ground_truth_cols_indices, actual_node_indices_pair.second);
39 | 
40 |     ground_truth_row_indices = {0, 0, 1, 2, 2};
41 |     ground_truth_cols_indices = {0, 1, 1, 0, 1};
42 |     actual_node_indices_pair = create_adjacent_node_vectors(mpl::location_2d(1, 0), &standard_graph_);
43 |     EXPECT_EQ(ground_truth_row_indices, actual_node_indices_pair.first);
44 |     EXPECT_EQ(ground_truth_cols_indices, actual_node_indices_pair.second);
45 | 
46 |     ground_truth_row_indices = {0, 0, 0, 1, 1, 2, 2, 2};
47 |     ground_truth_cols_indices = {0, 1, 2, 0, 2, 0, 1, 2};
48 |     actual_node_indices_pair = create_adjacent_node_vectors(mpl::location_2d(1, 1), &standard_graph_);
49 |     EXPECT_EQ(ground_truth_row_indices, actual_node_indices_pair.first);
50 |     EXPECT_EQ(ground_truth_cols_indices, actual_node_indices_pair.second);
51 | 
52 |     ground_truth_row_indices = {0, 0, 1, 2, 2};
53 |     ground_truth_cols_indices = {1, 2, 1, 1, 2};
54 |     actual_node_indices_pair = create_adjacent_node_vectors(mpl::location_2d(1, 2), &standard_graph_);
55 |     EXPECT_EQ(ground_truth_row_indices, actual_node_indices_pair.first);
56 |     EXPECT_EQ(ground_truth_cols_indices, actual_node_indices_pair.second);
57 | 
58 |     ground_truth_row_indices = {1, 1, 2};
59 |     ground_truth_cols_indices = {0 , 1, 1};
60 |     actual_node_indices_pair = create_adjacent_node_vectors(mpl::location_2d(2, 0), &standard_graph_);
61 |     EXPECT_EQ(ground_truth_row_indices, actual_node_indices_pair.first);
62 |     EXPECT_EQ(ground_truth_cols_indices, actual_node_indices_pair.second);
63 | 
64 |     ground_truth_row_indices = {1, 1, 1, 2, 2};
65 |     ground_truth_cols_indices = {0, 1, 2, 0, 2};
66 |     actual_node_indices_pair = create_adjacent_node_vectors(mpl::location_2d(2, 1), &standard_graph_);
67 |     EXPECT_EQ(ground_truth_row_indices, actual_node_indices_pair.first);
68 |     EXPECT_EQ(ground_truth_cols_indices, actual_node_indices_pair.second);
69 | 
70 |     ground_truth_row_indices = {1, 1, 2};
71 |     ground_truth_cols_indices = {1, 2, 1};
72 |     actual_node_indices_pair = create_adjacent_node_vectors(mpl::location_2d(2, 2), &standard_graph_);
73 |     EXPECT_EQ(ground_truth_row_indices, actual_node_indices_pair.first);
74 |     EXPECT_EQ(ground_truth_cols_indices, actual_node_indices_pair.second);
75 | }
76 | 
77 | int main(int argc, char **argv)
78 | {
79 |     testing::InitGoogleTest(&argc, argv);
80 |     return RUN_ALL_TESTS();
81 | }
82 | 
83 | 


--------------------------------------------------------------------------------
/tests/planner_fixture.h:
--------------------------------------------------------------------------------
  1 | //
  2 | // Created by yash on 9/14/19.
  3 | //
  4 | 
  5 | #pragma once
  6 | 
  7 | #include "mpl/astar.h"
  8 | #include "mpl/jps.h"
  9 | #include "mpl/planner.h"
 10 | 
 11 | #include <gtest/gtest.h>
 12 | 
 13 | class GraphFixture : public ::testing::Test
 14 | {
 15 | protected:
 16 |     GraphFixture()
 17 |     {
 18 |         search_graph_ <<  0, 0, 0, 1 , 0,
 19 |                 0, 0, 1, 1 , 0,
 20 |                 0, 0, 0, 1 , 1,
 21 |                 0, 0, 1, 1 , 0,
 22 |                 1, 0, 0, 0 , 0;
 23 |     }
 24 | 
 25 |     Eigen::Matrix<double , 5, 5> search_graph_;
 26 | };
 27 | 
 28 | class AstarPlannerFixture : public GraphFixture
 29 | {
 30 | protected:
 31 |     AstarPlannerFixture() : GraphFixture(), planner_(search_graph_)
 32 |     {
 33 |     }
 34 | 
 35 |     mpl::planner<mpl::astar> planner_;
 36 | };
 37 | 
 38 | 
 39 | class JPSGraphFixture : public ::testing::Test
 40 | {
 41 | protected:
 42 |     JPSGraphFixture() :
 43 |             empty_graph_(Eigen::MatrixXd::Zero(10, 10))
 44 |     {
 45 |         single_obstacle_graph_1 <<  0, 0, 1, 0 , 0,
 46 |                                         0, 0, 0, 0 , 0,
 47 |                                         0, 0, 0, 0 , 0,
 48 |                                         0, 0, 0, 0 , 0,
 49 |                                         0, 0, 0, 0 , 0;
 50 |         single_obstacle_graph_2 <<  0, 0, 0, 0 , 0,
 51 |                                     0, 0, 0, 0 , 0,
 52 |                                     1, 0, 0, 0 , 0,
 53 |                                     0, 0, 0, 0 , 0,
 54 |                                     0, 0, 0, 0 , 0;
 55 |         multiple_obstacle_graph_1 <<  0, 0, 0, 0 , 0,
 56 |                                     1, 0, 1, 1 , 0,
 57 |                                     0, 0, 0, 1 , 0,
 58 |                                     0, 0, 1, 1, 1,
 59 |                                     1, 0, 0, 0 , 0;
 60 |         multiple_obstacle_graph_2 <<  0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
 61 |                                       0, 0, 1, 1, 0, 0, 0, 0, 0, 0,
 62 |                                       0, 0, 1, 1, 0, 0, 0, 0, 0, 0,
 63 |                                       0, 0, 1, 1, 0, 0, 1, 1, 1, 0,
 64 |                                       0, 0, 0, 0, 0, 0, 1, 1, 1, 0,
 65 |                                       0, 0, 0, 0, 0, 0, 0, 1, 1, 1,
 66 |                                       0, 0, 1, 1, 1, 0, 0, 0, 1, 1,
 67 |                                       0, 0, 0, 1, 1, 0, 0, 0, 0, 0,
 68 |                                       0, 0, 1, 1, 1, 0, 0, 0, 0, 0,
 69 |                                       0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
 70 |         single_obstacle_graph_top_left_ <<  1, 0, 0,
 71 |                                             0, 0, 0,
 72 |                                             0, 0, 0;
 73 |         single_obstacle_graph_top_right_ <<  0, 0, 1,
 74 |                                             0, 0, 0,
 75 |                                             0, 0, 0;
 76 |         single_obstacle_graph_bottom_left_ <<  0, 0, 0,
 77 |                                                 0, 0, 0,
 78 |                                                 1, 0, 0;
 79 |         single_obstacle_graph_bottom_right_ <<  0, 0, 0,
 80 |                                                 0, 0, 0,
 81 |                                                 0, 0, 1;
 82 |     }
 83 | 
 84 |     Eigen::MatrixXd empty_graph_;
 85 |     Eigen::Matrix<double , 5, 5> single_obstacle_graph_1;
 86 |     Eigen::Matrix<double , 5, 5> single_obstacle_graph_2;
 87 |     Eigen::Matrix<double , 5, 5> multiple_obstacle_graph_1;
 88 |     Eigen::Matrix<double , 10, 10> multiple_obstacle_graph_2;
 89 |     Eigen::Matrix<double , 3, 3> single_obstacle_graph_top_left_;
 90 |     Eigen::Matrix<double , 3, 3> single_obstacle_graph_top_right_;
 91 |     Eigen::Matrix<double , 3, 3> single_obstacle_graph_bottom_left_;
 92 |     Eigen::Matrix<double , 3, 3> single_obstacle_graph_bottom_right_;
 93 | };
 94 | 
 95 | class JPSPlannerFixture : public JPSGraphFixture
 96 | {
 97 | protected:
 98 |     JPSPlannerFixture() : JPSGraphFixture(),
 99 |                           empty_planner_(this->empty_graph_),
100 |                           single_obstacle_planner_1(this->single_obstacle_graph_1),
101 |                           single_obstacle_planner_2(this->single_obstacle_graph_2),
102 |                           multiple_obstacle_planner_1(this->multiple_obstacle_graph_1),
103 |                           multiple_obstacle_planner_2(this->multiple_obstacle_graph_2),
104 |                           single_obstacle_planner_top_left_(this->single_obstacle_graph_top_left_),
105 |                           single_obstacle_planner_top_right_(this->single_obstacle_graph_top_right_),
106 |                           single_obstacle_planner_bottom_left_(this->single_obstacle_graph_bottom_left_),
107 |                           single_obstacle_planner_bottom_right_(this->single_obstacle_graph_bottom_right_)
108 |     {}
109 | 
110 |     mpl::planner<mpl::jps> empty_planner_;
111 |     mpl::planner<mpl::jps> single_obstacle_planner_1;
112 |     mpl::planner<mpl::jps> single_obstacle_planner_2;
113 |     mpl::planner<mpl::jps> multiple_obstacle_planner_1;
114 |     mpl::planner<mpl::jps> multiple_obstacle_planner_2;
115 |     mpl::planner<mpl::jps> single_obstacle_planner_top_left_;
116 |     mpl::planner<mpl::jps> single_obstacle_planner_top_right_;
117 |     mpl::planner<mpl::jps> single_obstacle_planner_bottom_left_;
118 |     mpl::planner<mpl::jps> single_obstacle_planner_bottom_right_;
119 | };
120 | 


--------------------------------------------------------------------------------
/src/mpl/visualization_utility.h:
--------------------------------------------------------------------------------
  1 | //
  2 | // Created by yash on 9/14/19.
  3 | //
  4 | 
  5 | #pragma once
  6 | 
  7 | #include <Eigen/Core>
  8 | #include <opencv2/core.hpp>
  9 | #include <opencv2/core/eigen.hpp>
 10 | #include <opencv2/imgcodecs.hpp>
 11 | #include <opencv2/imgproc/imgproc.hpp>
 12 | #include <opencv2/highgui.hpp>
 13 | #include <iostream>
 14 | 
 15 | 
 16 | namespace mpl
 17 | {
 18 | 
 19 | /// Used for visualization (mpl_visualization must be set to true)
 20 | /// General Color Terminology
 21 | /// Explored Nodes: "blue"
 22 | /// Explored Nodes but not added to Open Set: "red"
 23 | /// Start: "green"
 24 | struct viz_location_2d : public location_2d
 25 | {
 26 |     viz_location_2d() = delete;
 27 |     viz_location_2d(const location_2d& loc, int r, int g, int b): location_2d(loc), r(r), g(g), b(b)
 28 |     {}
 29 |     viz_location_2d(const location_2d& loc, const std::string& color) : location_2d(loc)
 30 |     {
 31 |         if(color == "red")
 32 |         {
 33 |             r = 255;
 34 |             b = 0;
 35 |             g = 0;
 36 |         }
 37 |         else if(color == "blue")
 38 |         {
 39 |             r = 0;
 40 |             b = 255;
 41 |             g = 0;
 42 |         }
 43 |         else if(color == "green")
 44 |         {
 45 |             r = 0;
 46 |             b = 0;
 47 |             g = 255;
 48 |         }
 49 |         else if(color == "yellow")
 50 |         {
 51 |             r = 255;
 52 |             b = 0;
 53 |             g = 255;
 54 |         }
 55 |         else if(color == "orange")
 56 |         {
 57 |             r = 255;
 58 |             b = 0;
 59 |             g = 69;
 60 |         }
 61 |         else
 62 |         {
 63 |             std::__throw_invalid_argument("Color not available: Use red, green, blue, or pass r, g, b values");
 64 |         }
 65 |     }
 66 |     int r;
 67 |     int g;
 68 |     int b;
 69 | };
 70 | 
 71 | 
 72 | /// Display Eigen Graph
 73 | /// @tparam Graph
 74 | /// @param graph
 75 | template <typename Graph>
 76 | void display(const Graph& graph)
 77 | {
 78 |     using namespace cv;
 79 |     Mat image;
 80 |     eigen2cv(graph, image);
 81 |     imshow( "Current ContainerType", image );
 82 |     waitKey(0);
 83 | }
 84 | 
 85 | ///
 86 | /// @tparam Path
 87 | /// @param graph
 88 | /// @param path
 89 | template <typename Graph, typename Path>
 90 | void display(Graph& graph, const Path& path)
 91 | {
 92 |     using namespace cv;
 93 |     Mat image;
 94 |     for(const auto& node:path)
 95 |     {
 96 |         graph(node.row, node.col) = 0.5;
 97 |     }
 98 | 
 99 |     eigen2cv(graph, image);
100 | 
101 |     imshow( "Current ContainerType", image );
102 |     waitKey(0);
103 | }
104 | 
105 | 
106 | /// Displays only 2d visual information
107 | /// @tparam Path
108 | /// @param graph
109 | /// @param path
110 | template <typename Graph>
111 | void display(Graph& graph, const std::vector<viz_location_2d>& visualization_info)
112 | {
113 |     for(int i=0; i<graph.rows(); i++)
114 |     {
115 |         for(int j=0; j<graph.cols(); ++j)
116 |         {
117 |             if(graph(i, j) == 1)
118 |             {
119 |                 graph(i, j) = 255;
120 |             }
121 |         }
122 |     }
123 | 
124 |     cv::Mat image;
125 |     eigen2cv(graph, image);
126 | 
127 |     std::cout << "Mat type = " << image.type();
128 |     cv::Mat image32;
129 |     image.cv::Mat::convertTo(image32, CV_8UC1);
130 | 
131 |     cv::Mat color(image32.rows,image32.cols, CV_8UC3, cv::Scalar(0,0,0));
132 | 
133 |     cv::cvtColor(image32, color, CV_GRAY2BGR);
134 | 
135 |     for(const auto& viz_node:visualization_info)
136 |     {
137 |         if(viz_node.b == 0 && viz_node.g == 255 && viz_node.r == 255)
138 |         {
139 |             if(viz_node.row > 0 && viz_node.row<color.rows && viz_node.col > 0 && viz_node.col<color.cols)
140 |             {
141 |                 color.at<cv::Vec3b>(viz_node.row+1, viz_node.col) = cv::Vec3b(viz_node.b, viz_node.g, viz_node.r);
142 |                 color.at<cv::Vec3b>(viz_node.row-1, viz_node.col) = cv::Vec3b(viz_node.b, viz_node.g, viz_node.r);
143 |                 color.at<cv::Vec3b>(viz_node.row, viz_node.col+1) = cv::Vec3b(viz_node.b, viz_node.g, viz_node.r);
144 |                 color.at<cv::Vec3b>(viz_node.row, viz_node.col-1) = cv::Vec3b(viz_node.b, viz_node.g, viz_node.r);
145 |             }
146 |         }
147 |         if(viz_node.b == 0 && viz_node.g == 255 && viz_node.r == 0)
148 |         {
149 |             if(viz_node.row > 0 && viz_node.row<color.rows && viz_node.col > 0 && viz_node.col<color.cols)
150 |             {
151 |                 color.at<cv::Vec3b>(viz_node.row+1, viz_node.col) = cv::Vec3b(viz_node.b, viz_node.g, viz_node.r);
152 |                 color.at<cv::Vec3b>(viz_node.row-1, viz_node.col) = cv::Vec3b(viz_node.b, viz_node.g, viz_node.r);
153 |                 color.at<cv::Vec3b>(viz_node.row, viz_node.col+1) = cv::Vec3b(viz_node.b, viz_node.g, viz_node.r);
154 |                 color.at<cv::Vec3b>(viz_node.row, viz_node.col-1) = cv::Vec3b(viz_node.b, viz_node.g, viz_node.r);
155 |             }
156 |         }
157 |         if(viz_node.r == 255)
158 |         {
159 |             if(viz_node.row > 0 && viz_node.row<color.rows && viz_node.col > 0 && viz_node.col<color.cols)
160 |             {
161 |                 color.at<cv::Vec3b>(viz_node.row+1, viz_node.col) = cv::Vec3b(viz_node.b, viz_node.g, viz_node.r);
162 |                 color.at<cv::Vec3b>(viz_node.row-1, viz_node.col) = cv::Vec3b(viz_node.b, viz_node.g, viz_node.r);
163 |                 color.at<cv::Vec3b>(viz_node.row, viz_node.col+1) = cv::Vec3b(viz_node.b, viz_node.g, viz_node.r);
164 |                 color.at<cv::Vec3b>(viz_node.row, viz_node.col-1) = cv::Vec3b(viz_node.b, viz_node.g, viz_node.r);
165 |             }
166 |         }
167 |         else
168 |         {
169 |             color.at<cv::Vec3b>(viz_node.row, viz_node.col) = cv::Vec3b(viz_node.b, viz_node.g, viz_node.r);
170 |         }
171 |     }
172 | 
173 |     imshow( "Current ContainerType", color );
174 |     cv::waitKey(0);
175 | }
176 | 
177 | }
178 | 


--------------------------------------------------------------------------------
/tests/planner_test.cpp:
--------------------------------------------------------------------------------
  1 | //
  2 | // Created by yash on 9/14/19.
  3 | //
  4 | 
  5 | #include "planner_fixture.h"
  6 | 
  7 | #include <gtest/gtest.h>
  8 | 
  9 | TEST_F(AstarPlannerFixture, invalid_endpoints)
 10 | {
 11 |     auto plan = planner_.get_plan(mpl::location_2d(0, 0), mpl::location_2d(3, 3));
 12 |     if(!plan) EXPECT_TRUE(true);
 13 |     else EXPECT_TRUE(false);
 14 | 
 15 |     plan = planner_.get_plan(mpl::location_2d(3, 3), mpl::location_2d(0, 0));
 16 |     if(!plan) EXPECT_TRUE(true);
 17 |     else EXPECT_TRUE(false);
 18 | 
 19 |     plan = planner_.get_plan(mpl::location_2d(3, 3), mpl::location_2d(0, 3));
 20 |     if(!plan) EXPECT_TRUE(true);
 21 |     else EXPECT_TRUE(false);
 22 | }
 23 | 
 24 | TEST_F(AstarPlannerFixture, shortest_path_size)
 25 | {
 26 |     auto plan = planner_.get_plan(mpl::location_2d(0, 0), mpl::location_2d(4, 4));
 27 |     if(!plan) EXPECT_TRUE(false);
 28 |     else EXPECT_EQ(plan->size(), 7);
 29 | 
 30 |     plan = planner_.get_plan(mpl::location_2d(2, 2), mpl::location_2d(4, 4));
 31 |     if(!plan) EXPECT_TRUE(false);
 32 |     else EXPECT_EQ(plan->size(), 5);
 33 | 
 34 |     plan = planner_.get_plan(mpl::location_2d(1, 0), mpl::location_2d(4, 3));
 35 |     if(!plan) EXPECT_TRUE(false);
 36 |     else EXPECT_EQ(plan->size(), 5);
 37 | }
 38 | 
 39 | TEST_F(AstarPlannerFixture, destination_unreachable)
 40 | {
 41 |     auto plan = planner_.get_plan(mpl::location_2d(0, 4), mpl::location_2d(4, 4));
 42 |     if(!plan) EXPECT_TRUE(true);
 43 |     else EXPECT_TRUE(false);
 44 | 
 45 |     plan = planner_.get_plan(mpl::location_2d(2, 2), mpl::location_2d(1, 4));
 46 |     if(!plan) EXPECT_TRUE(true);
 47 |     else EXPECT_TRUE(false);
 48 | }
 49 | 
 50 | TEST_F(JPSPlannerFixture, empty_graph_shortest_path)
 51 | {
 52 |     auto plan = empty_planner_.get_plan(mpl::location_2d(0, 0), mpl::location_2d(9, 9));
 53 |     if(!plan) EXPECT_TRUE(false);
 54 |     else EXPECT_EQ(plan->size(), 10);
 55 | 
 56 |     plan = empty_planner_.get_plan(mpl::location_2d(9, 9), mpl::location_2d(0, 0));
 57 |     if(!plan) EXPECT_TRUE(false);
 58 |     else EXPECT_EQ(plan->size(), 10);
 59 | 
 60 |     plan = empty_planner_.get_plan(mpl::location_2d(5, 5), mpl::location_2d(3, 9));
 61 |     if(!plan) EXPECT_TRUE(false);
 62 |     else EXPECT_EQ(plan->size(), 5);
 63 | 
 64 |     plan = empty_planner_.get_plan(mpl::location_2d(2, 8), mpl::location_2d(7, 5));
 65 |     if(!plan) EXPECT_TRUE(false);
 66 |     else EXPECT_EQ(plan->size(), 6);
 67 | 
 68 |     plan = empty_planner_.get_plan(mpl::location_2d(1, 1), mpl::location_2d(4, 8));
 69 |     if(!plan) EXPECT_TRUE(false);
 70 |     else EXPECT_EQ(plan->size(), 8);
 71 | 
 72 |     //TODO: Fix this failing test
 73 |     plan = empty_planner_.get_plan(mpl::location_2d(0, 0), mpl::location_2d(1, 1));
 74 |     if(!plan) EXPECT_TRUE(false);
 75 |     else EXPECT_EQ(plan->size(), 2);
 76 | 
 77 |     plan = empty_planner_.get_plan(mpl::location_2d(0, 0), mpl::location_2d(0, 0));
 78 |     if(!plan) EXPECT_TRUE(false);
 79 |     else EXPECT_EQ(plan->size(), 1);
 80 | 
 81 | }
 82 | 
 83 | TEST_F(JPSPlannerFixture, single_cell_obstacle5x5)
 84 | {
 85 |     auto plan = single_obstacle_planner_1.get_plan(mpl::location_2d(0, 0), mpl::location_2d(4, 4));
 86 |     if(!plan) EXPECT_TRUE(false);
 87 |     else EXPECT_EQ(plan->size(), 5);
 88 | 
 89 |     plan = single_obstacle_planner_1.get_plan(mpl::location_2d(4, 4), mpl::location_2d(0, 0));
 90 |     if(!plan) EXPECT_TRUE(false);
 91 |     else EXPECT_EQ(plan->size(), 5);
 92 | 
 93 |     plan = single_obstacle_planner_1.get_plan(mpl::location_2d(1, 1), mpl::location_2d(3, 4));
 94 |     if(!plan) EXPECT_TRUE(false);
 95 |     else EXPECT_EQ(plan->size(), 4);
 96 | 
 97 |     plan = single_obstacle_planner_1.get_plan(mpl::location_2d(2, 4), mpl::location_2d(1, 0));
 98 |     if(!plan) EXPECT_TRUE(false);
 99 |     else EXPECT_EQ(plan->size(), 5);
100 | 
101 |     plan = single_obstacle_planner_1.get_plan(mpl::location_2d(1, 4), mpl::location_2d(0, 0));
102 |     if(!plan) EXPECT_TRUE(false);
103 |     else EXPECT_EQ(plan->size(), 5);
104 | 
105 |     plan = single_obstacle_planner_1.get_plan(mpl::location_2d(0, 0), mpl::location_2d(1, 1));
106 |     if(!plan) EXPECT_TRUE(false);
107 |     else EXPECT_EQ(plan->size(), 2);
108 | 
109 |     plan = single_obstacle_planner_1.get_plan(mpl::location_2d(0, 0), mpl::location_2d(0, 0));
110 |     if(!plan) EXPECT_TRUE(false);
111 |     else EXPECT_EQ(plan->size(), 1);
112 | 
113 |     plan = single_obstacle_planner_2.get_plan(mpl::location_2d(0, 0), mpl::location_2d(4, 4));
114 |     if(!plan) EXPECT_TRUE(false);
115 |     else EXPECT_EQ(plan->size(), 5);
116 | 
117 |     plan = single_obstacle_planner_2.get_plan(mpl::location_2d(4, 4), mpl::location_2d(0, 0));
118 |     if(!plan) EXPECT_TRUE(false);
119 |     else EXPECT_EQ(plan->size(), 5);
120 | 
121 |     plan = single_obstacle_planner_2.get_plan(mpl::location_2d(1, 1), mpl::location_2d(3, 4));
122 |     if(!plan) EXPECT_TRUE(false);
123 |     else EXPECT_EQ(plan->size(), 4);
124 | 
125 |     plan = single_obstacle_planner_2.get_plan(mpl::location_2d(2, 4), mpl::location_2d(1, 0));
126 |     if(!plan) EXPECT_TRUE(false);
127 |     else EXPECT_EQ(plan->size(), 5);
128 | 
129 |     //TODO: Find a solution to get an optimal path in this test case
130 |     plan = single_obstacle_planner_2.get_plan(mpl::location_2d(1, 4), mpl::location_2d(0, 0));
131 |     if(!plan) EXPECT_TRUE(false);
132 |     else EXPECT_EQ(plan->size(), 5);
133 | 
134 |     plan = single_obstacle_planner_2.get_plan(mpl::location_2d(0, 0), mpl::location_2d(1, 1));
135 |     if(!plan) EXPECT_TRUE(false);
136 |     else EXPECT_EQ(plan->size(), 2);
137 | 
138 |     plan = single_obstacle_planner_2.get_plan(mpl::location_2d(0, 0), mpl::location_2d(0, 0));
139 |     if(!plan) EXPECT_TRUE(false);
140 |     else EXPECT_EQ(plan->size(), 1);
141 | }
142 | 
143 | TEST_F(JPSPlannerFixture, multiple_obstacles5x5)
144 | {
145 |     auto plan = multiple_obstacle_planner_1.get_plan(mpl::location_2d(0, 0), mpl::location_2d(4, 4));
146 |     if(!plan) EXPECT_TRUE(false);
147 |     else EXPECT_EQ(plan->size(), 7);
148 | 
149 |     plan = multiple_obstacle_planner_1.get_plan(mpl::location_2d(2, 0), mpl::location_2d(2, 4));
150 |     if(!plan) EXPECT_TRUE(false);
151 |     else EXPECT_EQ(plan->size(), 6);
152 | }
153 | 
154 | TEST_F(JPSPlannerFixture, multiple_obstacles10x10)
155 | {
156 |     auto plan = multiple_obstacle_planner_2.get_plan(mpl::location_2d(0, 0), mpl::location_2d(9, 9));
157 |     if(!plan) EXPECT_TRUE(false);
158 |     else EXPECT_EQ(plan->size(), 12);
159 | 
160 |     plan = multiple_obstacle_planner_2.get_plan(mpl::location_2d(9, 9), mpl::location_2d(0, 0));
161 |     if(!plan) EXPECT_TRUE(false);
162 |     else EXPECT_EQ(plan->size(), 12);
163 | }
164 | 
165 | int main(int argc, char **argv)
166 | {
167 |     testing::InitGoogleTest(&argc, argv);
168 |     return RUN_ALL_TESTS();
169 | }


--------------------------------------------------------------------------------
/include/mpl/astar.h:
--------------------------------------------------------------------------------
  1 | //
  2 | // Created by yash on 9/14/19.
  3 | //
  4 | 
  5 | #pragma once
  6 | 
  7 | #include "planner.h"
  8 | 
  9 | #include <iostream>
 10 | #include <queue>
 11 | #include <unordered_set>
 12 | 
 13 | namespace mpl
 14 | {
 15 | 
 16 | /// Astar Planner
 17 | class astar : public planner<astar>
 18 | {
 19 | public:
 20 |     std::optional<std::vector<location_2d>> get_plan(const location_2d& start, const location_2d& goal) const
 21 |     {
 22 |         if(graph_(start.row, start.col) != 0)
 23 |         {
 24 |             std::cout << "Cannot Find Path. Start Position is Occupied. \n";
 25 |             return std::nullopt;
 26 |         }
 27 |         const auto x = graph_(goal.row, goal.col);
 28 |         if(graph_(goal.row, goal.col) != 0)
 29 |         {
 30 |             std::cout << "Cannot Find Path. End Position is Occupied. \n";
 31 |             return std::nullopt;
 32 |         }
 33 | 
 34 |         std::vector<location_2d> path;
 35 |         std::unordered_set<location_2d> closed_set;
 36 | 
 37 |         std::unordered_map<location_2d, double> f_costs;
 38 |         std::unordered_map<location_2d, double> g_costs;
 39 | 
 40 |         auto less = [&](const location_2d& left, const location_2d& right) {
 41 |             return f_costs[left] > f_costs[right];
 42 |         };
 43 | 
 44 |         std::priority_queue<location_2d, std::vector<location_2d>, decltype(less)> open_queue(less);
 45 |         std::unordered_set<location_2d> open_set;
 46 | 
 47 |         std::unordered_map<location_2d, location_2d> parent_from_node;
 48 | 
 49 |         open_queue.push(start);
 50 |         open_set.insert(start);
 51 | 
 52 |         while(!open_queue.empty())
 53 |         {
 54 |             const auto current_location = open_queue.top();
 55 |             open_queue.pop();
 56 | 
 57 |             if(current_location == goal)
 58 |             {
 59 |                 auto path_node = goal;
 60 |                 while (path_node != start)
 61 |                 {
 62 |                     path.emplace_back(path_node);
 63 |                     if(parent_from_node.find(path_node) == parent_from_node.end())
 64 |                     {
 65 |                         std::__throw_logic_error("No Parent for Discovered Node! Check Logic");
 66 |                     }
 67 |                     path_node = parent_from_node[path_node];
 68 |                 }
 69 |                 path.emplace_back(start);
 70 |                 return path;
 71 |             }
 72 | 
 73 |             for_all_adjacent_nodes(current_location, graph_, [&](location_2d&& adjacent_loc){
 74 | 
 75 |                 if(graph_(adjacent_loc.row, adjacent_loc.col) == 0 &&closed_set.find(adjacent_loc) == closed_set.end())
 76 |                 {
 77 | 
 78 |                     const auto node_transition_cost = mpl::get_distance(&current_location, &adjacent_loc);
 79 | 
 80 |                     // If child not in open list
 81 |                     if(open_set.find(adjacent_loc) == open_set.end())
 82 |                     {
 83 |                         g_costs[adjacent_loc] = g_costs[current_location] + node_transition_cost;
 84 | 
 85 |                         const auto heuristic_cost = mpl::get_distance(&adjacent_loc, &goal);
 86 | 
 87 |                         f_costs[adjacent_loc] = g_costs[adjacent_loc] + heuristic_cost;
 88 | 
 89 |                         parent_from_node[adjacent_loc] = current_location;
 90 | 
 91 |                         open_set.insert(adjacent_loc);
 92 |                         open_queue.push(adjacent_loc);
 93 |                     }
 94 |                     // Else if child in open list
 95 |                     else
 96 |                     {
 97 |                         // cost of current child less than the cost of same child in open list
 98 |                         if(const auto g_cost_adjacent = g_costs[current_location] + node_transition_cost;
 99 |                         g_costs[adjacent_loc] > g_cost_adjacent)
100 |                         {
101 |                             g_costs[adjacent_loc] = g_cost_adjacent;
102 | 
103 |                             // replace the parent as current and cost with current cost
104 |                             parent_from_node[adjacent_loc] = current_location;
105 |                         }
106 |                     }
107 |                 }
108 |             });
109 |         }
110 | 
111 |         std::cout << "Path Not Found! \n";
112 |         return std::nullopt;
113 |     }
114 | 
115 |     std::optional<std::vector<location_2d>> get_plan_with_visualization(const location_2d& start, const location_2d& goal)
116 |     {
117 |         if(graph_(start.row, start.col) != 0)
118 |         {
119 |             std::cout << "Cannot Find Path. Start Position is Occupied. \n";
120 |             return std::nullopt;
121 |         }
122 |         const auto x = graph_(goal.row, goal.col);
123 |         if(graph_(goal.row, goal.col) != 0)
124 |         {
125 |             std::cout << "Cannot Find Path. End Position is Occupied. \n";
126 |             return std::nullopt;
127 |         }
128 | 
129 |         std::vector<location_2d> path;
130 |         std::unordered_set<location_2d> closed_set;
131 | 
132 |         std::unordered_map<location_2d, double> f_costs;
133 |         std::unordered_map<location_2d, double> g_costs;
134 | 
135 |         auto less = [&](const location_2d& left, const location_2d& right) {
136 |             return f_costs[left] > f_costs[right];
137 |         };
138 | 
139 |         std::priority_queue<location_2d, std::vector<location_2d>, decltype(less)> open_queue(less);
140 |         std::unordered_set<location_2d> open_set;
141 | 
142 |         std::unordered_map<location_2d, location_2d> parent_from_node;
143 | 
144 |         open_queue.push(start);
145 |         open_set.insert(start);
146 | 
147 |         while(!open_queue.empty())
148 |         {
149 |             const auto current_location = open_queue.top();
150 |             open_queue.pop();
151 | 
152 |             if(current_location == goal)
153 |             {
154 |                 auto path_node = goal;
155 |                 while (path_node != start)
156 |                 {
157 |                     explored_locations_.emplace_back(path_node, "yellow");
158 |                     path.emplace_back(path_node);
159 |                     if(parent_from_node.find(path_node) == parent_from_node.end())
160 |                     {
161 |                         std::__throw_logic_error("No Parent for Discovered Node! Check Logic");
162 |                     }
163 |                     path_node = parent_from_node[path_node];
164 |                 }
165 |                 path.emplace_back(start);
166 |                 explored_locations_.emplace_back(start, "red");
167 |                 explored_locations_.emplace_back(goal, "green");
168 |                 return path;
169 |             }
170 | 
171 |             for_all_adjacent_nodes(current_location, graph_, [&](location_2d&& adjacent_loc){
172 | 
173 |                 if(graph_(adjacent_loc.row, adjacent_loc.col) == 0 &&closed_set.find(adjacent_loc) == closed_set.end())
174 |                 {
175 | 
176 |                     const auto node_transition_cost = mpl::get_distance(&current_location, &adjacent_loc);
177 | 
178 |                     // If child not in open list
179 |                     if(open_set.find(adjacent_loc) == open_set.end())
180 |                     {
181 |                         g_costs[adjacent_loc] = g_costs[current_location] + node_transition_cost;
182 | 
183 |                         const auto heuristic_cost = mpl::get_distance(&adjacent_loc, &goal);
184 | 
185 |                         f_costs[adjacent_loc] = g_costs[adjacent_loc] + heuristic_cost;
186 | 
187 |                         parent_from_node[adjacent_loc] = current_location;
188 | 
189 |                         open_set.insert(adjacent_loc);
190 |                         open_queue.push(adjacent_loc);
191 | 
192 |                         explored_locations_.emplace_back(adjacent_loc, "blue");
193 |                     }
194 |                         // Else if child in open list
195 |                     else
196 |                     {
197 |                         // cost of current child less than the cost of same child in open list
198 |                         if(const auto g_cost_adjacent = g_costs[current_location] + node_transition_cost;
199 |                                 g_costs[adjacent_loc] > g_cost_adjacent)
200 |                         {
201 |                             g_costs[adjacent_loc] = g_cost_adjacent;
202 | 
203 |                             // replace the parent as current and cost with current cost
204 |                             parent_from_node[adjacent_loc] = current_location;
205 |                         }
206 |                     }
207 |                 }
208 |             });
209 |         }
210 | 
211 |         std::cout << "Path Not Found! \n";
212 |         return std::nullopt;
213 |     }
214 | };
215 | 
216 | }
217 | 


--------------------------------------------------------------------------------
/include/mpl/jps.h:
--------------------------------------------------------------------------------
  1 | //
  2 | // Created by yash on 9/14/19.
  3 | //
  4 | 
  5 | #pragma once
  6 | 
  7 | #include "planner.h"
  8 | 
  9 | #include <iostream>
 10 | #include <queue>
 11 | #include <unordered_set>
 12 | 
 13 | namespace mpl
 14 | {
 15 | 
 16 | /// JPS Planner
 17 | class jps : public planner<jps>
 18 | {
 19 | public:
 20 |     std::optional<std::vector<location_2d>> get_plan(const location_2d &start, const location_2d &goal) const
 21 |     {
 22 |         if(graph_(start.row, start.col) == 1 )
 23 |         {
 24 |             std::cout << "Cannot Find Path. Start Position is Occupied. \n";
 25 |             return std::nullopt;
 26 |         }
 27 |         if(graph_(goal.row, goal.col) == 1)
 28 |         {
 29 |             std::cout << "Cannot Find Path. End Position is Occupied. \n";
 30 |             return std::nullopt;
 31 |         }
 32 | 
 33 |         std::vector<location_2d> path;
 34 |         std::unordered_set<location_2d> closed_set;
 35 | 
 36 |         std::unordered_map<location_2d, double> f_costs;
 37 |         std::unordered_map<location_2d, double> g_costs;
 38 | 
 39 |         auto less = [&](const location_2d& left, const location_2d& right) {
 40 |             return f_costs[left] > f_costs[right];
 41 |         };
 42 | 
 43 |         std::priority_queue<location_2d, std::vector<location_2d>, decltype(less)> open_queue(less);
 44 |         std::unordered_set<location_2d> open_set;
 45 | 
 46 |         // Mapping from Parent Node to Current Node
 47 |         std::unordered_map<location_2d, location_2d> parent_from_node;
 48 | 
 49 |         open_queue.push(start);
 50 |         open_set.insert(start);
 51 | 
 52 |         while(!open_queue.empty())
 53 |         {
 54 |             const auto current_location = open_queue.top();
 55 |             open_queue.pop();
 56 | 
 57 |             if(current_location == goal)
 58 |             {
 59 |                 auto path_node = goal;
 60 |                 path.emplace_back(path_node);
 61 |                 while (path_node != start)
 62 |                 {
 63 |                     assert(parent_from_node.find(path_node) != parent_from_node.end() && "No Parent for Discovered Node! Check Logic");
 64 |                     linear_backtrace(path_node, parent_from_node[path_node], &path);
 65 |                     path_node = parent_from_node[path_node];
 66 |                 }
 67 |                 return path;
 68 |             }
 69 | 
 70 | 
 71 |             // do a-star using jps functions
 72 |             for_all_adjacent_nodes(current_location, graph_, [&](location_2d&& adjacent_loc){
 73 | 
 74 |                 const location_2d& step = location_2d(adjacent_loc.row - current_location.row,
 75 |                                                       adjacent_loc.col - current_location.col);
 76 | 
 77 |                 if(closed_set.find(adjacent_loc) == closed_set.end())
 78 |                 {
 79 |                     const auto successors = get_successors(current_location,
 80 |                                                            step,
 81 |                                                            start,
 82 |                                                            goal);
 83 | 
 84 |                     for (const auto &successor: successors)
 85 |                     {
 86 |                         if(closed_set.find(successor) == closed_set.end())
 87 |                         {
 88 |                             if (open_set.find(successor) == open_set.end())
 89 |                             {
 90 |                                 g_costs[successor] = g_costs[current_location] + get_distance(&successor, &current_location);
 91 |                                 f_costs[successor] = g_costs[successor] + get_distance(&successor, &goal);
 92 | 
 93 |                                 parent_from_node[successor] = current_location;
 94 |                                 open_set.insert(successor);
 95 |                                 open_queue.push(successor);
 96 |                             }
 97 |                             else if (const auto g_cost_adjacent = g_costs[successor] + get_distance(&successor, &current_location);
 98 |                                     g_costs[successor] > g_cost_adjacent)
 99 |                             {
100 |                                 g_costs[successor] = g_cost_adjacent;
101 | 
102 |                                 parent_from_node[successor] = current_location;
103 |                             }
104 |                         }
105 |                     }
106 |                     closed_set.insert(current_location);
107 |                 }
108 |             });
109 | 
110 |         }
111 | 
112 |         std::cout << "Path does not exist! \n";
113 |         return std::nullopt;
114 | 
115 |     }
116 | 
117 |     std::optional<std::vector<location_2d>> get_plan_with_visualization(const location_2d &start, const location_2d &goal)
118 |     {
119 |         if(graph_(start.row, start.col) == 1 )
120 |         {
121 |             std::cout << "Cannot Find Path. Start Position is Occupied. \n";
122 |             return std::nullopt;
123 |         }
124 |         if(graph_(goal.row, goal.col) == 1)
125 |         {
126 |             std::cout << "Cannot Find Path. End Position is Occupied. \n";
127 |             return std::nullopt;
128 |         }
129 | 
130 |         std::vector<location_2d> path;
131 |         std::unordered_set<location_2d> closed_set;
132 | 
133 |         std::unordered_map<location_2d, double> f_costs;
134 |         std::unordered_map<location_2d, double> g_costs;
135 | 
136 |         auto less = [&](const location_2d& left, const location_2d& right) {
137 |             return f_costs[left] > f_costs[right];
138 |         };
139 | 
140 |         std::priority_queue<location_2d, std::vector<location_2d>, decltype(less)> open_queue(less);
141 |         std::unordered_set<location_2d> open_set;
142 | 
143 |         // Mapping from Parent Node to Current Node
144 |         std::unordered_map<location_2d, location_2d> parent_from_node;
145 | 
146 |         open_queue.push(start);
147 |         open_set.insert(start);
148 | 
149 |         while(!open_queue.empty())
150 |         {
151 |             const auto current_location = open_queue.top();
152 |             open_queue.pop();
153 | 
154 |             if(current_location == goal)
155 |             {
156 |                 auto path_node = goal;
157 |                 path.emplace_back(path_node);
158 |                 while (path_node != start)
159 |                 {
160 |                     assert(parent_from_node.find(path_node) != parent_from_node.end() && "No Parent for Discovered Node! Check Logic");
161 |                     linear_backtrace(path_node, parent_from_node[path_node], &path);
162 |                     path_node = parent_from_node[path_node];
163 |                 }
164 | 
165 |                 for(const auto & node: path)
166 |                 {
167 |                     explored_locations_.emplace_back(node, "yellow");
168 |                 }
169 |                 explored_locations_.emplace_back(start, "red");
170 |                 explored_locations_.emplace_back(goal, "green");
171 |                 return path;
172 |             }
173 | 
174 | 
175 |             // do a-star using jps functions
176 |             for_all_adjacent_nodes(current_location, graph_, [&](location_2d&& adjacent_loc){
177 | 
178 |                 const location_2d& step = location_2d(adjacent_loc.row - current_location.row,
179 |                                                       adjacent_loc.col - current_location.col);
180 | 
181 |                 if(closed_set.find(adjacent_loc) == closed_set.end())
182 |                 {
183 |                     const auto successors = get_successors(current_location,
184 |                                                            step,
185 |                                                            start,
186 |                                                            goal);
187 | 
188 |                     for (const auto &successor: successors)
189 |                     {
190 |                         if(closed_set.find(successor) == closed_set.end())
191 |                         {
192 |                             if (open_set.find(successor) == open_set.end())
193 |                             {
194 |                                 g_costs[successor] = g_costs[current_location] + get_distance(&successor, &current_location);
195 |                                 f_costs[successor] = g_costs[successor] + get_distance(&successor, &goal);
196 | 
197 |                                 parent_from_node[successor] = current_location;
198 |                                 open_set.insert(successor);
199 |                                 open_queue.push(successor);
200 |                                 explored_locations_.emplace_back(successor, "orange");
201 |                             }
202 |                             else if (const auto g_cost_adjacent = g_costs[successor] + get_distance(&successor, &current_location);
203 |                                     g_costs[successor] > g_cost_adjacent)
204 |                             {
205 |                                 g_costs[successor] = g_cost_adjacent;
206 | 
207 |                                 parent_from_node[successor] = current_location;
208 |                             }
209 |                         }
210 |                     }
211 |                     closed_set.insert(adjacent_loc);
212 |                 }
213 |             });
214 | 
215 |         }
216 | 
217 |         std::cout << "Path does not exist! \n";
218 |         return std::nullopt;
219 | 
220 |     }
221 | 
222 | private:
223 | 
224 |     /// Get the JPS successors of a current node in a particular direction (Refer to JPS Paper for more info)
225 |     /// @param current_location
226 |     /// @param step
227 |     /// @param start
228 |     /// @param goal
229 |     /// @return
230 |     std::vector<mpl::location_2d> get_successors(const location_2d& current_location,
231 |                                                    const location_2d& step,
232 |                                                    const location_2d &start,
233 |                                                    const location_2d &goal) const
234 |     {
235 |         assert(is_within_boundary(current_location, n_rows, n_cols));
236 | 
237 |         std::vector<location_2d> successors;
238 | 
239 |         const auto pruned_neighbors = get_pruned_neighbors(current_location, step);
240 | 
241 |         for(const auto& neighbor: pruned_neighbors)
242 |         {
243 |             if(graph_(neighbor.first.row, neighbor.first.col) == 1) continue;
244 |             if(const auto successor = jump(current_location, neighbor.second, start, goal))
245 |             {
246 |                 assert(is_within_boundary(*successor, n_rows, n_cols));
247 |                 successors.emplace_back(*successor);
248 |             }
249 |         }
250 |         return successors;
251 |     }
252 | 
253 |     std::optional<location_2d> jump(const location_2d& cur_loc,
254 |                                         const location_2d& step,
255 |                                         const location_2d &start,
256 |                                         const location_2d &goal) const
257 |     {
258 |         assert(cur_loc.is_within_boundary(n_rows, n_cols));
259 |         // take a step in the input direction
260 |         auto next_location = cur_loc + step;
261 | 
262 |         if(!is_within_boundary(next_location, n_rows, n_cols) || graph_(next_location.row, next_location.col) == 1)
263 |         {
264 |             return std::nullopt;
265 |         }
266 |         else if(next_location == goal)
267 |         {
268 |             return next_location;
269 |         }
270 |         else if(has_forced_neighbors(next_location, step))
271 |         {
272 |             return next_location;
273 |         }
274 |         else if(step.row !=0 && step.col !=0)
275 |         {
276 |             if(jump(next_location, location_2d(step.row, 0), start, goal) ||
277 |                     jump(next_location, location_2d(0, step.col), start, goal)) return next_location;
278 |         }
279 |         return jump(next_location, step, start, goal);
280 |     }
281 | 
282 |     /// Checks if the current location has forced neighbors (Refer to JPS Paper for more info)
283 |     /// @param cur_loc - current node location in the graph
284 |     /// @param step - current step direction
285 |     /// @return true if the current location has a forced neighbor else false
286 |     bool has_forced_neighbors(const mpl::location_2d& cur_loc, const mpl::location_2d& step) const
287 |     {
288 |         assert(cur_loc.is_within_boundary(n_rows, n_cols));
289 | 
290 |         auto is_within_boundary_after_offset = [&](int step_row, int step_col){
291 |             return is_within_boundary(location_2d(cur_loc.row + step_row, cur_loc.col + step_col), n_rows, n_cols);
292 |         };
293 | 
294 |         if(step.row == 0 && step.col != 0) // Condition for Horizontal Direction
295 |         {
296 |             if(is_within_boundary_after_offset(-1, 0))
297 |             {
298 |                 if(graph_(cur_loc.row - 1, cur_loc.col) == 1 &&
299 |                         is_within_boundary_after_offset(-1, step.col) &&
300 |                         graph_(cur_loc.row - 1, cur_loc.col+step.col) == 0) return true;
301 |             }
302 |             if(is_within_boundary_after_offset(1, 0))
303 |             {
304 |                 if(graph_(cur_loc.row + 1, cur_loc.col) == 1 &&
305 |                         is_within_boundary_after_offset(1, step.col) &&
306 |                         graph_(cur_loc.row + 1, cur_loc.col+step.col) == 0) return true;
307 |             }
308 |             return false;
309 |         }
310 |         else if(step.row != 0 && step.col == 0) // Condition for Vertical Direction
311 |         {
312 |             if(is_within_boundary_after_offset(0, -1))
313 |             {
314 |                 if(graph_(cur_loc.row, cur_loc.col - 1) == 1 &&
315 |                         is_within_boundary_after_offset(step.row, 1) &&
316 |                         graph_(cur_loc.row + step.row, cur_loc.col-1) == 0) return true;
317 |             }
318 |             if(is_within_boundary_after_offset(0, 1))
319 |             {
320 |                 if(graph_(cur_loc.row, cur_loc.col + 1) == 1 &&
321 |                         is_within_boundary_after_offset(step.row, 1) &&
322 |                         graph_(cur_loc.row + step.row, cur_loc.col+1) == 0) return true;
323 |             }
324 |             return false;
325 |         }
326 |         else if(step.row == 1 && step.col == 1) // Condition for Diagonal 1
327 |         {
328 |             if(graph_(cur_loc.row, cur_loc.col - 1) == 1) return true;
329 |             if(graph_(cur_loc.row - 1, cur_loc.col) == 1) return true;
330 |             return false;
331 |         }
332 |         else if(step.row == -1 && step.col == -1) // Condition for Diagonal 2
333 |         {
334 |             if(graph_(cur_loc.row + 1, cur_loc.col) == 1) return true;
335 |             if(graph_(cur_loc.row, cur_loc.col + 1) == 1) return true;
336 |             return false;
337 |         }
338 |         else if(step.row == 1 && step.col == -1) // Condition for Diagonal 3
339 |         {
340 |             if(graph_(cur_loc.row - 1, cur_loc.col) == 1) return true;
341 |             if(graph_(cur_loc.row, cur_loc.col + 1) == 1) return true;
342 |             return false;
343 |         }
344 |         else if(step.row == -1 && step.col == 1) // Condition for Diagonal 4
345 |         {
346 |             if(graph_(cur_loc.row, cur_loc.col - 1) == 1) return true;
347 |             if(graph_(cur_loc.row + 1, cur_loc.col) == 1) return true;
348 |             return false;
349 |         }
350 |     }
351 | 
352 |     /// Get pruned neighbors of a node with a particular direction (Refer to JPS Paper for more info)
353 |     /// @param cur_loc
354 |     /// @param step
355 |     /// @return Vector of Pair of Neighbor Locations and their direction with respect to the current location
356 |     std::vector<std::pair<mpl::location_2d, mpl::location_2d>> get_pruned_neighbors(
357 |             const mpl::location_2d& cur_loc, const mpl::location_2d& step) const
358 |     {
359 |         assert(cur_loc.is_within_boundary(n_rows, n_cols));
360 | 
361 |         // Pair of Neighbor Location and Direction
362 |         std::vector<std::pair<mpl::location_2d, mpl::location_2d>> pruned_neighbors;
363 | 
364 |         auto add_neighbor_direction_pair_to_pruned_neighbors_if_within_boundary = [&](int step_row, int step_col){
365 |             const auto neighbor = location_2d(cur_loc.row + step_row, cur_loc.col + step_col);
366 |             if(is_within_boundary(neighbor, n_rows, n_cols) && graph_(neighbor.row, neighbor.col) == 0)
367 |             {
368 |                 pruned_neighbors.emplace_back(std::pair{neighbor, location_2d(step_row, step_col)});
369 |             }
370 |         };
371 | 
372 |         auto is_within_boundary_after_offset = [&](int step_row, int step_col){
373 |             return is_within_boundary(location_2d(cur_loc.row + step_row, cur_loc.col + step_col), n_rows, n_cols);
374 |         };
375 | 
376 |         if(step.row == 0 && step.col == 1) // Condition for Horizontal Direction 1
377 |         {
378 |             add_neighbor_direction_pair_to_pruned_neighbors_if_within_boundary(0, 1);
379 |             if(is_within_boundary_after_offset(-1, 0) && graph_(cur_loc.row - 1, cur_loc.col) == 1)
380 |             {
381 |                 add_neighbor_direction_pair_to_pruned_neighbors_if_within_boundary(-1, 1);
382 |             }
383 |             if(is_within_boundary_after_offset(1, 0) && graph_(cur_loc.row + 1, cur_loc.col) == 1)
384 |             {
385 |                 add_neighbor_direction_pair_to_pruned_neighbors_if_within_boundary(1, 1);
386 |             }
387 |         }
388 |         else if(step.row == 0 && step.col == -1) // Condition for Horizontal Direction 2
389 |         {
390 |             add_neighbor_direction_pair_to_pruned_neighbors_if_within_boundary(0, -1);
391 |             if(is_within_boundary_after_offset(-1, 0) && graph_(cur_loc.row - 1, cur_loc.col) == 1)
392 |             {
393 |                 add_neighbor_direction_pair_to_pruned_neighbors_if_within_boundary(-1, -1);
394 |             }
395 |             if(is_within_boundary_after_offset(1, 0) && graph_(cur_loc.row + 1, cur_loc.col) == 1)
396 |             {
397 |                 add_neighbor_direction_pair_to_pruned_neighbors_if_within_boundary(1, -1);
398 |             }
399 |         }
400 |         else if(step.row == 1 && step.col == 0) // Condition for Vertical Direction 1
401 |         {
402 |             add_neighbor_direction_pair_to_pruned_neighbors_if_within_boundary(1, 0);
403 |             if(is_within_boundary_after_offset(0, -1) && graph_(cur_loc.row, cur_loc.col - 1) == 1)
404 |             {
405 |                 add_neighbor_direction_pair_to_pruned_neighbors_if_within_boundary(1, -1);
406 |             }
407 |             if(is_within_boundary_after_offset(0, 1) && graph_(cur_loc.row, cur_loc.col + 1) == 1)
408 |             {
409 |                 add_neighbor_direction_pair_to_pruned_neighbors_if_within_boundary(1, 1);
410 |             }
411 |         }
412 |         else if(step.row == -1 && step.col == 0) // Condition for Vertical Direction 2
413 |         {
414 |             add_neighbor_direction_pair_to_pruned_neighbors_if_within_boundary(-1, 0);
415 |             if(is_within_boundary_after_offset(0, -1) && graph_(cur_loc.row , cur_loc.col - 1) == 1)
416 |             {
417 |                 add_neighbor_direction_pair_to_pruned_neighbors_if_within_boundary(-1, -1);
418 |             }
419 |             if(is_within_boundary_after_offset(0, 1) && graph_(cur_loc.row , cur_loc.col + 1) == 1)
420 |             {
421 |                 add_neighbor_direction_pair_to_pruned_neighbors_if_within_boundary(-1, 1);
422 |             }
423 |         }
424 |         else if(step.row == 1 && step.col == 1) // Condition for Diagonal 1
425 |         {
426 |             add_neighbor_direction_pair_to_pruned_neighbors_if_within_boundary(1, 0);
427 |             add_neighbor_direction_pair_to_pruned_neighbors_if_within_boundary(0, 1);
428 |             add_neighbor_direction_pair_to_pruned_neighbors_if_within_boundary(1, 1);
429 | 
430 |             if(is_within_boundary_after_offset(0, -1) && graph_(cur_loc.row, cur_loc.col - 1) == 1)
431 |             {
432 |                 add_neighbor_direction_pair_to_pruned_neighbors_if_within_boundary(1, -1);
433 |             }
434 |             if(is_within_boundary_after_offset(-1, 0) && graph_(cur_loc.row - 1, cur_loc.col) == 1)
435 |             {
436 |                 add_neighbor_direction_pair_to_pruned_neighbors_if_within_boundary(-1, 1);
437 |             }
438 |         }
439 |         else if(step.row == -1 && step.col == -1) // Condition for Diagonal 2
440 |         {
441 |             add_neighbor_direction_pair_to_pruned_neighbors_if_within_boundary(-1, 0);
442 |             add_neighbor_direction_pair_to_pruned_neighbors_if_within_boundary(0, -1);
443 |             add_neighbor_direction_pair_to_pruned_neighbors_if_within_boundary(-1, -1);
444 | 
445 |             if(is_within_boundary_after_offset(1, 0) && graph_(cur_loc.row + 1, cur_loc.col) == 1)
446 |             {
447 |                 add_neighbor_direction_pair_to_pruned_neighbors_if_within_boundary(1, -1);
448 |             }
449 |             if(is_within_boundary_after_offset(0, 1) && graph_(cur_loc.row, cur_loc.col + 1) == 1)
450 |             {
451 |                 add_neighbor_direction_pair_to_pruned_neighbors_if_within_boundary(-1, 1);
452 |             }
453 |         }
454 |         else if(step.row == 1 && step.col == -1) // Condition for Diagonal 3
455 |         {
456 |             add_neighbor_direction_pair_to_pruned_neighbors_if_within_boundary(1, 0);
457 |             add_neighbor_direction_pair_to_pruned_neighbors_if_within_boundary(0, -1);
458 |             add_neighbor_direction_pair_to_pruned_neighbors_if_within_boundary(1, -1);
459 | 
460 |             if(is_within_boundary_after_offset(-1, 0) && graph_(cur_loc.row - 1, cur_loc.col) == 1)
461 |             {
462 |                 add_neighbor_direction_pair_to_pruned_neighbors_if_within_boundary(-1, -1);
463 |             }
464 |             if(is_within_boundary_after_offset(0, 1) && graph_(cur_loc.row, cur_loc.col + 1) == 1)
465 |             {
466 |                 add_neighbor_direction_pair_to_pruned_neighbors_if_within_boundary(1, 1);
467 |             }
468 |         }
469 |         else if(step.row == -1 && step.col == 1) // Condition for Diagonal 4
470 |         {
471 |             add_neighbor_direction_pair_to_pruned_neighbors_if_within_boundary(-1, 0);
472 |             add_neighbor_direction_pair_to_pruned_neighbors_if_within_boundary(0, 1);
473 |             add_neighbor_direction_pair_to_pruned_neighbors_if_within_boundary(-1, 1);
474 | 
475 |             if(is_within_boundary_after_offset(0, -1) && graph_(cur_loc.row, cur_loc.col - 1) == 1)
476 |             {
477 |                 add_neighbor_direction_pair_to_pruned_neighbors_if_within_boundary(-1, -1);
478 |             }
479 |             if(is_within_boundary_after_offset(1, 0) && graph_(cur_loc.row + 1, cur_loc.col) == 1)
480 |             {
481 |                 add_neighbor_direction_pair_to_pruned_neighbors_if_within_boundary(1, 1);
482 |             }
483 |         }
484 |         return pruned_neighbors;
485 |     }
486 | 
487 |     /// Backtrack from the start to the end and add the intermediate points to the path vector
488 |     /// @param backtrace_start
489 |     /// @param backtrace_end
490 |     /// @param path
491 |     void linear_backtrace(const location_2d& backtrace_start,
492 |             const location_2d& backtrace_end, std::vector<location_2d>* path) const
493 |     {
494 |         assert(is_linear_backtrace_possible(backtrace_start, backtrace_end));
495 |         const location_2d step = [&](){
496 |             const auto diff = backtrace_start - backtrace_end;
497 |             auto get_single_dimension_direction = [&](int x){
498 |                 if(x < 0) return -1;
499 |                 else if(x > 0) return 1;
500 |                 else return 0;
501 |             };
502 |             return location_2d(get_single_dimension_direction(diff.row),
503 |                                get_single_dimension_direction(diff.col));
504 |         }();
505 |         location_2d current_node = backtrace_start;
506 |         while(current_node != backtrace_end)
507 |         {
508 |             current_node = current_node - step;
509 |             assert(is_within_boundary(current_node, n_rows, n_cols) &&
510 |             "Adding the new step puts the location out of range");
511 |             path->emplace_back(current_node);
512 |         }
513 |     }
514 | 
515 |     /// Debug function to check if linear backtracking is possible between two locations on the grid map
516 |     /// @param backtrace_start
517 |     /// @param backtrace_end
518 |     /// @return
519 |     bool is_linear_backtrace_possible(const location_2d& backtrace_start,
520 |                                       const location_2d& backtrace_end) const
521 |     {
522 |         const auto diff = backtrace_start - backtrace_end;
523 |         return !(diff.row != 0 && diff.col != 0 && diff.row != diff.col && diff.row != -diff.col);
524 |     }
525 | };
526 | 
527 | } // namespace mpl


--------------------------------------------------------------------------------