diff --git a/CMakeLists.txt b/CMakeLists.txt index f96b1de..77b2ce0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,23 +2,34 @@ cmake_minimum_required(VERSION 2.8.9) project(R2D2_pathfinding) find_package (Threads) -file(GLOB SOURCES "source/include/*.hpp" "source/src/*.cpp" "example/*.cpp" "../adt/source/include/*.hpp" "../adt/source/src/*.cpp" "../sharedobjects/source/include/*.hpp" "../sharedobjects/source/src/*.cpp" ) -file(GLOB SOURCES_GTEST "source/src/*.cpp" "source/include/*.hpp" - "test/*.cpp" "test/*.hpp" - "../adt/source/src/Coordinate.cpp" "../adt/source/include/Coordinate.hpp" - "../adt/source/src/Length.cpp" "../adt/source/include/Length.hpp" - "../adt/source/src/Translation.cpp" "../adt/source/include/Translation.hpp" - "../adt/source/src/Box.cpp" "../adt/source/include/Box.hpp" - "../sharedobjects/source/include/SharedObject.hpp" - "../sharedobjects/source/include/LockingSharedObject.hpp" - "../sharedobjects/source/include/NotCopyable.hpp" - ) -list(REMOVE_ITEM SOURCES_GTEST ${CMAKE_CURRENT_SOURCE_DIR}../adt/source/src/main.cpp) +file(GLOB SOURCES + source/include/*.hpp + source/src/*.cpp + example/*.cpp + ../map/source/src/MapInterface.cpp + ../adt/source/include/*.hpp + ../adt/source/src/*.cpp + ../sharedobjects/source/include/*.hpp + ../sharedobjects/source/src/*.cpp) + +file(GLOB SOURCES_GTEST + ../adt/source/src/Length.cpp + ../adt/source/src/Coordinate.cpp + ../adt/source/src/Translation.cpp + ../adt/source/src/Box.cpp + ../map/source/src/MapInterface.cpp + source/src/Dummy.cpp + source/src/AStarPathFinder.cpp + test/PathFinder_Test.cpp + ../sharedobjects/source/include/SharedObject.hpp + ../sharedobjects/source/include/LockingSharedObject.hpp + ../sharedobjects/source/include/NotCopyable.hpp) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=gnu++11 -Wall") include_directories( + ../map/source/include ../adt/source/include ../sharedobjects/source/include ../deps/gtest-1.7.0/include @@ -31,4 +42,4 @@ ADD_LIBRARY(gtest_main ../deps/gtest-1.7.0/src/gtest_main.cc) add_executable(R2D2_pathfinding_example ${SOURCES}) add_executable(R2D2_pathfinding_gtest ${GTEST} ${SOURCES_GTEST}) -target_link_libraries(R2D2_pathfinding_gtest gtest gtest_main ${CMAKE_THREAD_LIBS_INIT}) \ No newline at end of file +target_link_libraries(R2D2_pathfinding_gtest gtest gtest_main ${CMAKE_THREAD_LIBS_INIT}) diff --git a/example/main.cpp b/example/main.cpp index 4e93191..26d0249 100644 --- a/example/main.cpp +++ b/example/main.cpp @@ -1,59 +1,50 @@ - #include "../source/include/PathFinder.hpp" #include "../source/include/AStarPathFinder.hpp" -#include "../source/include/Dummy.hpp" -#include "../source/include/Astar.hpp" -#include "Box.hpp" -#include "Coordinate.hpp" -#include "Length.hpp" #include "Angle.hpp" -#include "Translation.hpp" -#include "SharedObject.hpp" #include "LockingSharedObject.hpp" -#include "iostream" - +#include -int main(int ac, char*av []){ -//Creating a map -std::vector> cornerSqueezeMap; -for (int x = 0; x < 50; x++) { - std::vector current; - for (int y = 50; y > 0; y--) { - if (x == y) { - if (x >= 4 && x <= 6){ - current.push_back(0); - } - else{ - current.push_back(1); - } - } else { - current.push_back(0); - } - } - cornerSqueezeMap.push_back(current); -} -//initializing a map -r2d2::Map map(cornerSqueezeMap); -map.print_map(); +int main(int ac, char *av[]) { + //Creating a map + std::vector> cornerSqueezeMap; + for (int x = 0; x < 50; x++) { + std::vector current; + for (int y = 50; y > 0; y--) { + if (x == y) { + if (x >= 4 && x <= 6) { + current.push_back(0); + } + else { + current.push_back(1); + } + } else { + current.push_back(0); + } + } + cornerSqueezeMap.push_back(current); + } + //initializing a map + r2d2::Dummy map(cornerSqueezeMap); + map.print_map(); -// initializing pathfinding -r2d2::Translation robotBox{ .5 * r2d2::Length::METER, - .5 * r2d2::Length::METER, - 0 * r2d2::Length::METER}; -LockingSharedObject sharedMap{map}; -r2d2::AStarPathFinder path_finder(sharedMap, {{}, robotBox}); + // initializing pathfinding + r2d2::Translation robotBox{.5 * r2d2::Length::METER, + .5 * r2d2::Length::METER, + 0 * r2d2::Length::METER}; + LockingSharedObject sharedMap{map}; + r2d2::AStarPathFinder path_finder(sharedMap, {{}, robotBox}); -//Computing a path between coordinates -r2d2::Coordinate c1 {1 * r2d2::Length::METER, 2 * r2d2::Length::METER, -0 * r2d2::Length::METER}; -r2d2::Coordinate c2 {4 * r2d2::Length::METER, - 3 * r2d2::Length::METER, - 0 * r2d2::Length::METER}; -std::vector path_vector{r2d2::Coordinate( - 1 * r2d2::Length::METER, - 3 * r2d2::Length::METER, - 0 * r2d2::Length::METER)}; -cout << path_finder.get_path_to_coordinate(c1, c2, path_vector); + //Computing a path between coordinates + r2d2::Coordinate c1{1 * r2d2::Length::METER, 2 * r2d2::Length::METER, + 0 * r2d2::Length::METER}; + r2d2::Coordinate c2{4 * r2d2::Length::METER, + 3 * r2d2::Length::METER, + 0 * r2d2::Length::METER}; + std::vector path_vector{r2d2::Coordinate( + 1 * r2d2::Length::METER, + 3 * r2d2::Length::METER, + 0 * r2d2::Length::METER)}; + std::cout << path_finder.get_path_to_coordinate(c1, c2, path_vector); } diff --git a/source/include/AStarPathFinder.hpp b/source/include/AStarPathFinder.hpp index 7b4c650..02e78a1 100644 --- a/source/include/AStarPathFinder.hpp +++ b/source/include/AStarPathFinder.hpp @@ -58,7 +58,7 @@ // defines the amount of nodes that will be visited per length of the robot // for instance, if the robot has a size of 1m, and this value is 2, a node will // be opened every .5m -#define SQUARES_PER_ROBOT 2 +#define SQUARES_PER_ROBOT 1 namespace r2d2 { @@ -69,7 +69,7 @@ namespace r2d2 { */ class AStarPathFinder : public PathFinder { public: - AStarPathFinder(SharedObject &map, Box robotBox); + AStarPathFinder(SharedObject &map, Box robotBox); virtual bool get_path_to_coordinate( Coordinate start, @@ -94,20 +94,21 @@ namespace r2d2 { virtual std::vector get_available_nodes( std::shared_ptr &self) override; - AStarPathFinder &pathFinder; - Coordinate coord, &startNodeCoord; + std::reference_wrapper pathFinder; + Coordinate coord; + std::reference_wrapper startNodeCoord; friend std::ostream &operator<<(std::ostream &lhs, const CoordNode &rhs) { - return lhs << "(" << rhs.coord << ", " << rhs.g - << ", " << rhs.h << ", " << rhs.f << ")"; + return lhs << "(" << rhs.coord << ", " << rhs.g + << ", " << rhs.h << ", " << rhs.f << ")"; } }; friend struct std::hash; - SharedObject ↦ - std::unique_ptr::Accessor> mapAccessor; + SharedObject ↦ + std::weak_ptr::Accessor> mapAccessor; std::atomic referenceCount; Translation robotBox; diff --git a/source/include/Astar.hpp b/source/include/Astar.hpp index 1e0791f..d0398a5 100644 --- a/source/include/Astar.hpp +++ b/source/include/Astar.hpp @@ -77,9 +77,9 @@ namespace r2d2 { parent{parent} { } - const Length g; + Length g; Length h, f; - const std::weak_ptr parent; + std::weak_ptr parent; /** * checks if two nodes can be considered equal for the algorithm @@ -142,7 +142,7 @@ namespace r2d2 { while (!open.empty() && --giveUpCount >= 0) { std::shared_ptr curOpen{open[0]}; // use the heap methods from std, - // as this is a very performat use case + // as this is a fitting use case std::pop_heap(open.begin(), open.end(), [](std::shared_ptr &n1, std::shared_ptr &n2) { @@ -156,11 +156,15 @@ namespace r2d2 { std::shared_ptr child{std::make_shared(c)}; // add the child to the closed set - auto result = closed.emplace(child); - if (result.second) { + auto result = closed.insert(child); + if (result.second || ((**result.first) > (*child))) { + if (!result.second) { + **result.first = *child; + // change the coordnode to be the better node + } // if the node did not yet exist in the set // push the heap with the new open node - open.emplace_back(*result.first); + open.push_back(*result.first); std::push_heap( open.begin(), open.end(), [](std::shared_ptr &n1, diff --git a/source/include/Dummy.hpp b/source/include/Dummy.hpp index 2522d51..99dfb94 100644 --- a/source/include/Dummy.hpp +++ b/source/include/Dummy.hpp @@ -53,15 +53,16 @@ #include #include #include -#include "../../../adt/source/include/Coordinate.hpp" -#include "../../../adt/source/include/Translation.hpp" +#include +#include +#include namespace r2d2 { //! Dummy Map /*! * Map for testing the pathfinder */ - class Map { + class Dummy : public ReadOnlyMap { public: //! Implementation of the map, where: 0 = clear, 1 = obstacle, 2 = unexplored std::vector> map; @@ -75,13 +76,13 @@ namespace r2d2 { * \param y The height of the map * \param obstacles Percentage of obstacles in the map */ - Map(int x = 100, int y = 100, float obstacles = 0.25f); + Dummy(int x = 100, int y = 100, float obstacles = 0.25f); //! Constructor /*! * \param map The map */ - Map(std::vector > map); + Dummy(std::vector > map); //! Print the map /*! @@ -89,28 +90,13 @@ namespace r2d2 { */ void print_map(); - //! Returns if position on the map has a obstacle within the robot size - /*! - * \param x The x position on the map - * \param y The y position on the map - * \param sizeX The width of the robot - * \param sizeY The height of the robot - * \return If there is a obstacle found - */ - bool has_obstacle(Coordinate coord, Translation size); + virtual const BoxInfo get_box_info(const Box box) override; - //! Returns if position on the map has a passable erea within the robot size - /*! - * \param x The x position on the map - * \param y The y position on the map - * \param sizeX The width of the robot - * \param sizeY The height of the robot - * \return If there is a passable erea - */ - bool has_passable(Coordinate coord, Translation size); + virtual const Box get_map_bounding_box() override; private: static std::mt19937_64 mersenne; + }; } diff --git a/source/include/PathFinder.hpp b/source/include/PathFinder.hpp index 89c5713..b7fab42 100644 --- a/source/include/PathFinder.hpp +++ b/source/include/PathFinder.hpp @@ -48,11 +48,10 @@ #ifndef R2D2_PATHFINDING_PATHFINDER_HPP #define R2D2_PATHFINDING_PATHFINDER_HPP + #include -#include #include "../../../adt/source/include/Box.hpp" #include "../../../sharedobjects/source/include/SharedObject.hpp" -// TODO replace this with the actual interface #include "Dummy.hpp" namespace r2d2 { @@ -70,7 +69,7 @@ namespace r2d2 { * \param map Reference to the world map * \param robotSize Reference to the robot size */ - PathFinder(SharedObject &map, Box robotBox) {}; + PathFinder(SharedObject &map, Box robotBox) {}; /** * Returns a path between two points diff --git a/source/src/AStarPathFinder.cpp b/source/src/AStarPathFinder.cpp index 165671c..b86f48f 100644 --- a/source/src/AStarPathFinder.cpp +++ b/source/src/AStarPathFinder.cpp @@ -52,7 +52,7 @@ namespace r2d2 { - AStarPathFinder::AStarPathFinder(SharedObject &map, Box robotBox) : + AStarPathFinder::AStarPathFinder(SharedObject &map, Box robotBox) : PathFinder{map, robotBox}, map(map), mapAccessor{}, @@ -70,10 +70,11 @@ namespace r2d2 { return true; } - if (mapAccessor == nullptr) { - mapAccessor = std::unique_ptr::Accessor>{new SharedObject::Accessor{map}}; + std::shared_ptr::Accessor> ptr{mapAccessor.lock()}; + if (ptr == nullptr) { + ptr = std::make_shared::Accessor>(map); + mapAccessor = ptr; // doesn't have to be atomic as it doesn't matter what pointer is stored } - referenceCount++; // do a check for end node accessibility before starting the search if (!can_travel(goal, goal)) { @@ -97,9 +98,6 @@ namespace r2d2 { smooth_path(path, start); } - if (--referenceCount == 0) { - mapAccessor.release(); - } return foundStart != nullptr; } @@ -124,17 +122,17 @@ namespace r2d2 { // the grid will be relative to the end position of the search Coordinate childPos{ coord + (Translation{ - x * pathFinder.robotBox.get_x(), - y * pathFinder.robotBox.get_y(), + x * pathFinder.get().robotBox.get_x(), + y * pathFinder.get().robotBox.get_y(), 0 * Length::METER } / SQUARES_PER_ROBOT)}; //check whether the successor is the end node - if (pathFinder.overlaps(childPos, startNodeCoord)) { - childPos = {startNodeCoord}; + if (pathFinder.get().overlaps(childPos, startNodeCoord)) { + childPos = {startNodeCoord.get()}; } // can_travel is used so that it can be ensured that there is no // obstacle in the path - if (pathFinder.can_travel(coord, childPos)) { + if (pathFinder.get().can_travel(coord, childPos)) { children.push_back( CoordNode{pathFinder, childPos, startNodeCoord, g + AStarPathFinder::get_heuristic( @@ -163,7 +161,8 @@ namespace r2d2 { (from.get_x() > to.get_x() ? from.get_x() : to.get_x()), (from.get_y() > to.get_y() ? from.get_y() : to.get_y()), 0 * Length::METER} - minCoord) + robotBox}; - return !mapAccessor->access().has_obstacle(minCoord - (robotBox / 2), size); + BoxInfo info{mapAccessor.lock()->access().get_box_info(Box{minCoord - (robotBox / 2), size})}; + return !(info.get_has_obstacle() || info.get_has_unknown()); } bool AStarPathFinder::overlaps(const Coordinate &c1, diff --git a/source/src/Dummy.cpp b/source/src/Dummy.cpp index 3efba09..36e0a1e 100644 --- a/source/src/Dummy.cpp +++ b/source/src/Dummy.cpp @@ -50,17 +50,18 @@ namespace r2d2 { - std::mt19937_64 Map::mersenne = std::mt19937_64{ + std::mt19937_64 Dummy::mersenne = std::mt19937_64{ (unsigned long) (time(0))}; - Map::Map(int x, int y, float obstacles) : - map{} { - map.reserve((unsigned long) (x)); - sizeX = x, sizeY = y; - for (int i1 = 0; i1 < x; i1++) { + Dummy::Dummy(int x, int y, float obstacles) : + map{}, + sizeX{x}, + sizeY{y} { + map.reserve((unsigned long) (y)); + for (int i1 = 0; i1 < y; i1++) { map.emplace_back(); - map[i1].reserve((unsigned long) (y)); - for (int i2 = 0; i2 < y; i2++) { + map[i1].reserve((unsigned long) (x)); + for (int i2 = 0; i2 < x; i2++) { map[i1].emplace_back(); map[i1][i2] = std::uniform_real_distribution{}(mersenne) < obstacles ? 1 : 0; @@ -69,51 +70,51 @@ namespace r2d2 { } - Map::Map(std::vector> map) : + Dummy::Dummy(std::vector> map) : map{map}, - sizeX{int(map.size())}, - sizeY{int(map[0].size())} { + sizeX{int(map[0].size())}, + sizeY{int(map.size())} { } - void Map::print_map() { - for (int i1 = 0; i1 < sizeX; i1++) { - for (int i2 = 0; i2 < sizeY; i2++) { + void Dummy::print_map() { + for (int i1 = 0; i1 < sizeY; i1++) { + for (int i2 = 0; i2 < sizeX; i2++) { std::cout << map[i1][i2]; } std::cout << std::endl; } } - bool Map::has_obstacle(Coordinate coord, Translation size) { - for (int i1 = int(coord.get_x() / Length::METER); - i1 <= int((coord.get_x() + size.get_x()) / Length::METER); i1++) { - for (int i2 = int(coord.get_y() / Length::METER); - i2 <= - int((coord.get_y() + size.get_y()) / Length::METER); i2++) { - if (i1 < 0 || i1 >= sizeX || - i2 < 0 || i2 >= sizeY || - map[i1][i2] == 1 || map[i1][i2] == 2) { - return true; + const BoxInfo Dummy::get_box_info(const Box box) { + bool obstacle = false, navigable = false, unknown = false; + for (int i1 = int(box.get_bottom_left().get_y() / Length::METER); + i1 <= int(box.get_top_right().get_y() / Length::METER); i1++) { + for (int i2 = int(box.get_bottom_left().get_x() / Length::METER); + i2 <= int(box.get_top_right().get_x() / Length::METER); i2++) { + if (i1 < 0 || i1 >= sizeY || + i2 < 0 || i2 >= sizeX) { + unknown = true; + } else { + switch (map[i1][i2]) { + case 0: + navigable = true; + break; + case 1: + obstacle = true; + break; + case 2: + unknown = true; + break; + default:; + } } } } - return false; + return {obstacle, navigable, unknown}; } - bool Map::has_passable(Coordinate coord, Translation size) { - for (int i1 = int(coord.get_x() / Length::METER); - i1 <= int((coord.get_x() + size.get_x()) / Length::METER); i1++) { - for (int i2 = int(coord.get_y() / Length::METER); - i2 <= - int((coord.get_y() + size.get_y()) / Length::METER); i2++) { - if (i1 < 0 && i1 >= sizeX && - i2 < 0 && i2 >= sizeY && - map[i1][i2] == 0) { - return true; - } - } - } - return false; + const Box Dummy::get_map_bounding_box() { + return {}; } } \ No newline at end of file diff --git a/test/PathFinder_Test.cpp b/test/PathFinder_Test.cpp index c75521d..a04fae7 100644 --- a/test/PathFinder_Test.cpp +++ b/test/PathFinder_Test.cpp @@ -93,28 +93,28 @@ std::vector> make_map(int pathSize, int x, int y) { #define MAX_TRIES 10000 // can be scaled down if it takes too much processing -std::tuple test_until_true(int mapX, int mapY, +std::tuple test_until_true(int mapX, int mapY, r2d2::Translation robotBox, r2d2::Coordinate start, r2d2::Coordinate goal, std::vector &path) { for (int i = 0; i < MAX_TRIES; i++) { - r2d2::Map map(mapX, mapY); - LockingSharedObject sharedMap{map}; + r2d2::Dummy map(mapX, mapY); + LockingSharedObject sharedMap{map}; r2d2::AStarPathFinder pf{sharedMap, {{}, robotBox}}; if (pf.get_path_to_coordinate(start, goal, path)) { - return std::tuple{true, map}; + return std::tuple{true, map}; } } - return std::tuple{false, r2d2::Map{}}; + return std::tuple{false, r2d2::Dummy{}}; } TEST(PathFinder, constructor) { - r2d2::Map map(50, 50, 0); + r2d2::Dummy map(50, 50, 0); r2d2::Translation robotBox{.5 * r2d2::Length::METER, .5 * r2d2::Length::METER, 0 * r2d2::Length::METER}; - LockingSharedObject sharedMap{map}; + LockingSharedObject sharedMap{map}; r2d2::AStarPathFinder pf(sharedMap, {{}, robotBox}); r2d2::Coordinate start{.5 * r2d2::Length::METER, .5 * r2d2::Length::METER, 0 * r2d2::Length::METER}; @@ -128,11 +128,11 @@ TEST(PathFinder, constructor) { } TEST(PathFinder, not_existing_begin) { - r2d2::Map map(50, 50, 0); + r2d2::Dummy map(50, 50, 0); r2d2::Translation robotBox{.5 * r2d2::Length::METER, .5 * r2d2::Length::METER, 0 * r2d2::Length::METER}; - LockingSharedObject sharedMap{map}; + LockingSharedObject sharedMap{map}; r2d2::AStarPathFinder pf(sharedMap, {{}, robotBox}); r2d2::Coordinate start{-1 * r2d2::Length::METER, -1 * r2d2::Length::METER, 0 * r2d2::Length::METER}; @@ -145,11 +145,11 @@ TEST(PathFinder, not_existing_begin) { } TEST(PathFinder, not_existing_end) { - r2d2::Map map(50, 50, 0); + r2d2::Dummy map(50, 50, 0); r2d2::Translation robotBox{.5 * r2d2::Length::METER, .5 * r2d2::Length::METER, 0 * r2d2::Length::METER}; - LockingSharedObject sharedMap{map}; + LockingSharedObject sharedMap{map}; r2d2::AStarPathFinder pf(sharedMap, {{}, robotBox}); r2d2::Coordinate start{.5 * r2d2::Length::METER, @@ -165,11 +165,11 @@ TEST(PathFinder, not_existing_end) { } TEST(PathFinder, without_obstacles) { - r2d2::Map map(50, 50, 0); + r2d2::Dummy map(50, 50, 0); r2d2::Translation robotBox{.5 * r2d2::Length::METER, .5 * r2d2::Length::METER, 0 * r2d2::Length::METER}; - LockingSharedObject sharedMap{map}; + LockingSharedObject sharedMap{map}; r2d2::AStarPathFinder pf(sharedMap, {{}, robotBox}); r2d2::Coordinate start{.5 * r2d2::Length::METER, .5 * r2d2::Length::METER, 0 * r2d2::Length::METER}; @@ -205,13 +205,13 @@ TEST(PathFinder, consistent) { r2d2::Coordinate goal{49.5 * r2d2::Length::METER, 49.5 * r2d2::Length::METER, 0 * r2d2::Length::METER}; std::vector path; - std::tuple result{ + std::tuple result{ test_until_true(50, 50, robotBox, start, goal, path) }; ASSERT_TRUE(std::get<0>(result)) << start << " " << goal << " first time"; ASSERT_FALSE(path.empty()) << "path empty"; std::vector currentpath = path; - LockingSharedObject sharedMap{std::get<1>(result)}; + LockingSharedObject sharedMap{std::get<1>(result)}; r2d2::AStarPathFinder p2{sharedMap, {{}, robotBox}}; ASSERT_TRUE(p2.get_path_to_coordinate(start, goal, path)) << start << " " << goal << " second time"; @@ -219,11 +219,11 @@ TEST(PathFinder, consistent) { } TEST(PathFinder, robot_size) { - r2d2::Map map{100, 100, 0}; + r2d2::Dummy map{100, 100, 0}; // size 0 r2d2::Translation robotBox{0 * r2d2::Length::METER, 0 * r2d2::Length::METER, 0 * r2d2::Length::METER}; - LockingSharedObject sharedMap{map}; + LockingSharedObject sharedMap{map}; r2d2::AStarPathFinder pf(sharedMap, {{}, robotBox}); r2d2::Coordinate start{10.5 * r2d2::Length::METER, @@ -241,7 +241,7 @@ TEST(PathFinder, robot_size) { r2d2::Translation robotBox2{5 * r2d2::Length::METER, 5 * r2d2::Length::METER, 0 * r2d2::Length::METER}; - LockingSharedObject sharedMap2{map}; + LockingSharedObject sharedMap2{map}; r2d2::AStarPathFinder pf1(sharedMap2, {{}, robotBox2}); ASSERT_TRUE(pf1.get_path_to_coordinate(start, goal, path)) << start << " " << goal << " robot with size 5"; @@ -251,11 +251,11 @@ TEST(PathFinder, robot_size) { TEST(PathFinder, obstacle_on_begin) { std::vector> vector = make_map(1, 50, 50); vector[0][0] = 1; - r2d2::Map map(vector); + r2d2::Dummy map(vector); r2d2::Translation robotBox{.5 * r2d2::Length::METER, .5 * r2d2::Length::METER, 0 * r2d2::Length::METER}; - LockingSharedObject sharedMap{map}; + LockingSharedObject sharedMap{map}; r2d2::AStarPathFinder pf(sharedMap, {{}, robotBox}); r2d2::Coordinate start{.5 * r2d2::Length::METER, @@ -273,11 +273,11 @@ TEST(PathFinder, obstacle_on_begin) { TEST(PathFinder, obstacle_on_end) { std::vector> vector = make_map(1, 50, 50); vector[49][49] = 1; - r2d2::Map map(vector); + r2d2::Dummy map(vector); r2d2::Translation robotBox{.5 * r2d2::Length::METER, .5 * r2d2::Length::METER, 0 * r2d2::Length::METER}; - LockingSharedObject sharedMap{map}; + LockingSharedObject sharedMap{map}; r2d2::AStarPathFinder pf(sharedMap, {{}, robotBox}); r2d2::Coordinate start{.5 * r2d2::Length::METER, @@ -293,11 +293,11 @@ TEST(PathFinder, obstacle_on_end) { } TEST(PathFinder, float) { - r2d2::Map map(make_map(2, 50, 50)); + r2d2::Dummy map(make_map(2, 50, 50)); r2d2::Translation robotBox{.5 * r2d2::Length::METER, .5 * r2d2::Length::METER, 0 * r2d2::Length::METER}; - LockingSharedObject sharedMap{map}; + LockingSharedObject sharedMap{map}; r2d2::AStarPathFinder pf(sharedMap, {{}, robotBox}); r2d2::Coordinate start{.56 * r2d2::Length::METER, .52 * r2d2::Length::METER, @@ -312,11 +312,11 @@ TEST(PathFinder, float) { } TEST(PathFinder, same_begin_as_end) { - r2d2::Map map(make_map(1, 50, 50)); + r2d2::Dummy map(make_map(1, 50, 50)); r2d2::Translation robotBox{.5 * r2d2::Length::METER, .5 * r2d2::Length::METER, 0 * r2d2::Length::METER}; - LockingSharedObject sharedMap{map}; + LockingSharedObject sharedMap{map}; r2d2::AStarPathFinder pf(sharedMap, {{}, robotBox}); r2d2::Coordinate start{1 * r2d2::Length::METER, @@ -341,14 +341,14 @@ TEST(PathFinder, corner_squeezing) { } else { current.push_back(0); } - cornerSqueezeMap.push_back(current); } + cornerSqueezeMap.push_back(current); } - r2d2::Map map(cornerSqueezeMap); + r2d2::Dummy map(cornerSqueezeMap); r2d2::Translation robotBox{.5 * r2d2::Length::METER, .5 * r2d2::Length::METER, 0 * r2d2::Length::METER}; - LockingSharedObject sharedMap{map}; + LockingSharedObject sharedMap{map}; r2d2::AStarPathFinder pf(sharedMap, {{}, robotBox}); r2d2::Coordinate start{.5 * r2d2::Length::METER, @@ -395,13 +395,13 @@ namespace std { // image of the map and a calculated path overlaid onto it // if you want to know how or why this code works // you should search the pnm format -void printMapWithPath(std::ostream &out, r2d2::Map &map, +void printMapWithPath(std::ostream &out, r2d2::Dummy &map, std::unordered_set path) { out << "P3" << std::endl; out << map.sizeX * SCALE << " " << map.sizeY * SCALE << " 2" << std::endl; for (int i1 = 0; i1 < map.sizeY * SCALE; i1++) { for (int i2 = 0; i2 < map.sizeX * SCALE; i2++) { - if (path.find(IntCoord(i1, i2)) != path.end()) { + if (path.find(IntCoord(i2, i1)) != path.end()) { out << "2 0 0 "; } else { int groundVal = 1 - map.map[i1 / SCALE][i2 / SCALE]; @@ -415,12 +415,12 @@ void printMapWithPath(std::ostream &out, r2d2::Map &map, TEST(PathFinder, image_test) { // debugging code for visualisation of paths - int mapX = 100, mapY = 100, mapCount = 0; + int mapX = 50, mapY = 50, mapCount = 0; bool done = false; while (!done) { + r2d2::Dummy map = {mapX, mapY, 0.4f}; - r2d2::Map map = {mapX, mapY, 0.4f}; - LockingSharedObject sharedMap{map}; + LockingSharedObject sharedMap{map}; r2d2::AStarPathFinder pathFinder = {sharedMap, {{}, r2d2::Translation{0.5 * r2d2::Length::METER,