diff --git a/CMakeLists.txt b/CMakeLists.txt index a0ada3c..77b2ce0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,24 +2,44 @@ 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 + 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 "source/src/*.cpp" "source/include/*.hpp" - "../adt/source/src/*.cpp" "../adt/source/include/*.hpp") -file(GLOB SOURCES_GTEST "source/src/*.cpp" "source/include/*.hpp" - "test/*.cpp" "test/*.hpp" - "../adt/source/src/*.cpp" "../adt/source/include/*.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) +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 ../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 ${SOURCES}) +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 new file mode 100644 index 0000000..26d0249 --- /dev/null +++ b/example/main.cpp @@ -0,0 +1,50 @@ +#include "../source/include/PathFinder.hpp" +#include "../source/include/AStarPathFinder.hpp" +#include "Angle.hpp" +#include "LockingSharedObject.hpp" + +#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(); + + // 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)}; + std::cout << path_finder.get_path_to_coordinate(c1, c2, path_vector); +} diff --git a/source/include/AStarPathFinder.hpp b/source/include/AStarPathFinder.hpp new file mode 100644 index 0000000..02e78a1 --- /dev/null +++ b/source/include/AStarPathFinder.hpp @@ -0,0 +1,192 @@ +//! \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 +#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 1 + +namespace r2d2 { + + /** + * interface for a pathfinder module + * + * computes a path between two points on a map + */ + class AStarPathFinder : public PathFinder { + public: + AStarPathFinder(SharedObject &map, Box 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; + + 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 << ")"; + } + + }; + friend struct std::hash; + + SharedObject ↦ + std::weak_ptr::Accessor> mapAccessor; + std::atomic referenceCount; + 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/Astar.hpp b/source/include/Astar.hpp index f35ed0b..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 @@ -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 fitting use case std::pop_heap(open.begin(), open.end(), [](std::shared_ptr &n1, std::shared_ptr &n2) { @@ -147,12 +150,21 @@ 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)}; - auto result = closed.emplace(child); - if (result.second) { - open.emplace_back(*result.first); + // add the child to the closed set + 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.push_back(*result.first); std::push_heap( open.begin(), open.end(), [](std::shared_ptr &n1, @@ -161,6 +173,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/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 0c1a789..b7fab42 100644 --- a/source/include/PathFinder.hpp +++ b/source/include/PathFinder.hpp @@ -49,145 +49,44 @@ #ifndef R2D2_PATHFINDING_PATHFINDER_HPP #define R2D2_PATHFINDING_PATHFINDER_HPP - #include -#include +#include "../../../adt/source/include/Box.hpp" +#include "../../../sharedobjects/source/include/SharedObject.hpp" #include "Dummy.hpp" -#include "Astar.hpp" -#include "../../../adt/source/include/Coordinate.hpp" -#include "../../../adt/source/include/Length.hpp" -#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 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); - - /** - * 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 << ")"; - } - }; - - private: - 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); - - std::vector get_path(std::shared_ptr start); - - 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(SharedObject &map, Box 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 73% rename from source/src/PathFinder.cpp rename to source/src/AStarPathFinder.cpp index e6595c7..b86f48f 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,19 @@ //! 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(SharedObject &map, Box robotBox) : + PathFinder{map, robotBox}, map(map), - robotBox(robotBox) { + mapAccessor{}, + referenceCount{0}, + robotBox{robotBox.get_axis_size()} { } - bool PathFinder::get_path_to_coordinate(Coordinate start, + bool AStarPathFinder::get_path_to_coordinate(Coordinate start, Coordinate goal, std::vector &path) { @@ -66,6 +69,13 @@ namespace r2d2 { path.clear(); return true; } + + 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 + } + // do a check for end node accessibility before starting the search if (!can_travel(goal, goal)) { return false; @@ -74,39 +84,37 @@ 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; } + + return foundStart != nullptr; } - 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++) { @@ -114,20 +122,20 @@ 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 + PathFinder::get_heuristic( + g + AStarPathFinder::get_heuristic( childPos - coord ), // distance from the search begin self}); @@ -138,12 +146,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()), @@ -153,10 +161,11 @@ 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); + BoxInfo info{mapAccessor.lock()->access().get_box_info(Box{minCoord - (robotBox / 2), size})}; + return !(info.get_has_obstacle() || info.get_has_unknown()); } - 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 +182,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,26 +199,29 @@ 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 (!curNode->parent.expired()) { // while next != nullptr curNode = std::shared_ptr(curNode->parent); path.emplace_back(*curNode); } 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(); 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/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/source/src/main.cpp b/source/src/main.cpp deleted file mode 100644 index 0e876af..0000000 --- a/source/src/main.cpp +++ /dev/null @@ -1,140 +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 -//! -//! \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)); - } - }; -} -// prints out a .pgm (portable grey r2d2::Map) image of the r2d2::Map and it's path -#define SCALE 4 - -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 << " "; - } else { - 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..a04fae7 100644 --- a/test/PathFinder_Test.cpp +++ b/test/PathFinder_Test.cpp @@ -51,7 +51,8 @@ #include #include #include "../source/include/Dummy.hpp" -#include "../source/include/PathFinder.hpp" +#include "../source/include/AStarPathFinder.hpp" +#include "../../sharedobjects/source/include/LockingSharedObject.hpp" bool equal(const std::vector &lhs, const std::vector &rhs) { @@ -92,27 +93,29 @@ 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); - r2d2::PathFinder pf(map, robotBox); + 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}; - r2d2::PathFinder 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, @@ -125,11 +128,12 @@ 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}; - r2d2::PathFinder 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, @@ -141,11 +145,12 @@ 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}; - r2d2::PathFinder pf(map, robotBox); + LockingSharedObject sharedMap{map}; + r2d2::AStarPathFinder pf(sharedMap, {{}, robotBox}); r2d2::Coordinate start{.5 * r2d2::Length::METER, .5 * r2d2::Length::METER, @@ -160,11 +165,12 @@ 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}; - r2d2::PathFinder 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, @@ -199,24 +205,26 @@ 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; - r2d2::PathFinder 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)); } 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}; - r2d2::PathFinder 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::PathFinder 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()); @@ -242,11 +251,12 @@ 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}; - r2d2::PathFinder pf(map, robotBox); + LockingSharedObject sharedMap{map}; + r2d2::AStarPathFinder pf(sharedMap, {{}, robotBox}); r2d2::Coordinate start{.5 * r2d2::Length::METER, .5 * r2d2::Length::METER, @@ -263,11 +273,12 @@ 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}; - r2d2::PathFinder pf(map, robotBox); + LockingSharedObject sharedMap{map}; + r2d2::AStarPathFinder pf(sharedMap, {{}, robotBox}); r2d2::Coordinate start{.5 * r2d2::Length::METER, .5 * r2d2::Length::METER, @@ -282,11 +293,12 @@ 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}; - r2d2::PathFinder 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}; @@ -300,11 +312,12 @@ 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}; - r2d2::PathFinder pf(map, robotBox); + LockingSharedObject sharedMap{map}; + r2d2::AStarPathFinder pf(sharedMap, {{}, robotBox}); r2d2::Coordinate start{1 * r2d2::Length::METER, 1 * r2d2::Length::METER, @@ -328,14 +341,15 @@ 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}; - r2d2::PathFinder pf(map, robotBox); + LockingSharedObject sharedMap{map}; + r2d2::AStarPathFinder pf(sharedMap, {{}, robotBox}); r2d2::Coordinate start{.5 * r2d2::Length::METER, .5 * r2d2::Length::METER, @@ -349,6 +363,96 @@ 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::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(i2, i1)) != 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 = 50, mapY = 50, mapCount = 0; + bool done = false; + while (!done) { + r2d2::Dummy map = {mapX, mapY, 0.4f}; + + 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, + 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();