From 5c299bf116dd881eb9b1b46289fe610d82cf05ac Mon Sep 17 00:00:00 2001 From: chieltbest Date: Mon, 25 Apr 2016 12:54:18 +0200 Subject: [PATCH 01/11] edited the files to be in accordance with the fagan inspection --- source/include/Astar.hpp | 10 ++++++++++ source/include/PathFinder.hpp | 17 ++++++++++++++++- source/src/PathFinder.cpp | 5 ++++- source/src/main.cpp | 14 +++++++++++--- 4 files changed, 41 insertions(+), 5 deletions(-) diff --git a/source/include/Astar.hpp b/source/include/Astar.hpp index f35ed0b..1e0791f 100644 --- a/source/include/Astar.hpp +++ b/source/include/Astar.hpp @@ -136,10 +136,13 @@ namespace r2d2 { * otherwise return nullptr */ std::shared_ptr search(T &start) { + // the amount of nodes left to search before the search is abandoned int giveUpCount = MAX_SEARCH_NODES; 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 std::pop_heap(open.begin(), open.end(), [](std::shared_ptr &n1, std::shared_ptr &n2) { @@ -147,11 +150,16 @@ namespace r2d2 { }); open.pop_back(); + // query the current node for the nodes + // that are accessible from that node for (T &c : curOpen->get_available_nodes(curOpen)) { std::shared_ptr child{std::make_shared(c)}; + // add the child to the closed set auto result = closed.emplace(child); if (result.second) { + // if the node did not yet exist in the set + // push the heap with the new open node open.emplace_back(*result.first); std::push_heap( open.begin(), open.end(), @@ -161,6 +169,8 @@ namespace r2d2 { }); } if (*child == start) { + // the opened child was the node the search + // was supposed to reach; terminate the search return *closed.find(child); } } diff --git a/source/include/PathFinder.hpp b/source/include/PathFinder.hpp index 0c1a789..24ad4fe 100644 --- a/source/include/PathFinder.hpp +++ b/source/include/PathFinder.hpp @@ -96,6 +96,8 @@ namespace r2d2 { Coordinate goal, std::vector &path); + private: + /** * implementation of the astar node from Astar.hpp */ @@ -122,7 +124,8 @@ namespace r2d2 { } }; - private: + friend struct std::hash; + Map ↦ Translation robotBox; @@ -161,8 +164,20 @@ namespace r2d2 { */ static Length get_heuristic(Translation coord); + /** + * extracts a path from a pathfinder search field + * + * \param start the node to start the search from + * \return the computed path from the supplied node to the goal node + */ std::vector get_path(std::shared_ptr start); + /** + * strips a path of all unneccecary nodes, smoothing the pathe in the process + * + * \param path the path to smooth + * \param start the original start coordinate + */ void smooth_path(std::vector &path, Coordinate start); }; diff --git a/source/src/PathFinder.cpp b/source/src/PathFinder.cpp index e6595c7..d8d3a32 100644 --- a/source/src/PathFinder.cpp +++ b/source/src/PathFinder.cpp @@ -194,7 +194,7 @@ namespace r2d2 { std::shared_ptr start) { std::shared_ptr curNode = start; std::vector path; - while (!curNode->parent.expired()) { + while (!curNode->parent.expired()) { // while next != nullptr curNode = std::shared_ptr(curNode->parent); path.emplace_back(*curNode); } @@ -207,9 +207,12 @@ namespace r2d2 { auto it = path.begin(); auto current = it++; while (it != path.end()) { + // check if the is a path from the current anchor node to the next if (can_travel(lastPos, *it)) { + // if so, remove the node(s) in between path.erase(current); } else { + // if not, make the current node the new anchor node lastPos = *it; current = it++; } diff --git a/source/src/main.cpp b/source/src/main.cpp index 0e876af..89e22a8 100644 --- a/source/src/main.cpp +++ b/source/src/main.cpp @@ -13,6 +13,8 @@ //! \brief main used for testing //! //! Main used for testing +//! This file will not be used for actual production code, +//! and is only for debugging purposes //! //! \copyright Copyright © 2016, HU University of Applied Sciences Utrecht. //! All rights reserved. @@ -75,19 +77,25 @@ namespace std { } }; } -// prints out a .pgm (portable grey r2d2::Map) image of the r2d2::Map and it's path + +// the pgm printer prints out a multiple of pixels for each tile in the tile +// map, this definition controls how much pixels are printed per tile #define SCALE 4 +// prints out a .pgm (portable grey map) +// 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, 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++) { - int groundVal = 1 - map.map[i1 / SCALE][i2 / SCALE]; if (path.find(IntCoord(i1, i2)) != path.end()) { - out << "2 " << 0 << " " << 0 << " "; + out << "2 0 0 "; } else { + int groundVal = 1 - map.map[i1 / SCALE][i2 / SCALE]; out << groundVal * 2 << " " << groundVal * 2 << " " << groundVal * 2 << " "; } From 1253e8ac3be24c35edd7c9a5c5765db80b3c4701 Mon Sep 17 00:00:00 2001 From: Jordan Ramirez Date: Mon, 25 Apr 2016 13:25:24 +0200 Subject: [PATCH 02/11] Changed cmakelist to only include used adt's instead of all adt's --- CMakeLists.txt | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index a0ada3c..e02db1a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,10 +5,14 @@ find_package (Threads) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=gnu++11 -Wall -Werror") file(GLOB SOURCES "source/src/*.cpp" "source/include/*.hpp" - "../adt/source/src/*.cpp" "../adt/source/include/*.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") file(GLOB SOURCES_GTEST "source/src/*.cpp" "source/include/*.hpp" "test/*.cpp" "test/*.hpp" - "../adt/source/src/*.cpp" "../adt/source/include/*.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") list(REMOVE_ITEM SOURCES_GTEST ${CMAKE_CURRENT_SOURCE_DIR}/source/src/main.cpp) list(REMOVE_ITEM SOURCES_GTEST ${CMAKE_CURRENT_SOURCE_DIR}../adt/source/src/main.cpp) From cb1569128d1af9d846098e56587731a8b2e08eae Mon Sep 17 00:00:00 2001 From: chieltbest Date: Tue, 10 May 2016 12:02:58 +0200 Subject: [PATCH 03/11] removed the main.cpp file, and seperated the pathfinder interface from the a* implementation --- CMakeLists.txt | 6 - source/include/AStarPathFinder.hpp | 188 ++++++++++++++++++ source/include/PathFinder.hpp | 184 ++++------------- .../{PathFinder.cpp => AStarPathFinder.cpp} | 39 ++-- source/src/main.cpp | 148 -------------- test/PathFinder_Test.cpp | 116 +++++++++-- 6 files changed, 344 insertions(+), 337 deletions(-) create mode 100644 source/include/AStarPathFinder.hpp rename source/src/{PathFinder.cpp => AStarPathFinder.cpp} (87%) delete mode 100644 source/src/main.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index e02db1a..ee820fc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,16 +4,11 @@ find_package (Threads) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=gnu++11 -Wall -Werror") -file(GLOB SOURCES "source/src/*.cpp" "source/include/*.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") 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") -list(REMOVE_ITEM SOURCES_GTEST ${CMAKE_CURRENT_SOURCE_DIR}/source/src/main.cpp) list(REMOVE_ITEM SOURCES_GTEST ${CMAKE_CURRENT_SOURCE_DIR}../adt/source/src/main.cpp) include_directories( @@ -24,6 +19,5 @@ link_directories(../deps/gtest-1.7.0/src) ADD_LIBRARY(gtest ../deps/gtest-1.7.0/src/gtest-all.cc) ADD_LIBRARY(gtest_main ../deps/gtest-1.7.0/src/gtest_main.cc) -add_executable(R2D2_pathfinding ${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 diff --git a/source/include/AStarPathFinder.hpp b/source/include/AStarPathFinder.hpp new file mode 100644 index 0000000..396ffda --- /dev/null +++ b/source/include/AStarPathFinder.hpp @@ -0,0 +1,188 @@ +//! \addtogroup 0007 Pathfinding +//! \brief A pathfinding module +//! +//! A pathfinding module that can be used in the R2D2 project. +//! The module is currently based on the A star algorithm. +//! +//! \file AStarPathFinder.hpp +//! \author Chiel Douwes 1666311 +//! \date Created: 30-03-2016 +//! \date Last Modified: 15-4-2016 +//! \brief Implementation of the pathfinder interface +//! +//! Takes a starting point and end point and a reference to a path. +//! If a path is possible it returns a path to the end point. If no path is +//! possible it will return false. +//! This implementation uses A* for the search, which is defined in the +//! "Astar" class. +//! +//! \copyright Copyright © 2016, HU University of Applied Sciences Utrecht. +//! All rights reserved. +//! +//! License: newBSD +//! +//! Redistribution and use in source and binary forms, +//! with or without modification, are permitted provided that +//! the following conditions are met: +//! - Redistributions of source code must retain the above copyright notice, +//! this list of conditions and the following disclaimer. +//! - Redistributions in binary form must reproduce the above copyright notice, +//! this list of conditions and the following disclaimer in the documentation +//! and/or other materials provided with the distribution. +//! - Neither the name of the HU University of Applied Sciences Utrecht +//! nor the names of its contributors may be used to endorse or promote +//! products derived from this software without specific prior written +//! permission. +//! +//! THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +//! "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, +//! BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY +//! AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. +//! IN NO EVENT SHALL THE HU UNIVERSITY OF APPLIED SCIENCES UTRECHT +//! BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +//! CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, +//! PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; +//! OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +//! WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR +//! OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, +//! EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// ~< HEADER_VERSION 2016 04 12 >~ + +#ifndef R2D2_PATHFINDING_ASTARPATHFINDER_HPP +#define R2D2_PATHFINDING_ASTARPATHFINDER_HPP + +#include "PathFinder.hpp" +#include "Astar.hpp" + +// 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 + +namespace r2d2 { + + /** + * interface for a pathfinder module + * + * computes a path between two points on a map + */ + class AStarPathFinder : public PathFinder { + public: + AStarPathFinder(Map &map, Translation robotBox); + + virtual bool get_path_to_coordinate( + Coordinate start, + Coordinate goal, + std::vector &path) override; + + private: + + /** + * implementation of the astar node from Astar.hpp + */ + class CoordNode : public Node { + public: + CoordNode(AStarPathFinder &pathFinder, Coordinate coord, + Coordinate &startCoord, + Length g = r2d2::Length::METER * + std::numeric_limits::infinity(), + std::weak_ptr parent = {}); + + virtual bool operator==(const CoordNode &lhs) const override; + + virtual std::vector get_available_nodes( + std::shared_ptr &self) override; + + AStarPathFinder &pathFinder; + Coordinate coord, &startNodeCoord; + + friend std::ostream &operator<<(std::ostream &lhs, + const CoordNode &rhs) { + return lhs << "(" << rhs.coord << ", " << rhs.g + << ", " << rhs.h << ", " << rhs.f << ")"; + } + }; + + friend struct std::hash; + + Map ↦ + Translation robotBox; + + /** + * test whether it is possible to travel from "from" directly to "to" + * + * if this function returns true, then it is guaranteed that there is a + * direct path between "from" and "to". the function may return false + * even if the is a direct connection because it has to absolutely + * certain. + * \param from the coordinate that will be travelled from + * \param to the coordinate that will be travelled to from "from" + * \return true if it is guaranteed that the robot can travel from + * "from" to "to" + */ + bool can_travel(const Coordinate &from, const Coordinate &to); + + /** + * check whether a coordinate will be overlapped by the robot when + * positioned on a second coordinate + * + * this could be used to check whether two nodes can possibly be + * considered as one + * \param c1 the position the robot is on + * \param c2 the coordinate that should be checked for overlapping + * with the robot + * \return c1 overlaps c2 within the size of the robot + */ + bool overlaps(const Coordinate &c1, const Coordinate &c2); + + /** + * get the traversed distance if the robot goes from {0, 0} to "coord" + * + * \param coord the coordinate that should be calculated the length for + * \return the distance from origin to "coord" + */ + static Length get_heuristic(Translation coord); + + /** + * extracts a path from a pathfinder search field + * + * \param start the node to start the search from + * \return the computed path from the supplied node to the goal node + */ + std::vector get_path(std::shared_ptr start); + + /** + * strips a path of all unnecessary nodes, smoothing the path in the process + * + * \param path the path to smooth + * \param start the original start coordinate + */ + void smooth_path(std::vector &path, Coordinate start); + }; + +} + +namespace std { + + template<> + struct hash { + std::size_t operator()(const r2d2::Coordinate &coord) const { + return std::hash()(coord.get_x() / r2d2::Length::METER) + ^ (std::hash()(coord.get_y() / r2d2::Length::METER) + << (sizeof(double) / 2)); + } + }; + + /** + * hash for the coordinate node class, used for set insertion + */ + template<> + struct hash { + std::size_t operator()(const r2d2::AStarPathFinder::CoordNode &node) const { + return std::hash()(node.coord); + } + }; + +} + +#endif //R2D2_PATHFINDING_ASTARPATHFINDER_HPP diff --git a/source/include/PathFinder.hpp b/source/include/PathFinder.hpp index 24ad4fe..91bd494 100644 --- a/source/include/PathFinder.hpp +++ b/source/include/PathFinder.hpp @@ -48,161 +48,45 @@ #ifndef R2D2_PATHFINDING_PATHFINDER_HPP #define R2D2_PATHFINDING_PATHFINDER_HPP - - #include -#include -#include "Dummy.hpp" -#include "Astar.hpp" -#include "../../../adt/source/include/Coordinate.hpp" -#include "../../../adt/source/include/Length.hpp" +#include #include "../../../adt/source/include/Translation.hpp" - -// 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 +// TODO replace this with the actual interface +#include "Dummy.hpp" namespace r2d2 { - /** - * interface for a pathfinder module - * - * computes a path between two points on a map - */ - class PathFinder { - public: - /** - * Constructor - * - * \param map Reference to the world map - * \param robotSize Reference to the robot size - */ - PathFinder(Map &map, Translation robotBox); - - /** - * Returns a path between two points - * - * This function computes a path from start to goal, - * based on the map and robot size given in the constructor - * - * \param start The start coordinate - * \param goal The goal coordinate - * \param path Vector where the path need to be written to - * \return If it was able to find a path - */ - virtual bool get_path_to_coordinate( - Coordinate start, - Coordinate goal, - std::vector &path); - - private: - - /** - * implementation of the astar node from Astar.hpp - */ - class CoordNode : public Node { - public: - CoordNode(PathFinder &pathFinder, Coordinate coord, - Coordinate &startCoord, - Length g = r2d2::Length::METER * - std::numeric_limits::infinity(), - std::weak_ptr parent = {}); - - virtual bool operator==(const CoordNode &lhs) const override; - - virtual std::vector get_available_nodes( - std::shared_ptr &self) override; - - PathFinder &pathFinder; - Coordinate coord, &startNodeCoord; - - friend std::ostream &operator<<(std::ostream &lhs, - const CoordNode &rhs) { - return lhs << "(" << rhs.coord << ", " << rhs.g - << ", " << rhs.h << ", " << rhs.f << ")"; - } - }; - - friend struct std::hash; - - Map ↦ - Translation robotBox; - - /** - * test whether it is possible to travel from "from" directly to "to" - * - * if this function returns true, then it is guaranteed that there is a - * direct path between "from" and "to". the function may return false - * even if the is a direct connection because it has to absolutely - * certain. - * \param from the coordinate that will be travelled from - * \param to the coordinate that will be travelled to from "from" - * \return true if it is guaranteed that the robot can travel from - * "from" to "to" - */ - bool can_travel(const Coordinate &from, const Coordinate &to); - - /** - * check whether a coordinate will be overlapped by the robot when - * positioned on a second coordinate - * - * this could be used to check whether two nodes can possibly be - * considered as one - * \param c1 the position the robot is on - * \param c2 the coordinate that should be checked for overlapping - * with the robot - * \return c1 overlaps c2 within the size of the robot - */ - bool overlaps(const Coordinate &c1, const Coordinate &c2); - - /** - * get the traversed distance if the robot goes from {0, 0} to "coord" - * - * \param coord the coordinate that should be calculated the length for - * \return the distance from orgin to "coord" - */ - static Length get_heuristic(Translation coord); - - /** - * extracts a path from a pathfinder search field - * - * \param start the node to start the search from - * \return the computed path from the supplied node to the goal node - */ - std::vector get_path(std::shared_ptr start); - - /** - * strips a path of all unneccecary nodes, smoothing the pathe in the process - * - * \param path the path to smooth - * \param start the original start coordinate - */ - void smooth_path(std::vector &path, Coordinate start); - }; - -} - -namespace std { - - template<> - struct hash { - std::size_t operator()(const r2d2::Coordinate &coord) const { - return std::hash()(coord.get_x() / r2d2::Length::METER) - ^ (std::hash()(coord.get_y() / r2d2::Length::METER) - << (sizeof(double) / 2)); - } - }; - - /** - * hash for the coordinate node class, used for set insertion - */ - template<> - struct hash { - std::size_t operator()(const r2d2::PathFinder::CoordNode &node) const { - return std::hash()(node.coord); - } - }; + /** + * interface for a pathfinder module + * + * computes a path between two points on a map + */ + class PathFinder { + public: + /** + * Constructor + * + * \param map Reference to the world map + * \param robotSize Reference to the robot size + */ + PathFinder(Map &map, Translation robotBox) {}; + + /** + * Returns a path between two points + * + * This function computes a path from start to goal, + * based on the map and robot size given in the constructor + * + * \param start The start coordinate + * \param goal The goal coordinate + * \param path Vector where the path need to be written to + * \return If it was able to find a path + */ + virtual bool get_path_to_coordinate( + Coordinate start, + Coordinate goal, + std::vector &path) = 0; + }; } diff --git a/source/src/PathFinder.cpp b/source/src/AStarPathFinder.cpp similarity index 87% rename from source/src/PathFinder.cpp rename to source/src/AStarPathFinder.cpp index d8d3a32..9e0c9a0 100644 --- a/source/src/PathFinder.cpp +++ b/source/src/AStarPathFinder.cpp @@ -4,7 +4,7 @@ //! A pathfinding module that can be used in the R2D2 project. //! The module is currently based on the A star algorithm. //! -//! \file PathFinder.cpp +//! \file AStarPathFinder.cpp //! \author Jasper Schoenmaker 1661818 //! \author Chiel Douwes 1666311 //! \author Ole Achterberg 1651981 @@ -48,16 +48,17 @@ //! EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. // ~< HEADER_VERSION 2016 04 12 >~ -#include "../include/PathFinder.hpp" +#include "../include/AStarPathFinder.hpp" namespace r2d2 { - PathFinder::PathFinder(Map &map, Translation robotBox) : + AStarPathFinder::AStarPathFinder(Map &map, Translation robotBox) : + PathFinder{map, robotBox}, map(map), robotBox(robotBox) { } - bool PathFinder::get_path_to_coordinate(Coordinate start, + bool AStarPathFinder::get_path_to_coordinate(Coordinate start, Coordinate goal, std::vector &path) { @@ -93,20 +94,20 @@ namespace r2d2 { } } - PathFinder::CoordNode::CoordNode( - PathFinder &pathFinder, Coordinate coord, + AStarPathFinder::CoordNode::CoordNode( + AStarPathFinder &pathFinder, Coordinate coord, Coordinate &startCoord, Length g, std::weak_ptr parent) : - Node{g, PathFinder::get_heuristic(startCoord - coord), + Node{g, AStarPathFinder::get_heuristic(startCoord - coord), parent}, pathFinder(pathFinder), coord(coord), startNodeCoord(startCoord) { } - std::vector - PathFinder::CoordNode::get_available_nodes( - std::shared_ptr &self) { + std::vector + AStarPathFinder::CoordNode::get_available_nodes( + std::shared_ptr &self) { std::vector children; for (int x = -1; x <= 1; x++) { for (int y = -1; y <= 1; y++) { @@ -127,7 +128,7 @@ namespace r2d2 { if (pathFinder.can_travel(coord, childPos)) { children.push_back( CoordNode{pathFinder, childPos, startNodeCoord, - g + PathFinder::get_heuristic( + g + AStarPathFinder::get_heuristic( childPos - coord ), // distance from the search begin self}); @@ -138,12 +139,12 @@ namespace r2d2 { return children; } - bool PathFinder::CoordNode::operator==( - const PathFinder::CoordNode &lhs) const { + bool AStarPathFinder::CoordNode::operator==( + const AStarPathFinder::CoordNode &lhs) const { return (coord - lhs.coord).get_length() / Length::METER == 0; } - bool PathFinder::can_travel(const Coordinate &from, + bool AStarPathFinder::can_travel(const Coordinate &from, const Coordinate &to) { Coordinate minCoord{ (from.get_x() < to.get_x() ? from.get_x() : to.get_x()), @@ -156,7 +157,7 @@ namespace r2d2 { return !map.has_obstacle(minCoord - (robotBox / 2), size); } - bool PathFinder::overlaps(const Coordinate &c1, + bool AStarPathFinder::overlaps(const Coordinate &c1, const Coordinate &c2) { Translation diff = c1 - c2; return (diff.get_x() < 0 * Length::METER ? @@ -173,7 +174,7 @@ namespace r2d2 { // in this case 10 digits is "good enough" #define SQ_ROOT_2 1.414213562f - Length PathFinder::get_heuristic(Translation coord) { + Length AStarPathFinder::get_heuristic(Translation coord) { // diagonal distance Length xDist = (coord.get_x() < 0 * Length::METER) ? (0 * Length::METER - coord.get_x()) : coord.get_x(); @@ -190,8 +191,8 @@ namespace r2d2 { return (shortDist * SQ_ROOT_2) + (longDist - shortDist); } - std::vector PathFinder::get_path( - std::shared_ptr start) { + std::vector AStarPathFinder::get_path( + std::shared_ptr start) { std::shared_ptr curNode = start; std::vector path; while (!curNode->parent.expired()) { // while next != nullptr @@ -201,7 +202,7 @@ namespace r2d2 { return path; } - void PathFinder::smooth_path(std::vector &path, + void AStarPathFinder::smooth_path(std::vector &path, Coordinate start) { Coordinate lastPos = start; auto it = path.begin(); diff --git a/source/src/main.cpp b/source/src/main.cpp deleted file mode 100644 index 89e22a8..0000000 --- a/source/src/main.cpp +++ /dev/null @@ -1,148 +0,0 @@ -//! \addtogroup 0007 Pathfinding -//! \brief A pathfinding module -//! -//! A pathfinding module that can be used in the R2D2 project. -//! The module is currently based on the A star algorithm. -//! -//! \file main.cpp -//! \author Jasper Schoenmaker 1661818 -//! \author Chiel Douwes 1666311 -//! \author Ole Achterberg 1651981 -//! \date Created: 01-04-2016 -//! \date Last Modified: 15-4-2016 -//! \brief main used for testing -//! -//! Main used for testing -//! This file will not be used for actual production code, -//! and is only for debugging purposes -//! -//! \copyright Copyright © 2016, HU University of Applied Sciences Utrecht. -//! All rights reserved. -//! -//! License: newBSD -//! -//! Redistribution and use in source and binary forms, -//! with or without modification, are permitted provided that -//! the following conditions are met: -//! - Redistributions of source code must retain the above copyright notice, -//! this list of conditions and the following disclaimer. -//! - Redistributions in binary form must reproduce the above copyright notice, -//! this list of conditions and the following disclaimer in the documentation -//! and/or other materials provided with the distribution. -//! - Neither the name of the HU University of Applied Sciences Utrecht -//! nor the names of its contributors may be used to endorse or promote -//! products derived from this software without specific prior written -//! permission. -//! -//! THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -//! "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, -//! BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY -//! AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. -//! IN NO EVENT SHALL THE HU UNIVERSITY OF APPLIED SCIENCES UTRECHT -//! BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -//! CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, -//! PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; -//! OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, -//! WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR -//! OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, -//! EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -// ~< HEADER_VERSION 2016 04 12 >~ - -#include -#include -#include -#include -#include -#include "../include/Dummy.hpp" -#include "../include/PathFinder.hpp" - -struct IntCoord { -public: - IntCoord() : x{0}, y{0} { } - - IntCoord(int x, int y) : x{x}, y{y} { } - - int x, y; - - bool operator==(const IntCoord &lhs) const { - return x == lhs.x && y == lhs.y; - } -}; -namespace std { - template<> - struct hash { - std::size_t operator()(const IntCoord &coord) const { - return std::hash()(coord.x) - ^ (std::hash()(coord.y) << (sizeof(int) / 2)); - } - }; -} - -// the pgm printer prints out a multiple of pixels for each tile in the tile -// map, this definition controls how much pixels are printed per tile -#define SCALE 4 - -// prints out a .pgm (portable grey map) -// 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, - 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()) { - out << "2 0 0 "; - } else { - int groundVal = 1 - map.map[i1 / SCALE][i2 / SCALE]; - out << groundVal * 2 << " " << groundVal * 2 << " " << - groundVal * 2 << " "; - } - } - out << std::endl; - } -} - -int main() { - // debugging code for visualisation of paths - int mapX = 300, mapY = 300, mapCount = 0; - bool done = false; - while (!done) { - - r2d2::Map map = {mapX, mapY, 0.4f}; - r2d2::PathFinder pathFinder = {map, - {0.5 * r2d2::Length::METER, - 0.5 * r2d2::Length::METER, - 0 * r2d2::Length::METER}}; - std::vector path; - done = pathFinder.get_path_to_coordinate( - {5.5f * r2d2::Length::METER, - 5.5f * r2d2::Length::METER, - 0.0f * r2d2::Length::METER}, - {(mapX - 5.5f) * r2d2::Length::METER, - (mapY - 5.5f) * r2d2::Length::METER, - 0.0f * r2d2::Length::METER}, - path); - mapCount++; - - if (done) { - std::unordered_set intPath; - for (r2d2::Coordinate &coord : path) { - std::cout << coord << std::endl; - intPath.emplace(coord.get_x() / r2d2::Length::METER * SCALE, - coord.get_y() / r2d2::Length::METER * SCALE); - } - std::cout.flush(); - - std::ofstream ofs{"path.pgm"}; - printMapWithPath(ofs, map, intPath); - ofs.flush(); - - std::cout << "searched " << mapCount << " maps" << std::endl; - } - } - - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - return 0; -} \ No newline at end of file diff --git a/test/PathFinder_Test.cpp b/test/PathFinder_Test.cpp index ea6fac9..d1f4546 100644 --- a/test/PathFinder_Test.cpp +++ b/test/PathFinder_Test.cpp @@ -51,7 +51,7 @@ #include #include #include "../source/include/Dummy.hpp" -#include "../source/include/PathFinder.hpp" +#include "../source/include/AStarPathFinder.hpp" bool equal(const std::vector &lhs, const std::vector &rhs) { @@ -99,7 +99,7 @@ std::tuple test_until_true(int mapX, int mapY, std::vector &path) { for (int i = 0; i < MAX_TRIES; i++) { r2d2::Map map(mapX, mapY); - r2d2::PathFinder pf(map, robotBox); + r2d2::AStarPathFinder pf(map, robotBox); if (pf.get_path_to_coordinate(start, goal, path)) { return std::tuple{true, map}; @@ -112,7 +112,7 @@ TEST(PathFinder, constructor) { r2d2::Map map(50, 50, 0); r2d2::Translation robotBox{.5 * r2d2::Length::METER, .5 * r2d2::Length::METER, 0 * r2d2::Length::METER}; - r2d2::PathFinder pf(map, robotBox); + r2d2::AStarPathFinder pf(map, robotBox); r2d2::Coordinate start{.5 * r2d2::Length::METER, .5 * r2d2::Length::METER, 0 * r2d2::Length::METER}; r2d2::Coordinate goal{49.5 * r2d2::Length::METER, 49.5 * r2d2::Length::METER, @@ -129,7 +129,7 @@ TEST(PathFinder, not_existing_begin) { r2d2::Translation robotBox{.5 * r2d2::Length::METER, .5 * r2d2::Length::METER, 0 * r2d2::Length::METER}; - r2d2::PathFinder pf(map, robotBox); + r2d2::AStarPathFinder pf(map, robotBox); r2d2::Coordinate start{-1 * r2d2::Length::METER, -1 * r2d2::Length::METER, 0 * r2d2::Length::METER}; r2d2::Coordinate goal{49.5 * r2d2::Length::METER, 49.5 * r2d2::Length::METER, @@ -145,7 +145,7 @@ TEST(PathFinder, not_existing_end) { r2d2::Translation robotBox{.5 * r2d2::Length::METER, .5 * r2d2::Length::METER, 0 * r2d2::Length::METER}; - r2d2::PathFinder pf(map, robotBox); + r2d2::AStarPathFinder pf(map, robotBox); r2d2::Coordinate start{.5 * r2d2::Length::METER, .5 * r2d2::Length::METER, @@ -164,7 +164,7 @@ TEST(PathFinder, without_obstacles) { r2d2::Translation robotBox{.5 * r2d2::Length::METER, .5 * r2d2::Length::METER, 0 * r2d2::Length::METER}; - r2d2::PathFinder pf(map, robotBox); + r2d2::AStarPathFinder pf(map, robotBox); r2d2::Coordinate start{.5 * r2d2::Length::METER, .5 * r2d2::Length::METER, 0 * r2d2::Length::METER}; r2d2::Coordinate goal{49.5 * r2d2::Length::METER, 49.5 * r2d2::Length::METER, @@ -205,7 +205,7 @@ TEST(PathFinder, consistent) { ASSERT_TRUE(std::get<0>(result)) << start << " " << goal << " first time"; ASSERT_FALSE(path.empty()) << "path empty"; std::vector currentpath = path; - r2d2::PathFinder p2{std::get<1>(result), robotBox}; + r2d2::AStarPathFinder p2{std::get<1>(result), robotBox}; ASSERT_TRUE(p2.get_path_to_coordinate(start, goal, path)) << start << " " << goal << " second time"; ASSERT_TRUE(equal(currentpath, path)); @@ -216,7 +216,7 @@ TEST(PathFinder, robot_size) { // size 0 r2d2::Translation robotBox{0 * r2d2::Length::METER, 0 * r2d2::Length::METER, 0 * r2d2::Length::METER}; - r2d2::PathFinder pf(map, robotBox); + r2d2::AStarPathFinder pf(map, robotBox); r2d2::Coordinate start{10.5 * r2d2::Length::METER, 10.5 * r2d2::Length::METER, @@ -233,7 +233,7 @@ TEST(PathFinder, robot_size) { r2d2::Translation robotBox2{5 * r2d2::Length::METER, 5 * r2d2::Length::METER, 0 * r2d2::Length::METER}; - r2d2::PathFinder pf1(map, robotBox2); + r2d2::AStarPathFinder pf1(map, robotBox2); ASSERT_TRUE(pf1.get_path_to_coordinate(start, goal, path)) << start << " " << goal << " robot with size 5"; ASSERT_FALSE(path.empty()); @@ -246,7 +246,7 @@ TEST(PathFinder, obstacle_on_begin) { r2d2::Translation robotBox{.5 * r2d2::Length::METER, .5 * r2d2::Length::METER, 0 * r2d2::Length::METER}; - r2d2::PathFinder pf(map, robotBox); + r2d2::AStarPathFinder pf(map, robotBox); r2d2::Coordinate start{.5 * r2d2::Length::METER, .5 * r2d2::Length::METER, @@ -267,7 +267,7 @@ TEST(PathFinder, obstacle_on_end) { r2d2::Translation robotBox{.5 * r2d2::Length::METER, .5 * r2d2::Length::METER, 0 * r2d2::Length::METER}; - r2d2::PathFinder pf(map, robotBox); + r2d2::AStarPathFinder pf(map, robotBox); r2d2::Coordinate start{.5 * r2d2::Length::METER, .5 * r2d2::Length::METER, @@ -286,7 +286,7 @@ TEST(PathFinder, float) { r2d2::Translation robotBox{.5 * r2d2::Length::METER, .5 * r2d2::Length::METER, 0 * r2d2::Length::METER}; - r2d2::PathFinder pf(map, robotBox); + r2d2::AStarPathFinder pf(map, robotBox); r2d2::Coordinate start{.56 * r2d2::Length::METER, .52 * r2d2::Length::METER, 0 * r2d2::Length::METER}; @@ -304,7 +304,7 @@ TEST(PathFinder, same_begin_as_end) { r2d2::Translation robotBox{.5 * r2d2::Length::METER, .5 * r2d2::Length::METER, 0 * r2d2::Length::METER}; - r2d2::PathFinder pf(map, robotBox); + r2d2::AStarPathFinder pf(map, robotBox); r2d2::Coordinate start{1 * r2d2::Length::METER, 1 * r2d2::Length::METER, @@ -335,7 +335,7 @@ TEST(PathFinder, corner_squeezing) { r2d2::Translation robotBox{.5 * r2d2::Length::METER, .5 * r2d2::Length::METER, 0 * r2d2::Length::METER}; - r2d2::PathFinder pf(map, robotBox); + r2d2::AStarPathFinder pf(map, robotBox); r2d2::Coordinate start{.5 * r2d2::Length::METER, .5 * r2d2::Length::METER, @@ -349,6 +349,94 @@ TEST(PathFinder, corner_squeezing) { ASSERT_TRUE(path.empty()); } +// we need a specialized integer coordinate class +// to be able to access the image coordinates +struct IntCoord { +public: + IntCoord() : x{0}, y{0} { } + + IntCoord(int x, int y) : x{x}, y{y} { } + + int x, y; + + bool operator==(const IntCoord &lhs) const { + return x == lhs.x && y == lhs.y; + } +}; +namespace std { + template<> + struct hash { + std::size_t operator()(const IntCoord &coord) const { + return std::hash()(coord.x) + ^ (std::hash()(coord.y) << (sizeof(int) / 2)); + } + }; +} + +// the pgm printer prints out a multiple of pixels for each tile in the tile +// map, this definition controls how much pixels are printed per tile +#define SCALE 4 + +// prints out a .pgm (portable grey map) +// 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, + 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()) { + out << "2 0 0 "; + } else { + int groundVal = 1 - map.map[i1 / SCALE][i2 / SCALE]; + out << groundVal * 2 << " " << groundVal * 2 << " " << + groundVal * 2 << " "; + } + } + out << std::endl; + } +} + +TEST(PathFinder, image_test) { + // debugging code for visualisation of paths + int mapX = 100, mapY = 100, mapCount = 0; + bool done = false; + while (!done) { + + r2d2::Map map = {mapX, mapY, 0.4f}; + r2d2::AStarPathFinder pathFinder = {map, + {0.5 * r2d2::Length::METER, + 0.5 * r2d2::Length::METER, + 0 * r2d2::Length::METER}}; + std::vector path; + done = pathFinder.get_path_to_coordinate( + {5.5f * r2d2::Length::METER, + 5.5f * r2d2::Length::METER, + 0.0f * r2d2::Length::METER}, + {(mapX - 5.5f) * r2d2::Length::METER, + (mapY - 5.5f) * r2d2::Length::METER, + 0.0f * r2d2::Length::METER}, + path); + mapCount++; + + if (done) { + std::unordered_set intPath; + for (r2d2::Coordinate &coord : path) { + intPath.emplace(coord.get_x() / r2d2::Length::METER * SCALE, + coord.get_y() / r2d2::Length::METER * SCALE); + } + + std::ofstream ofs{"path.pgm"}; + printMapWithPath(ofs, map, intPath); + ofs.flush(); + + std::cout << "searched " << mapCount << " maps" << std::endl; + } + } +} + int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); From 60ecf4a636176c59d63704deb5c913ae16ae1f71 Mon Sep 17 00:00:00 2001 From: chieltbest Date: Tue, 10 May 2016 17:46:02 +0200 Subject: [PATCH 04/11] edited the implementation to use a shared object as the map and Box for the robot size --- CMakeLists.txt | 6 +++- source/include/AStarPathFinder.hpp | 9 ++++-- source/include/PathFinder.hpp | 5 +-- source/src/AStarPathFinder.cpp | 29 +++++++++++------ test/PathFinder_Test.cpp | 50 ++++++++++++++++++++---------- 5 files changed, 66 insertions(+), 33 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ee820fc..0c2aace 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -8,7 +8,11 @@ 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/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) include_directories( diff --git a/source/include/AStarPathFinder.hpp b/source/include/AStarPathFinder.hpp index 396ffda..7b4c650 100644 --- a/source/include/AStarPathFinder.hpp +++ b/source/include/AStarPathFinder.hpp @@ -51,6 +51,7 @@ #ifndef R2D2_PATHFINDING_ASTARPATHFINDER_HPP #define R2D2_PATHFINDING_ASTARPATHFINDER_HPP +#include #include "PathFinder.hpp" #include "Astar.hpp" @@ -68,7 +69,7 @@ namespace r2d2 { */ class AStarPathFinder : public PathFinder { public: - AStarPathFinder(Map &map, Translation robotBox); + AStarPathFinder(SharedObject &map, Box robotBox); virtual bool get_path_to_coordinate( Coordinate start, @@ -101,11 +102,13 @@ namespace r2d2 { return lhs << "(" << rhs.coord << ", " << rhs.g << ", " << rhs.h << ", " << rhs.f << ")"; } - }; + }; friend struct std::hash; - Map ↦ + SharedObject ↦ + std::unique_ptr::Accessor> mapAccessor; + std::atomic referenceCount; Translation robotBox; /** diff --git a/source/include/PathFinder.hpp b/source/include/PathFinder.hpp index 91bd494..89c5713 100644 --- a/source/include/PathFinder.hpp +++ b/source/include/PathFinder.hpp @@ -50,7 +50,8 @@ #define R2D2_PATHFINDING_PATHFINDER_HPP #include #include -#include "../../../adt/source/include/Translation.hpp" +#include "../../../adt/source/include/Box.hpp" +#include "../../../sharedobjects/source/include/SharedObject.hpp" // TODO replace this with the actual interface #include "Dummy.hpp" @@ -69,7 +70,7 @@ namespace r2d2 { * \param map Reference to the world map * \param robotSize Reference to the robot size */ - PathFinder(Map &map, Translation 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 9e0c9a0..165671c 100644 --- a/source/src/AStarPathFinder.cpp +++ b/source/src/AStarPathFinder.cpp @@ -52,10 +52,12 @@ namespace r2d2 { - AStarPathFinder::AStarPathFinder(Map &map, Translation robotBox) : + AStarPathFinder::AStarPathFinder(SharedObject &map, Box robotBox) : PathFinder{map, robotBox}, map(map), - robotBox(robotBox) { + mapAccessor{}, + referenceCount{0}, + robotBox{robotBox.get_axis_size()} { } bool AStarPathFinder::get_path_to_coordinate(Coordinate start, @@ -67,6 +69,12 @@ namespace r2d2 { path.clear(); return true; } + + if (mapAccessor == nullptr) { + mapAccessor = std::unique_ptr::Accessor>{new SharedObject::Accessor{map}}; + } + referenceCount++; + // do a check for end node accessibility before starting the search if (!can_travel(goal, goal)) { return false; @@ -75,23 +83,24 @@ namespace r2d2 { // parent is at this point unknown for the start node, // so construct it as unknown CoordNode endNode{*this, goal, start, 0 * Length::METER}, - startNode{*this, - start, - start}; + startNode{*this, start, start}; AStarSearch search{endNode}; std::shared_ptr foundStart = search.search(startNode); - if (foundStart == nullptr) { - return false; - } else { + if (foundStart != nullptr) { std::vector foundPath{get_path(foundStart)}; path.clear(); for (CoordNode &node : foundPath) { path.push_back(node.coord); } + smooth_path(path, start); - return true; } + + if (--referenceCount == 0) { + mapAccessor.release(); + } + return foundStart != nullptr; } AStarPathFinder::CoordNode::CoordNode( @@ -154,7 +163,7 @@ 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 !map.has_obstacle(minCoord - (robotBox / 2), size); + return !mapAccessor->access().has_obstacle(minCoord - (robotBox / 2), size); } bool AStarPathFinder::overlaps(const Coordinate &c1, diff --git a/test/PathFinder_Test.cpp b/test/PathFinder_Test.cpp index d1f4546..c75521d 100644 --- a/test/PathFinder_Test.cpp +++ b/test/PathFinder_Test.cpp @@ -52,6 +52,7 @@ #include #include "../source/include/Dummy.hpp" #include "../source/include/AStarPathFinder.hpp" +#include "../../sharedobjects/source/include/LockingSharedObject.hpp" bool equal(const std::vector &lhs, const std::vector &rhs) { @@ -99,7 +100,8 @@ std::tuple test_until_true(int mapX, int mapY, std::vector &path) { for (int i = 0; i < MAX_TRIES; i++) { r2d2::Map map(mapX, mapY); - r2d2::AStarPathFinder pf(map, robotBox); + LockingSharedObject sharedMap{map}; + r2d2::AStarPathFinder pf{sharedMap, {{}, robotBox}}; if (pf.get_path_to_coordinate(start, goal, path)) { return std::tuple{true, map}; @@ -112,7 +114,8 @@ TEST(PathFinder, constructor) { r2d2::Map map(50, 50, 0); r2d2::Translation robotBox{.5 * r2d2::Length::METER, .5 * r2d2::Length::METER, 0 * r2d2::Length::METER}; - r2d2::AStarPathFinder pf(map, robotBox); + LockingSharedObject sharedMap{map}; + r2d2::AStarPathFinder pf(sharedMap, {{}, robotBox}); r2d2::Coordinate start{.5 * r2d2::Length::METER, .5 * r2d2::Length::METER, 0 * r2d2::Length::METER}; r2d2::Coordinate goal{49.5 * r2d2::Length::METER, 49.5 * r2d2::Length::METER, @@ -129,7 +132,8 @@ TEST(PathFinder, not_existing_begin) { r2d2::Translation robotBox{.5 * r2d2::Length::METER, .5 * r2d2::Length::METER, 0 * r2d2::Length::METER}; - r2d2::AStarPathFinder pf(map, robotBox); + LockingSharedObject sharedMap{map}; + r2d2::AStarPathFinder pf(sharedMap, {{}, robotBox}); r2d2::Coordinate start{-1 * r2d2::Length::METER, -1 * r2d2::Length::METER, 0 * r2d2::Length::METER}; r2d2::Coordinate goal{49.5 * r2d2::Length::METER, 49.5 * r2d2::Length::METER, @@ -145,7 +149,8 @@ TEST(PathFinder, not_existing_end) { r2d2::Translation robotBox{.5 * r2d2::Length::METER, .5 * r2d2::Length::METER, 0 * r2d2::Length::METER}; - r2d2::AStarPathFinder pf(map, robotBox); + LockingSharedObject sharedMap{map}; + r2d2::AStarPathFinder pf(sharedMap, {{}, robotBox}); r2d2::Coordinate start{.5 * r2d2::Length::METER, .5 * r2d2::Length::METER, @@ -164,7 +169,8 @@ TEST(PathFinder, without_obstacles) { r2d2::Translation robotBox{.5 * r2d2::Length::METER, .5 * r2d2::Length::METER, 0 * r2d2::Length::METER}; - r2d2::AStarPathFinder pf(map, robotBox); + LockingSharedObject sharedMap{map}; + r2d2::AStarPathFinder pf(sharedMap, {{}, robotBox}); r2d2::Coordinate start{.5 * r2d2::Length::METER, .5 * r2d2::Length::METER, 0 * r2d2::Length::METER}; r2d2::Coordinate goal{49.5 * r2d2::Length::METER, 49.5 * r2d2::Length::METER, @@ -205,7 +211,8 @@ TEST(PathFinder, consistent) { ASSERT_TRUE(std::get<0>(result)) << start << " " << goal << " first time"; ASSERT_FALSE(path.empty()) << "path empty"; std::vector currentpath = path; - r2d2::AStarPathFinder p2{std::get<1>(result), robotBox}; + 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"; ASSERT_TRUE(equal(currentpath, path)); @@ -216,7 +223,8 @@ TEST(PathFinder, robot_size) { // size 0 r2d2::Translation robotBox{0 * r2d2::Length::METER, 0 * r2d2::Length::METER, 0 * r2d2::Length::METER}; - r2d2::AStarPathFinder pf(map, robotBox); + LockingSharedObject sharedMap{map}; + r2d2::AStarPathFinder pf(sharedMap, {{}, robotBox}); r2d2::Coordinate start{10.5 * r2d2::Length::METER, 10.5 * r2d2::Length::METER, @@ -233,7 +241,8 @@ TEST(PathFinder, robot_size) { r2d2::Translation robotBox2{5 * r2d2::Length::METER, 5 * r2d2::Length::METER, 0 * r2d2::Length::METER}; - r2d2::AStarPathFinder pf1(map, robotBox2); + LockingSharedObject sharedMap2{map}; + r2d2::AStarPathFinder pf1(sharedMap2, {{}, robotBox2}); ASSERT_TRUE(pf1.get_path_to_coordinate(start, goal, path)) << start << " " << goal << " robot with size 5"; ASSERT_FALSE(path.empty()); @@ -246,7 +255,8 @@ TEST(PathFinder, obstacle_on_begin) { r2d2::Translation robotBox{.5 * r2d2::Length::METER, .5 * r2d2::Length::METER, 0 * r2d2::Length::METER}; - r2d2::AStarPathFinder pf(map, robotBox); + LockingSharedObject sharedMap{map}; + r2d2::AStarPathFinder pf(sharedMap, {{}, robotBox}); r2d2::Coordinate start{.5 * r2d2::Length::METER, .5 * r2d2::Length::METER, @@ -267,7 +277,8 @@ TEST(PathFinder, obstacle_on_end) { r2d2::Translation robotBox{.5 * r2d2::Length::METER, .5 * r2d2::Length::METER, 0 * r2d2::Length::METER}; - r2d2::AStarPathFinder pf(map, robotBox); + LockingSharedObject sharedMap{map}; + r2d2::AStarPathFinder pf(sharedMap, {{}, robotBox}); r2d2::Coordinate start{.5 * r2d2::Length::METER, .5 * r2d2::Length::METER, @@ -286,7 +297,8 @@ TEST(PathFinder, float) { r2d2::Translation robotBox{.5 * r2d2::Length::METER, .5 * r2d2::Length::METER, 0 * r2d2::Length::METER}; - r2d2::AStarPathFinder pf(map, robotBox); + LockingSharedObject sharedMap{map}; + r2d2::AStarPathFinder pf(sharedMap, {{}, robotBox}); r2d2::Coordinate start{.56 * r2d2::Length::METER, .52 * r2d2::Length::METER, 0 * r2d2::Length::METER}; @@ -304,7 +316,8 @@ TEST(PathFinder, same_begin_as_end) { r2d2::Translation robotBox{.5 * r2d2::Length::METER, .5 * r2d2::Length::METER, 0 * r2d2::Length::METER}; - r2d2::AStarPathFinder pf(map, robotBox); + LockingSharedObject sharedMap{map}; + r2d2::AStarPathFinder pf(sharedMap, {{}, robotBox}); r2d2::Coordinate start{1 * r2d2::Length::METER, 1 * r2d2::Length::METER, @@ -335,7 +348,8 @@ TEST(PathFinder, corner_squeezing) { r2d2::Translation robotBox{.5 * r2d2::Length::METER, .5 * r2d2::Length::METER, 0 * r2d2::Length::METER}; - r2d2::AStarPathFinder pf(map, robotBox); + LockingSharedObject sharedMap{map}; + r2d2::AStarPathFinder pf(sharedMap, {{}, robotBox}); r2d2::Coordinate start{.5 * r2d2::Length::METER, .5 * r2d2::Length::METER, @@ -406,10 +420,12 @@ TEST(PathFinder, image_test) { while (!done) { r2d2::Map map = {mapX, mapY, 0.4f}; - r2d2::AStarPathFinder pathFinder = {map, - {0.5 * r2d2::Length::METER, - 0.5 * r2d2::Length::METER, - 0 * r2d2::Length::METER}}; + LockingSharedObject sharedMap{map}; + r2d2::AStarPathFinder pathFinder = {sharedMap, + {{}, + r2d2::Translation{0.5 * r2d2::Length::METER, + 0.5 * r2d2::Length::METER, + 0 * r2d2::Length::METER}}}; std::vector path; done = pathFinder.get_path_to_coordinate( {5.5f * r2d2::Length::METER, From 0c47e6c90a9cc3affdb428f4231d70bf03bc96ac Mon Sep 17 00:00:00 2001 From: ilias-a Date: Wed, 15 Jun 2016 12:16:54 +0200 Subject: [PATCH 05/11] Created folder structure --- CMakeLists.txt | 17 ++++++++++++----- example/main.cpp | 45 +++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 57 insertions(+), 5 deletions(-) create mode 100644 example/main.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 0c2aace..f96b1de 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,26 +2,33 @@ cmake_minimum_required(VERSION 2.8.9) project(R2D2_pathfinding) find_package (Threads) -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=gnu++11 -Wall -Werror") - -file(GLOB SOURCES_GTEST "source/src/*.cpp" "source/include/*.hpp" - "test/*.cpp" "test/*.hpp" +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") + "../sharedobjects/source/include/NotCopyable.hpp" + ) list(REMOVE_ITEM SOURCES_GTEST ${CMAKE_CURRENT_SOURCE_DIR}../adt/source/src/main.cpp) + +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=gnu++11 -Wall") + include_directories( + ../adt/source/include + ../sharedobjects/source/include ../deps/gtest-1.7.0/include ../deps/gtest-1.7.0) + link_directories(../deps/gtest-1.7.0/src) ADD_LIBRARY(gtest ../deps/gtest-1.7.0/src/gtest-all.cc) 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 diff --git a/example/main.cpp b/example/main.cpp new file mode 100644 index 0000000..ac277d7 --- /dev/null +++ b/example/main.cpp @@ -0,0 +1,45 @@ + +#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 "Translation.hpp" +#include "SharedObject.hpp" +#include "LockingSharedObject.hpp" + +#include "iostream" + +int main(int ac, char*av []){ + +//initializing 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); +} +r2d2::Map 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}); + +} From f7a18ed6e5a20c7f63a7091892ddc6d12be3d09a Mon Sep 17 00:00:00 2001 From: ilias-a Date: Wed, 15 Jun 2016 13:57:00 +0200 Subject: [PATCH 06/11] pathfinding examples fully written and doumented --- example/main.cpp | 40 +++++++++++++++++++++++++++------------- 1 file changed, 27 insertions(+), 13 deletions(-) diff --git a/example/main.cpp b/example/main.cpp index ac277d7..4e93191 100644 --- a/example/main.cpp +++ b/example/main.cpp @@ -6,40 +6,54 @@ #include "Box.hpp" #include "Coordinate.hpp" #include "Length.hpp" +#include "Angle.hpp" #include "Translation.hpp" #include "SharedObject.hpp" #include "LockingSharedObject.hpp" #include "iostream" + int main(int ac, char*av []){ -//initializing a map +//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) { + std::vector current; + for (int y = 50; y > 0; y--) { + if (x == y) { if (x >= 4 && x <= 6){ - current.push_back(0); + current.push_back(0); } else{ - current.push_back(1); + current.push_back(1); } - } else { + } else { current.push_back(0); - } - } - cornerSqueezeMap.push_back(current); + } + } + cornerSqueezeMap.push_back(current); } +//initializing a map r2d2::Map map(cornerSqueezeMap); map.print_map(); // initializing pathfinding -r2d2::Translation robotBox{.5 * r2d2::Length::METER, - .5 * r2d2::Length::METER, - 0 * r2d2::Length::METER}; +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); } From 9e87b1be94f2ea9e19dd57a6ec7efcb583cd02d1 Mon Sep 17 00:00:00 2001 From: chieltbest Date: Wed, 15 Jun 2016 14:03:29 +0200 Subject: [PATCH 07/11] replaced the map stub with ReadOnlyMap, addressed some issues in the code --- CMakeLists.txt | 23 +++++---- source/include/AStarPathFinder.hpp | 17 ++++--- source/include/Astar.hpp | 16 +++--- source/include/Dummy.hpp | 28 +++-------- source/include/PathFinder.hpp | 5 +- source/src/AStarPathFinder.cpp | 25 +++++----- source/src/Dummy.cpp | 79 +++++++++++++++--------------- test/PathFinder_Test.cpp | 68 ++++++++++++------------- 8 files changed, 127 insertions(+), 134 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 0c2aace..d9e51cb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,16 +4,19 @@ find_package (Threads) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=gnu++11 -Wall -Werror") -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_GTEST + ../adt/source/src/Length.cpp + ../adt/source/src/Coordinate.cpp + ../adt/source/src/Translation.cpp + ../adt/source/src/Box.cpp + ../adt/source/src/Box.hpp + ../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) include_directories( ../deps/gtest-1.7.0/include 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..11a002b 100644 --- a/source/include/Dummy.hpp +++ b/source/include/Dummy.hpp @@ -53,6 +53,7 @@ #include #include #include +#include "../../../map/source/include/MapInterface.hpp" #include "../../../adt/source/include/Coordinate.hpp" #include "../../../adt/source/include/Translation.hpp" @@ -61,7 +62,7 @@ namespace r2d2 { /*! * 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, From 728931e56c24d4a92ccd25e450a3fb8349d82755 Mon Sep 17 00:00:00 2001 From: chieltbest Date: Thu, 16 Jun 2016 16:07:30 +0200 Subject: [PATCH 08/11] hotfix include directories --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d9e51cb..791819e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,7 +9,6 @@ file(GLOB SOURCES_GTEST ../adt/source/src/Coordinate.cpp ../adt/source/src/Translation.cpp ../adt/source/src/Box.cpp - ../adt/source/src/Box.hpp ../map/source/src/MapInterface.cpp source/src/Dummy.cpp source/src/AStarPathFinder.cpp @@ -19,6 +18,7 @@ file(GLOB SOURCES_GTEST ../sharedobjects/source/include/NotCopyable.hpp) include_directories( + ../adt/source/include ../deps/gtest-1.7.0/include ../deps/gtest-1.7.0) From cbf3a446fe9a14e9a1bf0abb340a48afed8c8c78 Mon Sep 17 00:00:00 2001 From: chieltbest Date: Thu, 16 Jun 2016 23:08:11 +0200 Subject: [PATCH 09/11] (probably) actually fixed the includes now --- source/include/Dummy.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/source/include/Dummy.hpp b/source/include/Dummy.hpp index 99dfb94..11a002b 100644 --- a/source/include/Dummy.hpp +++ b/source/include/Dummy.hpp @@ -53,9 +53,9 @@ #include #include #include -#include -#include -#include +#include "../../../map/source/include/MapInterface.hpp" +#include "../../../adt/source/include/Coordinate.hpp" +#include "../../../adt/source/include/Translation.hpp" namespace r2d2 { //! Dummy Map From 0fa435477f402b22ef9d4a453c4ebcc37ac8121f Mon Sep 17 00:00:00 2001 From: chieltbest Date: Thu, 16 Jun 2016 23:18:19 +0200 Subject: [PATCH 10/11] Revert "(probably) actually fixed the includes now" This reverts commit cbf3a446fe9a14e9a1bf0abb340a48afed8c8c78. --- source/include/Dummy.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/source/include/Dummy.hpp b/source/include/Dummy.hpp index 11a002b..99dfb94 100644 --- a/source/include/Dummy.hpp +++ b/source/include/Dummy.hpp @@ -53,9 +53,9 @@ #include #include #include -#include "../../../map/source/include/MapInterface.hpp" -#include "../../../adt/source/include/Coordinate.hpp" -#include "../../../adt/source/include/Translation.hpp" +#include +#include +#include namespace r2d2 { //! Dummy Map From 48e34ba8d6376b3be07c19aacef272d2f24edd25 Mon Sep 17 00:00:00 2001 From: chieltbest Date: Thu, 16 Jun 2016 23:30:46 +0200 Subject: [PATCH 11/11] fixed std namespace classifier --- example/main.cpp | 89 ++++++++++++++++++++++-------------------------- 1 file changed, 40 insertions(+), 49 deletions(-) diff --git a/example/main.cpp b/example/main.cpp index 61237ce..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::Dummy 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); }