├── samples ├── output │ └── .gitkeep ├── input │ └── map.bmp ├── inflator.cpp ├── global_path_planner.cpp ├── local_path_planner.cpp └── coverage_path_planner.cpp ├── .gitignore ├── test results ├── test1.jpg └── test2.jpg ├── include ├── common │ ├── Types.hpp │ ├── ColourDefinitions.hpp │ ├── ImageMarker.hpp │ ├── CoordMapper.hpp │ ├── Shifts.hpp │ ├── Coord.hpp │ └── CellMapCreator.hpp ├── global_path_planner │ ├── DistanceTransformer.hpp │ ├── ObstacleTransformer.hpp │ ├── GlobalPathPlanner.hpp │ └── PathTransformer.hpp ├── path_analizer │ └── PathAnalizer.hpp ├── inflator │ └── Inflator.hpp └── local_path_planner │ └── LocalPathPlanner.hpp ├── LICENSE ├── src ├── common │ ├── CoordMapper.cpp │ ├── CellMapCreator.cpp │ └── ImageMarker.cpp ├── global_path_planner │ ├── PathTransformer.cpp │ ├── DistanceTransformer.cpp │ ├── GlobalPathPlanner.cpp │ └── ObstacleTransformer.cpp ├── path_analizer │ └── PathAnalizer.cpp ├── inflator │ └── Inflator.cpp └── local_path_planner │ └── LocalPathPlanner.cpp ├── CMakeLists.txt └── readme.txt /samples/output/.gitkeep: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | include/common/bitmap_image.hpp 2 | build/ -------------------------------------------------------------------------------- /samples/input/map.bmp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AzamatAlmukhametov/coverage-path-planning/HEAD/samples/input/map.bmp -------------------------------------------------------------------------------- /test results/test1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AzamatAlmukhametov/coverage-path-planning/HEAD/test results/test1.jpg -------------------------------------------------------------------------------- /test results/test2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AzamatAlmukhametov/coverage-path-planning/HEAD/test results/test2.jpg -------------------------------------------------------------------------------- /include/common/Types.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | enum class cell { 4 | FREE, 5 | OBSTACLE, 6 | PARTIALLY_FREE 7 | }; 8 | 9 | struct Cell { 10 | cell type; 11 | int weight; 12 | }; 13 | -------------------------------------------------------------------------------- /include/common/ColourDefinitions.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "bitmap_image.hpp" 4 | 5 | const rgb_t obstColour{ 0, 0, 0 }, 6 | freeSpaceColour{ 255, 255, 255 }, 7 | borderColor{ 0, 255, 0 }, 8 | infColour{ 0, 0, 255 }, 9 | obstacleRectColour{255, 0, 0}, 10 | partiallyFreeRectColour{0, 255, 255}, 11 | freeRectColour{135, 206, 235}, 12 | pathMarkColour{160, 32, 240}; -------------------------------------------------------------------------------- /include/common/ImageMarker.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "bitmap_image.hpp" 4 | 5 | #include "Coord.hpp" 6 | #include "Types.hpp" 7 | 8 | namespace ImageMarker { 9 | using CellMap = std::vector>; 10 | 11 | // Mark cells on an image using cell map. 12 | void markCells(bitmap_image& image, const CellMap& cellMap, int cellSize); 13 | 14 | // Mark path on an image using cell path coordinates and cell size. 15 | void markPath(bitmap_image& image, 16 | const std::vector>& pathCoords, 17 | int cellSize); 18 | }; -------------------------------------------------------------------------------- /include/common/CoordMapper.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "Coord.hpp" 6 | 7 | namespace CoordMapper { 8 | 9 | // Map cell map coordinate to another cell map coordinte. 10 | Coord map(const Coord& src, int srcCellSize, int dstCellSize); 11 | 12 | // Map cell map coordinates to image coordinates. 13 | std::vector> 14 | mapCellMapToImg(const std::vector>& src, int cellSize, int width, int height); 15 | 16 | // Map image coordinates to cell map coordinates. 17 | std::vector> 18 | mapImgToCellMap(const std::vector>& src, int cellSize, int width, int height); 19 | } -------------------------------------------------------------------------------- /include/common/Shifts.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "Coord.hpp" 6 | 7 | const std::vector> shift4{ { 0, -1 }, 8 | { 0, 1 }, 9 | {-1, 0 }, 10 | { 1, 0 } }; 11 | 12 | const std::vector> shift8{ {-1, -1 }, 13 | { 1, -1 }, 14 | {-1, 1 }, 15 | { 1, 1 }, 16 | { 0, -1 }, 17 | {-1, 0 }, 18 | { 1, 0 }, 19 | { 0, 1 } }; -------------------------------------------------------------------------------- /include/global_path_planner/DistanceTransformer.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include "Coord.hpp" 7 | #include "Types.hpp" 8 | 9 | // Create a weight map using wave proragation algorithm. 10 | // The Wave starts at start coordinate and go through free space. 11 | class DistanceTransformer { 12 | public: 13 | using CellMap = std::vector>; 14 | 15 | CellMap transform(const CellMap& cellMap, const Coord& start); 16 | 17 | private: 18 | bool isInImage(const Coord& coord) const; 19 | 20 | void updateRowsAndCols(const CellMap& map); 21 | 22 | bool setStartCoord(const CellMap& map, const Coord& startArg); 23 | 24 | int rows = 0, cols = 0; 25 | Coord start = {0, 0}; 26 | }; -------------------------------------------------------------------------------- /samples/inflator.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "bitmap_image.hpp" 4 | 5 | #include "Inflator.hpp" 6 | 7 | namespace configs { 8 | const std::string inputMapFileName = "./input/map.bmp"; 9 | const std::string outputMapFileName = "./output/inflated.bmp"; 10 | const int radius = 25; 11 | }; 12 | 13 | int main() 14 | { 15 | bitmap_image image(configs::inputMapFileName); 16 | if (!image) { 17 | std::cerr << "Error - Failed to open: " 18 | << configs::inputMapFileName << std::endl; 19 | return 1; 20 | } 21 | 22 | Inflator inf(image); 23 | inf.inflate(configs::radius); 24 | 25 | std::cout << "saving image" << std::endl; 26 | inf.getImage().save_image(configs::outputMapFileName); 27 | 28 | return 0; 29 | } -------------------------------------------------------------------------------- /include/global_path_planner/ObstacleTransformer.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include "Coord.hpp" 7 | #include "Types.hpp" 8 | 9 | // Create a weight map using wave proragation algorithm. 10 | // The Wave starts at obstacles border coordinates and go through free space. 11 | class ObstacleTransformer { 12 | public: 13 | using CellMap = std::vector>; 14 | 15 | CellMap transform(const CellMap& cellMap); 16 | 17 | private: 18 | bool isInImage(const Coord& coord) const; 19 | 20 | void addObstacleBorder(const CellMap& map, 21 | std::queue>& coords, 22 | std::vector>& visited) const; 23 | 24 | void updateRowsAndCols(const CellMap& cellMap); 25 | 26 | int rows = 0, cols = 0; 27 | }; 28 | -------------------------------------------------------------------------------- /include/common/Coord.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | template 4 | struct Coord { 5 | T x, y; 6 | 7 | Coord(T x, T y) : x(x), y(y) {}; 8 | Coord() { x = 0; y = 0; }; 9 | Coord(const Coord& a) : x(a.x), y(a.y) {}; 10 | 11 | void reInit(T x, T y) { 12 | this->x = x; 13 | this->y = y; 14 | } 15 | 16 | Coord& operator=(const Coord& a) { 17 | x = a.x; 18 | y = a.y; 19 | 20 | return *this; 21 | } 22 | 23 | Coord operator+(const Coord& a) const { 24 | return Coord{ x + a.x, y + a.y }; 25 | } 26 | 27 | Coord operator-(const Coord& a) const { 28 | return Coord{ x - a.x, y - a.y }; 29 | } 30 | 31 | bool operator==(const Coord& a) const { 32 | return (x == a.x && y == a.y); 33 | } 34 | 35 | bool operator!=(const Coord& a) const { 36 | return !(*this == a); 37 | } 38 | }; 39 | -------------------------------------------------------------------------------- /include/path_analizer/PathAnalizer.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "Coord.hpp" 6 | 7 | // Simple path analizer. 8 | // Use path length and overall turn angle to choose the best path. 9 | // Costfunction = length * lengthWeight + angle * angleWeight. 10 | class PathAnalizer { 11 | public: 12 | PathAnalizer(double lengthWeightArg, double angleWeightArg); 13 | 14 | std::vector> 15 | findBestPath(const std::vector>>& paths); 16 | 17 | private: 18 | float computeAngle(const std::vector>& path) const; 19 | 20 | long long computePathLength(const std::vector>& path) const; 21 | 22 | inline double getDist(const Coord& c0, const Coord& c1) const; 23 | 24 | struct PathStat { 25 | int i; 26 | long long length; 27 | float angle; 28 | }; 29 | 30 | std::vector pathsStat; 31 | 32 | double lengthWeight = 0.7; 33 | double angleWeight = 0.3; 34 | }; -------------------------------------------------------------------------------- /include/common/CellMapCreator.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "bitmap_image.hpp" 6 | 7 | #include "Types.hpp" 8 | 9 | // Create cell map from image. 10 | // Non free space colour pixels related as an obstacle. 11 | // Parameters: 12 | // cellSize - size of the the square cell 13 | // occupiedThreshold - 0..100% - number of non free space pixel color divided by bumbers of pixels in one cell 14 | // freeSpaceColour - free space image pixel colour 15 | class CellMapCreator { 16 | public: 17 | using CellMap = std::vector>; 18 | 19 | CellMap create(const bitmap_image& image, int cellSize) const; 20 | 21 | bool setOccupiedThreshold(int val); 22 | 23 | void setFreeSpaceColour(rgb_t colour); 24 | 25 | private: 26 | int calcOccupiedArea(const bitmap_image& image, int x0, int y0, int cellSize) const; 27 | 28 | int occupiedThreshold = 80; // 0 .. 100%; 29 | rgb_t freeSpaceColour = {255, 255, 255}; // default is white colour 30 | }; 31 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2024 AzamatAlmukhametov 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. -------------------------------------------------------------------------------- /include/global_path_planner/GlobalPathPlanner.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "Coord.hpp" 6 | #include "Types.hpp" 7 | 8 | // Plan path using weight map and start coordinate. 9 | // If weight map is the representation of potential field in plane, 10 | // then planned path lies at the bottom of the potential field. 11 | // At each step the next cell with least weight is choosen among neighbour cells. 12 | class GlobalPathPlanner { 13 | public: 14 | using CellMap = std::vector>; 15 | 16 | std::vector> planPath(const CellMap& weightMap, 17 | const Coord& start); 18 | 19 | private: 20 | struct Node { 21 | Coord coord; 22 | Cell cell; 23 | 24 | bool operator>(const Node& n) const { 25 | return cell.weight > n.cell.weight; 26 | } 27 | }; 28 | 29 | bool isInImage(const Coord& coord) const; 30 | 31 | void updateRowsAndCols(const CellMap& weightMap); 32 | 33 | bool setStartCoord(const CellMap& weightMap, const Coord& startArg); 34 | 35 | int rows = 0, cols = 0; 36 | Coord start = {0, 0}; 37 | }; -------------------------------------------------------------------------------- /include/inflator/Inflator.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include "bitmap_image.hpp" 7 | 8 | #include "Coord.hpp" 9 | 10 | class Inflator { 11 | public: 12 | Inflator(const bitmap_image& img); 13 | 14 | void inflate(int radius); 15 | 16 | bitmap_image getImage() const; 17 | 18 | private: 19 | struct Wave { 20 | Coord coord; 21 | int i; 22 | }; 23 | 24 | void applyBinFilter(); 25 | 26 | bool isInImage(int x, int y) const; 27 | 28 | std::vector> getObstacleBorder() const; 29 | 30 | void filterInflatedCoords(std::vector>& src, 31 | std::vector>& dst) const; 32 | 33 | void addLineCoords(int x0, int y0, int dx, int dy, int l, int group, 34 | std::queue& wave) const; 35 | 36 | void addSeedsCoords(const std::vector>& bases, int radius, 37 | std::queue& wave) const; 38 | 39 | void inflateFromBase(const std::vector>& bases, int radius, 40 | std::vector>& newBases); 41 | 42 | bitmap_image image; 43 | const uint frameWidth = 1; 44 | }; -------------------------------------------------------------------------------- /include/global_path_planner/PathTransformer.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "Coord.hpp" 6 | #include "Types.hpp" 7 | 8 | // Create a weight map using costfunction with the following arguments: 9 | // distance transformer map (dtMap) 10 | // obstacle transformer map (otMap) 11 | // max weight in obstacle transformer map (maxWeight) 12 | // Costfunciton: 13 | // const double scaler = dtMaxWeight == 0 ? 1 : (double)otMaxWeight / dtMaxWeight; 14 | // map[y][x].weight = (int)round(scaler * (dtMulti * dtMap[y][x].weight)) + 15 | // otMulti * otMaxWeight / otMap[y][x].weight; 16 | class PathTransformer { 17 | public: 18 | using CellMap = std::vector>; 19 | 20 | PathTransformer(const CellMap& cellMap, const Coord& start); 21 | 22 | CellMap transform() const; 23 | 24 | void setObstacleTransformerMultiplier(int multiplier) { 25 | otMulti = multiplier; 26 | } 27 | 28 | void setDistanceTransformerMultiplier(int multiplier) { 29 | dtMulti = multiplier; 30 | } 31 | 32 | private: 33 | 34 | int findMaxWeight(const CellMap& map) const; 35 | 36 | CellMap otMap; 37 | CellMap dtMap; 38 | int otMulti = 1; 39 | int dtMulti = 1; 40 | }; -------------------------------------------------------------------------------- /include/local_path_planner/LocalPathPlanner.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include "Coord.hpp" 7 | #include "Types.hpp" 8 | 9 | // Plan the shortest path between two points of sequence of points. 10 | // The approach use wave propagation algorithm. 11 | class LocalPathPlanner { 12 | public: 13 | enum class wavePropAlg { 14 | AROUND, 15 | TOWARD_TO_AIM 16 | }; 17 | 18 | using CellMap = std::vector>; 19 | 20 | LocalPathPlanner(wavePropAlg algArg); 21 | 22 | std::vector> 23 | planPath(const CellMap& cellMap, const std::vector>& coords); 24 | 25 | private: 26 | bool isInImage(const Coord& coord) const; 27 | 28 | bool findNearNotObstCell(Coord& base); 29 | 30 | bool propagateWaveAround(const Coord& start, const Coord& end, int operationId); 31 | bool propagateWaveTowardToAim(const Coord& start, const Coord& end, int operationId); 32 | 33 | void addToPath(const Coord& start, const Coord& end, 34 | std::vector>& path, int operationId); 35 | 36 | struct Node { 37 | Cell cell; 38 | int operationId = 0; 39 | }; 40 | 41 | wavePropAlg alg = wavePropAlg::AROUND; 42 | std::function&, const Coord&, int)> propagateWave; 43 | 44 | std::vector> map; 45 | int rows = 0, cols = 0; 46 | const int initOperationId = -1; 47 | 48 | const int searchAreaRadius = 4; // Radius of search area to find non-obstacle cell 49 | const int findNearNotObstCellOperationId = -2; 50 | }; -------------------------------------------------------------------------------- /src/common/CoordMapper.cpp: -------------------------------------------------------------------------------- 1 | #include "CoordMapper.hpp" 2 | 3 | #include 4 | 5 | Coord CoordMapper::map(const Coord& src, int srcCellSize, int dstCellSize) { 6 | Coord dst; 7 | dst.x = (src.x * srcCellSize + srcCellSize / 2) / dstCellSize; 8 | dst.y = (src.y * srcCellSize + srcCellSize / 2) / dstCellSize; 9 | return dst; 10 | } 11 | 12 | std::vector> 13 | CoordMapper::mapCellMapToImg(const std::vector>& src, 14 | int scrCellSize, int width, int height) { 15 | std::vector> dst; 16 | for (auto coord : src) { 17 | coord.x *= scrCellSize; 18 | coord.y *= scrCellSize; 19 | if (coord.x >= width || coord.y >= height) { 20 | throw std::runtime_error("coordinates out of range"); 21 | } 22 | 23 | coord.x += (coord.x + scrCellSize < width) ? scrCellSize / 2 : (width - coord.x) / 2; 24 | coord.y += (coord.y + scrCellSize < height) ? scrCellSize / 2 : (height - coord.y) / 2; 25 | dst.push_back(coord); 26 | } 27 | 28 | return dst; 29 | } 30 | 31 | std::vector> 32 | CoordMapper::mapImgToCellMap(const std::vector>& src, 33 | int dstCellSize, int width, int height) { 34 | std::vector> dst; 35 | for (auto coord : src) { 36 | coord.x = coord.x / dstCellSize; 37 | coord.y = coord.y / dstCellSize; 38 | if (coord.x >= width || coord.y >= height) { 39 | throw std::runtime_error("coordinates out of range"); 40 | } 41 | dst.push_back(coord); 42 | } 43 | 44 | return dst; 45 | } -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | 3 | project(coverage_path_planning) 4 | 5 | set(CMAKE_CXX_STANDARD 14) 6 | 7 | include_directories( 8 | include/common 9 | include/global_path_planner 10 | include/inflator 11 | include/local_path_planner 12 | include/path_analizer 13 | ) 14 | 15 | set(COMMON 16 | src/common/CellMapCreator.cpp 17 | src/common/CoordMapper.cpp 18 | src/common/ImageMarker.cpp 19 | ) 20 | 21 | set(GLOBAL_PATH_PLANNER 22 | src/global_path_planner/DistanceTransformer.cpp 23 | src/global_path_planner/GlobalPathPlanner.cpp 24 | src/global_path_planner/ObstacleTransformer.cpp 25 | src/global_path_planner/PathTransformer.cpp 26 | ) 27 | 28 | set(INFLATOR 29 | src/inflator/Inflator.cpp 30 | ) 31 | 32 | set(LOCAL_PATH_PLANNER 33 | src/local_path_planner/LocalPathPlanner.cpp 34 | ) 35 | 36 | set(PATH_ANALIZER 37 | src/path_analizer/PathAnalizer.cpp 38 | ) 39 | 40 | add_executable( 41 | ${PROJECT_NAME} samples/coverage_path_planner.cpp 42 | ${COMMON} 43 | ${GLOBAL_PATH_PLANNER} 44 | ${INFLATOR} 45 | ${LOCAL_PATH_PLANNER} 46 | ${PATH_ANALIZER} 47 | ) 48 | 49 | add_executable( 50 | inflator samples/inflator.cpp 51 | ${COMMON} 52 | ${INFLATOR} 53 | ) 54 | 55 | add_executable( 56 | global_path_planner samples/global_path_planner.cpp 57 | ${COMMON} 58 | ${GLOBAL_PATH_PLANNER} 59 | ) 60 | 61 | add_executable( 62 | local_path_planner samples/local_path_planner.cpp 63 | ${COMMON} 64 | ${LOCAL_PATH_PLANNER} 65 | ) 66 | 67 | file(COPY samples/input DESTINATION ${CMAKE_BINARY_DIR}) 68 | file(COPY samples/output DESTINATION ${CMAKE_BINARY_DIR}) -------------------------------------------------------------------------------- /src/global_path_planner/PathTransformer.cpp: -------------------------------------------------------------------------------- 1 | #include "PathTransformer.hpp" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include "DistanceTransformer.hpp" 8 | #include "ObstacleTransformer.hpp" 9 | 10 | using CellMap = std::vector>; 11 | 12 | PathTransformer::PathTransformer(const CellMap& cellMap, 13 | const Coord& start) { 14 | dtMap = DistanceTransformer().transform(cellMap, start); 15 | otMap = ObstacleTransformer().transform(cellMap); 16 | } 17 | 18 | int PathTransformer::findMaxWeight(const CellMap& map) const { 19 | int maxWeight = 0; 20 | for (auto row : map) { 21 | for (auto el : row) { 22 | maxWeight = std::max(maxWeight, el.weight); 23 | } 24 | } 25 | 26 | return maxWeight; 27 | } 28 | 29 | CellMap PathTransformer::transform() const { 30 | if (dtMap.empty() || otMap.empty()) { 31 | return {}; 32 | } 33 | 34 | assert(dtMap.size() == otMap.size()); 35 | assert(dtMap[0].size() == otMap[0].size()); 36 | 37 | const int rows = dtMap.size(); 38 | const int cols = rows > 0 ? dtMap[0].size() : 0; 39 | CellMap map = CellMap(rows, std::vector(cols)); 40 | 41 | const int otMaxWeight = findMaxWeight(otMap); 42 | const int dtMaxWeight = findMaxWeight(dtMap); 43 | const double scaler = dtMaxWeight == 0 ? 1 : (double)otMaxWeight / dtMaxWeight; 44 | 45 | for (int y = 0; y < rows; ++y) { 46 | for (int x = 0; x < cols; ++x) { 47 | map[y][x].type = dtMap[y][x].type; 48 | 49 | if (dtMap[y][x].type == cell::OBSTACLE) continue; 50 | 51 | assert(otMap[y][x].weight != 0); 52 | map[y][x].weight = (int)round(scaler * (dtMulti * dtMap[y][x].weight)) + 53 | otMulti * otMaxWeight / otMap[y][x].weight; 54 | } 55 | } 56 | 57 | return map; 58 | } -------------------------------------------------------------------------------- /src/common/CellMapCreator.cpp: -------------------------------------------------------------------------------- 1 | #include "CellMapCreator.hpp" 2 | 3 | using CellMap = std::vector>; 4 | 5 | CellMap CellMapCreator::create(const bitmap_image& image, int cellSize) const { 6 | const int rows = image.height() / cellSize + (image.height() % cellSize == 0 ? 0 : 1); 7 | const int cols = image.width() / cellSize + (image.width() % cellSize == 0 ? 0 : 1); 8 | const int initWeight = 1; 9 | 10 | CellMap matrix(rows, std::vector(cols)); 11 | for (int y = 0, imageY = 0; y < rows; ++y, imageY += cellSize) { 12 | for (int x = 0, imageX = 0; x < cols; ++x, imageX += cellSize) { 13 | int occupied = calcOccupiedArea(image, imageX, imageY, cellSize); 14 | if (occupied == 0) { 15 | matrix[y][x] = {cell::FREE, initWeight}; 16 | } else if (occupied > occupiedThreshold) { 17 | matrix[y][x] = {cell::OBSTACLE, initWeight}; 18 | } else { 19 | matrix[y][x] = {cell::PARTIALLY_FREE, initWeight}; 20 | } 21 | } 22 | } 23 | 24 | return matrix; 25 | } 26 | 27 | bool CellMapCreator::setOccupiedThreshold(int val) { 28 | if (val < 0 || 100 > val) { 29 | return false; 30 | } 31 | 32 | occupiedThreshold = val; 33 | return true; 34 | } 35 | 36 | void CellMapCreator::setFreeSpaceColour(rgb_t colour) { 37 | freeSpaceColour = colour; 38 | } 39 | 40 | int CellMapCreator::calcOccupiedArea(const bitmap_image& image, int x0, int y0, int cellSize) const { 41 | int count = 0; 42 | 43 | const int yBorder = (y0 + cellSize >= image.height() ? image.height() : y0 + cellSize); 44 | const int xBorder = (x0 + cellSize >= image.width() ? image.width() : x0 + cellSize); 45 | 46 | for (int y = y0; y < yBorder; ++y) { 47 | for (int x = x0; x < xBorder; ++x) { 48 | rgb_t colour; 49 | image.get_pixel(x, y, colour); 50 | if (colour != freeSpaceColour) { 51 | ++count; 52 | } 53 | } 54 | } 55 | 56 | return count * 100 / ((yBorder - y0) * (xBorder - x0)); 57 | } -------------------------------------------------------------------------------- /src/global_path_planner/DistanceTransformer.cpp: -------------------------------------------------------------------------------- 1 | #include "DistanceTransformer.hpp" 2 | 3 | #include 4 | #include 5 | 6 | #include "Shifts.hpp" 7 | 8 | using CellMap = std::vector>; 9 | 10 | CellMap DistanceTransformer::transform(const CellMap& cellMap, 11 | const Coord& start) { 12 | std::cout << "DistanceTransformer::transform: starts" << std::endl; 13 | 14 | CellMap map = cellMap; 15 | 16 | updateRowsAndCols(map); 17 | if (!setStartCoord(map, start)) { 18 | throw std::runtime_error("start coordinate is invalid"); 19 | } 20 | 21 | std::queue> q; 22 | q.push(start); 23 | 24 | std::vector> visited(rows, std::vector(cols)); 25 | visited[start.y][start.x] = true; 26 | 27 | while (!q.empty()) { 28 | size_t size = q.size(); 29 | while (size--) { 30 | Coord base = q.front(); 31 | q.pop(); 32 | 33 | for (auto sh : shift8) { 34 | Coord coord = base + sh; 35 | 36 | if (!isInImage(coord)) continue; 37 | if (map[coord.y][coord.x].type == cell::OBSTACLE) continue; 38 | if (visited[coord.y][coord.x]) continue; 39 | 40 | map[coord.y][coord.x].weight = map[base.y][base.x].weight + 1; 41 | visited[coord.y][coord.x] = true; 42 | q.push(coord); 43 | } 44 | } 45 | } 46 | 47 | std::cout << "DistanceTransformer::transform: successfull" << std::endl; 48 | return map; 49 | } 50 | 51 | bool DistanceTransformer::isInImage(const Coord& coord) const { 52 | return coord.y >= 0 && coord.y < rows && 53 | coord.x >= 0 && coord.x < cols; 54 | } 55 | 56 | void DistanceTransformer::updateRowsAndCols(const CellMap& map) { 57 | rows = map.size(); 58 | cols = rows > 0 ? map[0].size() : 0; 59 | } 60 | 61 | bool DistanceTransformer::setStartCoord(const CellMap& map, 62 | const Coord& startArg) { 63 | if (isInImage(startArg) && 64 | map[startArg.y][startArg.x].type != cell::OBSTACLE) { 65 | start = startArg; 66 | return true; 67 | } 68 | 69 | return false; 70 | } -------------------------------------------------------------------------------- /src/global_path_planner/GlobalPathPlanner.cpp: -------------------------------------------------------------------------------- 1 | #include "GlobalPathPlanner.hpp" 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | #include "Shifts.hpp" 9 | 10 | using CellMap = std::vector>; 11 | 12 | std::vector> 13 | GlobalPathPlanner::planPath(const CellMap& weightMap, 14 | const Coord& start) { 15 | updateRowsAndCols(weightMap); 16 | if (!setStartCoord(weightMap, start)) { 17 | throw std::runtime_error("start coordinate is invalid"); 18 | } 19 | 20 | std::vector> path; 21 | std::vector> visited(rows, std::vector(cols)); 22 | 23 | std::stack> s; 24 | s.push({{start, weightMap[start.y][start.x]}}); 25 | 26 | while (!s.empty()) { 27 | auto& nodes = s.top(); 28 | if (nodes.empty()) { 29 | s.pop(); 30 | continue; 31 | } 32 | 33 | Node node = nodes.back(); 34 | nodes.pop_back(); 35 | if (visited[node.coord.y][node.coord.x]) continue; 36 | 37 | std::vector newNodes; 38 | for (auto sh : shift8) { 39 | Coord coord = node.coord + sh; 40 | 41 | if (!isInImage(coord)) continue; 42 | if (weightMap[coord.y][coord.x].type == cell::OBSTACLE) continue; 43 | if (visited[coord.y][coord.x]) continue; 44 | 45 | newNodes.push_back({coord, weightMap[coord.y][coord.x]}); 46 | } 47 | 48 | visited[node.coord.y][node.coord.x] = true; 49 | path.push_back(node.coord); 50 | 51 | if (newNodes.empty()) continue; 52 | std::sort(newNodes.begin(), newNodes.end(), std::greater()); 53 | s.push(newNodes); 54 | } 55 | 56 | return path; 57 | } 58 | 59 | bool GlobalPathPlanner::isInImage(const Coord& coord) const { 60 | return coord.y >= 0 && coord.y < rows && 61 | coord.x >= 0 && coord.x < cols; 62 | } 63 | 64 | void GlobalPathPlanner::updateRowsAndCols(const CellMap& weightMap) { 65 | rows = weightMap.size(); 66 | cols = rows > 0 ? weightMap[0].size() : 0; 67 | } 68 | 69 | bool GlobalPathPlanner::setStartCoord(const CellMap& weightMap, 70 | const Coord& startArg) { 71 | if (isInImage(startArg) && 72 | weightMap[startArg.y][startArg.x].type != cell::OBSTACLE) { 73 | start = startArg; 74 | return true; 75 | } 76 | 77 | return false; 78 | } 79 | -------------------------------------------------------------------------------- /src/global_path_planner/ObstacleTransformer.cpp: -------------------------------------------------------------------------------- 1 | #include "ObstacleTransformer.hpp" 2 | 3 | #include 4 | 5 | #include "Shifts.hpp" 6 | 7 | using CellMap = std::vector>; 8 | 9 | CellMap ObstacleTransformer::transform(const CellMap& cellMap) { 10 | std::cout << "ObstacleTransformer::transform: starts" << std::endl; 11 | 12 | CellMap map = cellMap; 13 | 14 | updateRowsAndCols(map); 15 | 16 | std::queue> q; 17 | std::vector> visited(rows, std::vector(cols)); 18 | addObstacleBorder(map, q, visited); 19 | 20 | while (!q.empty()) { 21 | size_t size = q.size(); 22 | while (size--) { 23 | Coord base = q.front(); 24 | q.pop(); 25 | 26 | for (auto sh : shift8) { 27 | Coord coord = base + sh; 28 | 29 | if (!isInImage(coord)) continue; 30 | if (map[coord.y][coord.x].type == cell::OBSTACLE) continue; 31 | if (visited[coord.y][coord.x]) continue; 32 | 33 | map[coord.y][coord.x].weight = map[base.y][base.x].weight + 1; 34 | visited[coord.y][coord.x] = true; 35 | q.push(coord); 36 | } 37 | } 38 | } 39 | 40 | std::cout << "ObstacleTransformer::transform: successfull" << std::endl; 41 | return map; 42 | } 43 | 44 | 45 | bool ObstacleTransformer::isInImage(const Coord& coord) const { 46 | return coord.y >= 0 && coord.y < rows && 47 | coord.x >= 0 && coord.x < cols; 48 | } 49 | 50 | void ObstacleTransformer::addObstacleBorder(const CellMap& map, 51 | std::queue>& coords, 52 | std::vector>& visited) const { 53 | for (int y = 0; y < rows; ++y) { 54 | for (int x = 0; x < cols; ++x) { 55 | if (map[y][x].type != cell::OBSTACLE) continue; 56 | 57 | for (auto sh : shift8) { 58 | Coord coord = Coord{x, y} + sh; 59 | if (!isInImage(coord)) continue; 60 | 61 | if (map[coord.y][coord.x].type != cell::OBSTACLE) { 62 | coords.push({x, y}); 63 | visited[y][x] = true; 64 | break; 65 | } 66 | } 67 | } 68 | } 69 | } 70 | 71 | void ObstacleTransformer::updateRowsAndCols(const CellMap& map) { 72 | rows = map.size(); 73 | cols = rows > 0 ? map[0].size() : 0; 74 | } -------------------------------------------------------------------------------- /src/path_analizer/PathAnalizer.cpp: -------------------------------------------------------------------------------- 1 | #include "PathAnalizer.hpp" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | PathAnalizer::PathAnalizer(double lengthWeightArg, double angleWeightArg) { 8 | if (lengthWeightArg < 0 || angleWeightArg < 0) { 9 | throw std::runtime_error("weights must be positive"); 10 | } 11 | 12 | if (lengthWeightArg + angleWeightArg - 1 > 0.1) { 13 | throw std::runtime_error("weights sum must be <= 1"); 14 | } 15 | 16 | lengthWeight = lengthWeightArg; 17 | angleWeight = angleWeightArg; 18 | } 19 | 20 | std::vector> 21 | PathAnalizer::findBestPath(const std::vector>>& paths) { 22 | if (paths.empty()) { 23 | std::cout << "there is no paths no analize" << std::endl; 24 | return {}; 25 | } 26 | std::cout << paths.size() << " paths will be analized" << std::endl; 27 | 28 | pathsStat.resize(paths.size()); 29 | 30 | for (int i = 0; i < paths.size(); ++i) { 31 | long long pathLength = computePathLength(paths[i]); 32 | float angle = computeAngle(paths[i]); 33 | pathsStat[i] = {i, pathLength, angle}; 34 | } 35 | 36 | auto cmp = [lengthWeight=lengthWeight, angleWeight=angleWeight] 37 | (const PathStat& lhs, const PathStat& rhs) { 38 | return (lhs.length - rhs.length) * lengthWeight + 39 | (lhs.angle - rhs.angle) * angleWeight < 0; 40 | }; 41 | 42 | std::sort(pathsStat.begin(), pathsStat.end(), cmp); 43 | 44 | return paths[pathsStat[0].i]; 45 | } 46 | 47 | float PathAnalizer::computeAngle(const std::vector>& path) const { 48 | if (path.size() < 3) { 49 | return 0; 50 | } 51 | 52 | float angle = 0; 53 | for (int i = 1; i < path.size() - 1; ++i) { 54 | if (path[i + 1].x - path[i].x == 0) { 55 | angle += M_PI / 2; 56 | } else { 57 | angle += std::abs(std::atan(float(path[i + 1].y - path[i].y) / 58 | float(path[i + 1].x - path[i].x))); 59 | } 60 | } 61 | 62 | return angle; 63 | } 64 | 65 | long long PathAnalizer::computePathLength(const std::vector>& path) const { 66 | double length = 0; 67 | for (int i = 0; i < path.size() - 1; ++i) { 68 | length += getDist(path[i], path[i + 1]); 69 | } 70 | return lround(length); 71 | } 72 | 73 | inline double PathAnalizer::getDist(const Coord& c0, const Coord& c1) const { 74 | return sqrt(pow(c0.x - c1.x, 2) + pow(c0.y - c1.y, 2)); 75 | } 76 | -------------------------------------------------------------------------------- /src/common/ImageMarker.cpp: -------------------------------------------------------------------------------- 1 | #include "ImageMarker.hpp" 2 | 3 | #include 4 | #include 5 | 6 | #include "ColourDefinitions.hpp" 7 | 8 | using CellMap = std::vector>; 9 | 10 | void ImageMarker::markCells(bitmap_image& image, const CellMap& cellMap, int cellSize) { 11 | const int penWidth = 1; 12 | assert(cellSize >= penWidth * 2); 13 | 14 | image_drawer drawer(image); 15 | drawer.pen_width(penWidth); 16 | 17 | const int rows = cellMap.size(); 18 | const int cols = rows > 0 ? cellMap[0].size() : 0; 19 | 20 | for (int y = 0, y1 = 0; y < rows; ++y, y1 += cellSize) { 21 | for (int x = 0, x1 = 0; x < cols; ++x, x1 += cellSize) { 22 | rgb_t penColour; 23 | if (cellMap[y][x].type == cell::FREE) { 24 | penColour = freeRectColour; 25 | } else if (cellMap[y][x].type == cell::OBSTACLE) { 26 | penColour = obstacleRectColour; 27 | } else if (cellMap[y][x].type == cell::PARTIALLY_FREE) { 28 | penColour = partiallyFreeRectColour; 29 | } else { 30 | throw std::runtime_error("unknown cell type"); 31 | } 32 | drawer.pen_color(penColour); 33 | 34 | int y2 = (y1 + cellSize >= image.height() ? image.height() - 1 : y1 + cellSize) - penWidth; 35 | int x2 = (x1 + cellSize >= image.width() ? image.width() - 1 : x1 + cellSize) - penWidth; 36 | drawer.rectangle(x1, y1, x2, y2); 37 | } 38 | } 39 | } 40 | 41 | void ImageMarker::markPath(bitmap_image& image, 42 | const std::vector>& pathCoords, 43 | int cellSize) { 44 | const int penWidth = 1; 45 | const int radius = 1; 46 | assert(radius >= penWidth); 47 | 48 | image_drawer circleDrawer(image); 49 | circleDrawer.pen_width(penWidth); 50 | circleDrawer.pen_color(pathMarkColour); 51 | 52 | image_drawer pathDrawer(image); 53 | pathDrawer.pen_width(penWidth); 54 | pathDrawer.pen_color(pathMarkColour); 55 | 56 | int xPrev = 0, yPrev = 0; 57 | for (int i = 0; i < pathCoords.size(); ++i) { 58 | int x = pathCoords[i].x * cellSize + cellSize / 2; 59 | if (x >= image.width()) { 60 | x = pathCoords[i].x * cellSize + (image.width() - 1 - pathCoords[i].x * cellSize) / 2; 61 | } 62 | 63 | int y = pathCoords[i].y * cellSize + cellSize / 2; 64 | if (y >= image.height()) { 65 | y = pathCoords[i].y * cellSize + (image.height() - 1 - pathCoords[i].y * cellSize) / 2; 66 | } 67 | 68 | circleDrawer.circle(x, y, radius); 69 | 70 | if (i > 0) { 71 | pathDrawer.line_segment(x, y, xPrev, yPrev); 72 | } 73 | xPrev = x, yPrev = y; 74 | } 75 | } 76 | -------------------------------------------------------------------------------- /samples/global_path_planner.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "bitmap_image.hpp" 5 | 6 | #include "CellMapCreator.hpp" 7 | #include "ColourDefinitions.hpp" 8 | #include "Coord.hpp" 9 | #include "CoordMapper.hpp" 10 | #include "GlobalPathPlanner.hpp" 11 | #include "ImageMarker.hpp" 12 | #include "PathTransformer.hpp" 13 | #include "Types.hpp" 14 | 15 | namespace configs { 16 | const std::string inputMapFileName = "./output/inflated.bmp"; 17 | const std::string outputMapFileName = "./output/routeMap.bmp"; 18 | const std::string outputGlobalPathCoordsFileName = "./output/globalPathCoords.txt"; 19 | const int cellSize = 50; 20 | const int occupiedThreshold = 80; // 0..100% 21 | const Coord startCoord = {1, 1}; 22 | const int otMulti = 1; 23 | const int dtMulti = 1; 24 | }; 25 | 26 | int main() 27 | { 28 | std::cout << "Read inflated map" << std::endl; 29 | bitmap_image image(configs::inputMapFileName); 30 | 31 | if (!image) { 32 | std::cerr << "Error - Failed to open: " 33 | << configs::inputMapFileName << std::endl; 34 | return 1; 35 | } 36 | 37 | std::cout << "Create cell map" << std::endl; 38 | CellMapCreator omc; 39 | omc.setOccupiedThreshold(configs::occupiedThreshold); 40 | omc.setFreeSpaceColour(freeSpaceColour); 41 | auto cellMap = omc.create(image, configs::cellSize); 42 | 43 | std::cout << "Apply Path transformer" << std::endl; 44 | PathTransformer pt(cellMap, configs::startCoord); 45 | pt.setObstacleTransformerMultiplier(configs::otMulti); 46 | pt.setDistanceTransformerMultiplier(configs::dtMulti); 47 | auto weightedMap = pt.transform(); 48 | 49 | std::cout << "Plan Global path" << std::endl; 50 | GlobalPathPlanner gpp; 51 | auto globalPath = gpp.planPath(weightedMap, configs::startCoord); 52 | if (globalPath.empty()) { 53 | std::cout << "No global path coordinates are generated" << std::endl; 54 | return 0; 55 | } 56 | 57 | std::cout << "Output global path coords" << std::endl; 58 | std::ofstream globalPathCoords(configs::outputGlobalPathCoordsFileName); 59 | auto globalPathMapped = 60 | CoordMapper::mapCellMapToImg(globalPath, configs::cellSize, 61 | image.width(), image.height()); 62 | for (auto coord : globalPathMapped) { 63 | globalPathCoords << coord.x << ' ' << coord.y << std::endl; 64 | } 65 | 66 | std::cout << "Mark cells" << std::endl; 67 | ImageMarker::markCells(image, cellMap, configs::cellSize); 68 | 69 | std::cout << "Mark path" << std::endl; 70 | ImageMarker::markPath(image, globalPath, configs::cellSize); 71 | 72 | std::cout << "Save image" << std::endl; 73 | image.save_image(configs::outputMapFileName); 74 | 75 | return 0; 76 | } 77 | -------------------------------------------------------------------------------- /samples/local_path_planner.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include "bitmap_image.hpp" 6 | 7 | #include "CellMapCreator.hpp" 8 | #include "ColourDefinitions.hpp" 9 | #include "Coord.hpp" 10 | #include "CoordMapper.hpp" 11 | #include "ImageMarker.hpp" 12 | #include "LocalPathPlanner.hpp" 13 | #include "Types.hpp" 14 | 15 | namespace configs { 16 | const std::string inputMapFileName = "./output/inflated.bmp"; 17 | const std::string inputGlobalPathCoordsFileName = "./output/globalPathCoords.txt"; 18 | const std::string outputMapFileName = "./output/pathMap.bmp"; 19 | const std::string outputLocalPathCoordsFileName = "./output/localPathCoords.txt"; 20 | const int cellSize = 5; 21 | const int occupiedThreshold = 80; // 0..100% 22 | const LocalPathPlanner::wavePropAlg lppWavePropAlg = LocalPathPlanner::wavePropAlg::TOWARD_TO_AIM; 23 | }; 24 | 25 | int main() 26 | { 27 | std::cout << "Read inflated map" << std::endl; 28 | bitmap_image image(configs::inputMapFileName); 29 | 30 | if (!image) { 31 | std::cerr << "Error - Failed to open: " 32 | << configs::inputMapFileName << std::endl; 33 | return 1; 34 | } 35 | 36 | std::cout << "Create cell map" << std::endl; 37 | CellMapCreator omc; 38 | omc.setOccupiedThreshold(configs::occupiedThreshold); 39 | omc.setFreeSpaceColour(freeSpaceColour); 40 | auto cellMap = omc.create(image, configs::cellSize); 41 | 42 | std::cout << "Read global path coords" << std::endl; 43 | std::ifstream globalPathCoords(configs::inputGlobalPathCoordsFileName); 44 | std::vector> globalPath; 45 | Coord buf; 46 | while (globalPathCoords >> buf.x >> buf.y) { 47 | globalPath.push_back(buf); 48 | } 49 | 50 | std::cout << "Convert global path coords into cell map coordinates" << std::endl; 51 | int cellMapRows = cellMap.size(); 52 | int cellMapCols = cellMapRows > 0 ? cellMap[0].size() : 0; 53 | auto globalPathCellCoords = 54 | CoordMapper::mapImgToCellMap(globalPath, configs::cellSize, 55 | cellMapCols, cellMapRows); 56 | 57 | std::cout << "Plan local path" << std::endl; 58 | LocalPathPlanner lpp(configs::lppWavePropAlg); 59 | auto localPath = lpp.planPath(cellMap, globalPathCellCoords); 60 | 61 | std::cout << "Output local path coords" << std::endl; 62 | std::ofstream localPathCoords(configs::outputLocalPathCoordsFileName); 63 | auto localPathMapped = 64 | CoordMapper::mapCellMapToImg(localPath, configs::cellSize, 65 | image.width(), image.height()); 66 | for (auto coord : localPathMapped) { 67 | localPathCoords << coord.x << ' ' << coord.y << std::endl; 68 | } 69 | 70 | std::cout << "Mark cells" << std::endl; 71 | ImageMarker::markCells(image, cellMap, configs::cellSize); 72 | 73 | std::cout << "Mark path" << std::endl; 74 | ImageMarker::markPath(image, localPath, configs::cellSize); 75 | 76 | std::cout << "Save image" << std::endl; 77 | image.save_image(configs::outputMapFileName); 78 | 79 | return 0; 80 | } 81 | -------------------------------------------------------------------------------- /readme.txt: -------------------------------------------------------------------------------- 1 | Additional libs (http://www.partow.net/programming/bitmap/index.html): 2 | C++ Bitmap Library (bitmap_image.hpp). Lib path in project: include/common/bitmap_image.hpp 3 | 4 | This work based on article by A. Zelinsky1, R.A. Jarvis2, J.C. Byrne2 and 5 | S. Yuta3 "Planning Paths of Complete Coverage of an Unstructured Environment 6 | by a Mobile Robot" 7 | 8 | The program is a draft. Coverage path planning implementation use cell decomposition 9 | of image, Obstacle transformation, Distance transformation, wave algorithm, cost function. 10 | The approach consider the initial map as a static (not changing environment). 11 | 12 | Process steps: 13 | 1 Inflate obstacles on the initial map. 14 | 2 Create cell map; Apply Path transformer; Plan Global path. 15 | 3 Create cell map; Plan Local path. 16 | (4 Mark cells, Mark path, Save image.) 17 | 18 | Inflator. 19 | Puppose: forming safe distance around obstacle. 20 | Brute force algorithm for inflation obstacle borders is slow. A new approuch was 21 | used to speed up the obstacle inflation, but it has some flaws (shapes are not purfect). 22 | Inflation process is performed by subinflation steps. If an end radius of inflation 23 | is set, then in each step obstacle inflated to the part (ri + dr) of the whole radius(r). 24 | Each step dr is is changind (ddr). At each step a goal radius ri + dr is calculeted. 25 | r0 = 0; 26 | r1 = r0 + dr; dr = dr + ddr; 27 | r2 = r1 + dr; dr = dr + ddr; ... 28 | To fill the area ri + dr around obstacle wave algorithm is used. The starting 29 | coordinates of the wave include: 30 | obstacle border; 31 | coordinates of a line segment that starts at the obstacle border and with the 32 | length of ri + dr. The direction of the line segment is defined by shift8 matrix 33 | (see in code). 34 | 35 | Global path planner. 36 | Purpose: planning visiting cells order. 37 | After reading inflated map from input file, the map converting into cell map. Each cell 38 | represent the occupied/non-occupied status. The Distance and Obstacle transformations are 39 | appying next to cell map creating two weighted maps. The weights depends on the 40 | transformation types (see in article). Then using two weighted maps and cost function 41 | (see in article) with scaler a new weight map is creating. Global planner use the weighted map to 42 | plan map coverage path. At each step global planner choose the cell with least weight. 43 | If there is no free cells nearby, then the previous positions are checked. 44 | 45 | Local path planner. 46 | Purpose: finding path between global path coordinates. 47 | Based on wave propogation algorithms (toward to aim, around), weight map. With given 48 | cell map and sequence of coordinates the Local path planner tries to connect coordinates 49 | (cells) in given order. The connection is supposed to form a the shortest path between 50 | two coordinates (cells). Sequentially all coordinates connected forming path. 51 | 52 | Configs: 53 | general: 54 | Initial\output map file path; 55 | Output coordinates file path; 56 | Start coordinate on image; 57 | 58 | inflator: 59 | inflation radius - radius of safe distance around obstacle; 60 | 61 | global path planner: 62 | cell size - size of the cell global planner plan to visit (influences to 63 | distance between paths); 64 | occupied threshold - cell evaluated as an obstacel 65 | if numbers of occupied pixel more that this value; (0..100%) 66 | obstacle transformer multiplier - weight multiplier for obstacle transformer map; 67 | distance transformer multiplier - weight multiplier for distance transformer map; 68 | const double scaler = dtMaxWeight == 0 ? 1 : (double)otMaxWeight / dtMaxWeight; 69 | map[y][x].weight = (int)round(scaler * (dtMulti * dtMap[y][x].weight)) + 70 | otMulti * otMaxWeight / otMap[y][x].weight; 71 | 72 | local path planner: 73 | cell size - size of the cell to find path between global coordinates; 74 | occupied threshold (0..100%); 75 | wave propogation algorithm: 76 | toward to aim - wave tend to propogate toward to aim; 77 | around - wave tend to propogate around (not formig round) initial position; 78 | 79 | Configure samples: 80 | All settings under namespace "configs". 81 | After changing settings sample need to be rebuild. 82 | 83 | Building samples: 84 | cd coverage-path-planning 85 | mkdir build 86 | cd build 87 | cmake .. 88 | 89 | build all: 90 | make 91 | 92 | build specific: 93 | make coverage_path_planning 94 | make inflator 95 | make global_path_planner 96 | make local_path_planner 97 | 98 | Run samples: 99 | ./coverage_path_planning 100 | ./inflator 101 | ./global_path_planner 102 | ./local_path_planner 103 | 104 | Default output results folder is "output". -------------------------------------------------------------------------------- /src/inflator/Inflator.cpp: -------------------------------------------------------------------------------- 1 | #include "Inflator.hpp" 2 | 3 | #include 4 | #include 5 | 6 | #include "Shifts.hpp" 7 | #include "ColourDefinitions.hpp" 8 | 9 | Inflator::Inflator(const bitmap_image& img) { 10 | image.setwidth_height(img.width() + frameWidth * 2, img.height() + frameWidth * 2); 11 | if (!image.copy_from(img, frameWidth, frameWidth)) { 12 | throw std::runtime_error("inflator init error: copy from failed"); 13 | } 14 | 15 | applyBinFilter(); 16 | } 17 | 18 | void Inflator::inflate(int radius) { 19 | const int maxSteps = 10; 20 | const int ddrMin = 3; 21 | 22 | int ddr = 2 * radius / (maxSteps * maxSteps); 23 | ddr = ddr < ddrMin ? ddrMin : ddr; 24 | 25 | int dr = 3; 26 | int r = 0; 27 | 28 | std::vector> bases = getObstacleBorder(); 29 | std::vector> newBases; 30 | 31 | std::cout << "inflate from: " << r << " to: " << radius << std::endl; 32 | std::cout << "------------------------------" << std::endl; 33 | 34 | while (r + dr <= radius) { 35 | std::cout << "inflating from: " << r << " to: " << r + dr << std::endl; 36 | inflateFromBase(bases, dr, newBases); 37 | filterInflatedCoords(newBases, bases); 38 | r += dr; 39 | dr += ddr; 40 | } 41 | 42 | if (r + dr > radius) { 43 | std::cout << "inflating from: " << r << " to: " << radius << std::endl; 44 | inflateFromBase(bases, radius - r, newBases); 45 | } 46 | } 47 | 48 | bitmap_image Inflator::getImage() const { 49 | bitmap_image imageToReturn; 50 | image.region(frameWidth, frameWidth, 51 | image.width() - frameWidth * 2, 52 | image.height() - frameWidth * 2, imageToReturn); 53 | return imageToReturn; 54 | } 55 | 56 | void Inflator::applyBinFilter() { 57 | // not freeSpaceColour pixel colour -> obstColour 58 | std::cout << "applying binary filter" << std::endl; 59 | for (int y = frameWidth; y < image.height() - frameWidth; ++y) { 60 | for (int x = frameWidth; x < image.width() - frameWidth; ++x) { 61 | rgb_t colour; 62 | image.get_pixel(x, y, colour); 63 | if (colour != freeSpaceColour) { 64 | image.set_pixel(x, y, obstColour); 65 | } 66 | } 67 | } 68 | } 69 | 70 | bool Inflator::isInImage(int x, int y) const { 71 | return x >= frameWidth && x < image.width() - frameWidth && 72 | y >= frameWidth && y < image.height() - frameWidth; 73 | } 74 | 75 | std::vector> Inflator::getObstacleBorder() const { 76 | std::cout << "getting obstacle border coordinates" << std::endl; 77 | 78 | std::vector> border; 79 | for (int y = frameWidth; y < image.height() - frameWidth; ++y) { 80 | for (int x = frameWidth; x < image.width() - frameWidth; ++x) { 81 | rgb_t colour; 82 | image.get_pixel(x, y, colour); 83 | if (colour == obstColour) { 84 | for (auto sh : shift4) { 85 | rgb_t colour; 86 | Coord coord = Coord{x, y} + sh; 87 | image.get_pixel(coord.x, coord.y, colour); 88 | 89 | if (colour == freeSpaceColour) { 90 | border.push_back({x, y}); 91 | break; 92 | } 93 | } 94 | } 95 | } 96 | } 97 | 98 | return border; 99 | } 100 | 101 | void Inflator::filterInflatedCoords(std::vector>& src, 102 | std::vector>& dst) const { 103 | dst.clear(); 104 | 105 | for (auto coord : src) { 106 | rgb_t colour; 107 | image.get_pixel(coord.x, coord.y, colour); 108 | if (colour == infColour) { 109 | for (auto sh : shift4) { 110 | rgb_t colour; 111 | Coord newCoord = coord + sh; 112 | image.get_pixel(newCoord.x, newCoord.y, colour); 113 | 114 | if (colour == freeSpaceColour) { 115 | dst.push_back(coord); 116 | break; 117 | } 118 | } 119 | } 120 | } 121 | } 122 | 123 | void Inflator::addLineCoords(int x0, int y0, int dx, int dy, int l, int group, 124 | std::queue& wave) const { 125 | int x = x0 + dx, y = y0 + dy, dl = sqrt(pow(x - x0, 2) + pow(y - y0, 2)); 126 | while (dl < l) { 127 | if (!isInImage(x, y)) break; 128 | 129 | rgb_t colour; 130 | image.get_pixel(x, y, colour); 131 | if (colour == freeSpaceColour) { 132 | wave.push({{x, y}, group}); 133 | } 134 | 135 | x += dx, y += dy; 136 | dl = sqrt(pow(x - x0, 2) + pow(y - y0, 2)); 137 | } 138 | } 139 | 140 | void Inflator::addSeedsCoords(const std::vector>& bases, int radius, 141 | std::queue& wave) const { 142 | for (int i = 0; i < bases.size(); ++i) { 143 | Coord base = bases[i]; 144 | wave.push({base, i}); 145 | for (auto sh : shift8) { 146 | rgb_t colour; 147 | Coord coord = base + sh; 148 | image.get_pixel(coord.x, coord.y, colour); 149 | if (colour == infColour || colour == obstColour) { 150 | addLineCoords(base.x, base.y, -sh.x, -sh.y, radius, i, wave); 151 | } 152 | } 153 | } 154 | } 155 | 156 | void Inflator::inflateFromBase(const std::vector>& bases, int radius, 157 | std::vector>& newBases) { 158 | std::queue wave; 159 | addSeedsCoords(bases, radius, wave); 160 | 161 | newBases.clear(); 162 | 163 | while (!wave.empty()) { 164 | size_t size = wave.size(); 165 | while (size--) { 166 | auto baseWave = wave.front(); 167 | wave.pop(); 168 | for (auto sh : shift8) { 169 | rgb_t colour; 170 | Coord newCoord = baseWave.coord + sh; 171 | Coord origin = bases[baseWave.i]; 172 | 173 | int dist = round(sqrt((pow(newCoord.x - (double)origin.x, 2) + 174 | pow(newCoord.y - (double)origin.y, 2)))); 175 | 176 | if (dist > radius) continue; 177 | 178 | image.get_pixel(newCoord.x, newCoord.y, colour); 179 | 180 | if (colour == freeSpaceColour) { 181 | wave.push({newCoord, baseWave.i}); 182 | image.set_pixel(newCoord.x, newCoord.y, infColour); 183 | newBases.push_back({newCoord}); 184 | } 185 | } 186 | } 187 | } 188 | } -------------------------------------------------------------------------------- /samples/coverage_path_planner.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include "bitmap_image.hpp" 8 | 9 | #include "CellMapCreator.hpp" 10 | #include "ColourDefinitions.hpp" 11 | #include "Coord.hpp" 12 | #include "CoordMapper.hpp" 13 | #include "GlobalPathPlanner.hpp" 14 | #include "ImageMarker.hpp" 15 | #include "Inflator.hpp" 16 | #include "LocalPathPlanner.hpp" 17 | #include "PathAnalizer.hpp" 18 | #include "PathTransformer.hpp" 19 | #include "Types.hpp" 20 | 21 | namespace configs { 22 | const std::string inputMapFileName = "./input/map.bmp"; 23 | const std::string inputInflatedMapFileName = "./output/inflated.bmp"; 24 | const std::string outputMapFileName = "./output/coveragePathMap.bmp"; 25 | const std::string outputPathCoordsFileName = "./output/coveragePathCoords.txt"; 26 | 27 | const Coord startCoordOnImage = {35, 35}; 28 | 29 | // Inflator settings. 30 | const int inflationRadius = 25; 31 | 32 | // Global planner settings. 33 | const int globalPathCellSize = 50; 34 | const int globalPathOccupiedThreshold = 80; // 0..100% 35 | 36 | // Costfunction weights range settings. 37 | const int otMultiMin = 0, otMultiMax = 40; 38 | const int dtMultiMin = 0, dtMultiMax = 40; 39 | const int otDelta = 3, dtDelta = 3; 40 | 41 | // Path analizer settings. Sum of weights must be <= 1 42 | const double lengthWeight = 0.7; 43 | const double angleWeight = 0.3; 44 | 45 | // Local planner settings. 46 | const int localPathCellSize = 5; 47 | const int localPathOccupiedThreshold = 80; // 0..100% 48 | const LocalPathPlanner::wavePropAlg lppWavePropAlg = LocalPathPlanner::wavePropAlg::TOWARD_TO_AIM; 49 | }; 50 | 51 | bitmap_image getInflatedImage() { 52 | bitmap_image image(configs::inputMapFileName); 53 | if (!image) { 54 | throw std::runtime_error("Error - Failed to open: " + configs::inputMapFileName); 55 | } 56 | 57 | Inflator inf(image); 58 | inf.inflate(configs::inflationRadius); 59 | 60 | bitmap_image inflated = inf.getImage(); 61 | 62 | std::cout << "saving image" << std::endl; 63 | inflated.save_image(configs::inputInflatedMapFileName); 64 | 65 | return inflated; 66 | } 67 | 68 | std::vector> planGlobalPath(const bitmap_image& image) { 69 | const Coord startCoord = 70 | CoordMapper::map(configs::startCoordOnImage, 1, configs::globalPathCellSize); 71 | 72 | std::cout << "Create cell map" << std::endl; 73 | CellMapCreator omc; 74 | omc.setOccupiedThreshold(configs::globalPathOccupiedThreshold); 75 | omc.setFreeSpaceColour(freeSpaceColour); 76 | auto cellMap = omc.create(image, configs::globalPathCellSize); 77 | 78 | std::cout << "Generate paths" << std::endl; 79 | std::vector>> paths; 80 | PathTransformer pt(cellMap, startCoord); 81 | for (int dtMulti = configs::dtMultiMin; 82 | dtMulti <= configs::dtMultiMax; dtMulti += configs::dtDelta) { 83 | for (int otMulti = configs::otMultiMin; 84 | otMulti <= configs::otMultiMax; otMulti += configs::otDelta) { 85 | pt.setDistanceTransformerMultiplier(dtMulti); 86 | pt.setObstacleTransformerMultiplier(otMulti); 87 | auto weightedMap = pt.transform(); 88 | 89 | auto globalPathCellCoords = GlobalPathPlanner().planPath(weightedMap, startCoord); 90 | paths.push_back(globalPathCellCoords); 91 | } 92 | } 93 | 94 | std::cout << "Find the best path" << std::endl; 95 | PathAnalizer pa(configs::lengthWeight, configs::angleWeight); 96 | auto bestPath = pa.findBestPath(paths); 97 | 98 | std::cout << "Map coordinates into image coordinates" << std::endl; 99 | auto globalPath = 100 | CoordMapper::mapCellMapToImg(bestPath, configs::globalPathCellSize, 101 | image.width(), image.height()); 102 | return globalPath; 103 | } 104 | 105 | std::vector> planLocalPath(const bitmap_image& image, 106 | const std::vector>& globalPath) { 107 | 108 | std::cout << "Create cell map" << std::endl; 109 | CellMapCreator omc; 110 | omc.setOccupiedThreshold(configs::localPathOccupiedThreshold); 111 | omc.setFreeSpaceColour(freeSpaceColour); 112 | auto cellMap = omc.create(image, configs::localPathCellSize); 113 | 114 | std::cout << "Convert global path coords into cell map coordinates" << std::endl; 115 | int cellMapRows = cellMap.size(); 116 | int cellMapCols = cellMapRows > 0 ? cellMap[0].size() : 0; 117 | auto globalPathCellCoords = 118 | CoordMapper::mapImgToCellMap(globalPath, configs::localPathCellSize, 119 | cellMapCols, cellMapRows); 120 | 121 | std::cout << "Plan local path" << std::endl; 122 | LocalPathPlanner lpp(configs::lppWavePropAlg); 123 | auto localPathCellCoords = lpp.planPath(cellMap, globalPathCellCoords); 124 | 125 | auto localPath = 126 | CoordMapper::mapCellMapToImg(localPathCellCoords, configs::localPathCellSize, 127 | image.width(), image.height()); 128 | 129 | return localPath; 130 | } 131 | 132 | bool isFileExists(const std::string& fileName) { 133 | std::ifstream f(fileName); 134 | return f.good(); 135 | } 136 | 137 | int main() 138 | { 139 | std::cout << "---------------------------------" << std::endl; 140 | std::cout << "Create infalted image" << std::endl; 141 | 142 | bitmap_image inflatedImage; 143 | if (isFileExists(configs::inputInflatedMapFileName)) { 144 | std::cout << "Infalted image already created" << std::endl; 145 | inflatedImage = bitmap_image(configs::inputInflatedMapFileName); 146 | if (!inflatedImage) { 147 | std::cerr << "Error - Failed to open: " 148 | << configs::inputInflatedMapFileName << std::endl; 149 | return 1; 150 | } 151 | } else { 152 | inflatedImage = getInflatedImage(); 153 | } 154 | 155 | std::cout << "successfull" << std::endl; 156 | std::cout << "---------------------------------" << std::endl; 157 | 158 | std::cout << "---------------------------------" << std::endl; 159 | std::cout << "Plan Global path" << std::endl; 160 | auto globalPath = planGlobalPath(inflatedImage); 161 | if (globalPath.empty()) { 162 | std::cout << "No global path coordinates are generated" << std::endl; 163 | return 0; 164 | } 165 | std::cout << "successfull" << std::endl; 166 | std::cout << "---------------------------------" << std::endl; 167 | 168 | std::cout << "---------------------------------" << std::endl; 169 | std::cout << "Plan Local path" << std::endl; 170 | auto localPath = planLocalPath(inflatedImage, globalPath); 171 | std::cout << "successfull" << std::endl; 172 | std::cout << "---------------------------------" << std::endl; 173 | 174 | ImageMarker::markPath(inflatedImage, localPath, 1); 175 | 176 | inflatedImage.save_image(configs::outputMapFileName); 177 | 178 | std::ofstream pathCoords(configs::outputPathCoordsFileName); 179 | for (auto coord : localPath) { 180 | pathCoords << coord.x << ' ' << coord.y << std::endl; 181 | } 182 | 183 | return 0; 184 | } 185 | -------------------------------------------------------------------------------- /src/local_path_planner/LocalPathPlanner.cpp: -------------------------------------------------------------------------------- 1 | #include "LocalPathPlanner.hpp" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include "Shifts.hpp" 9 | 10 | using CellMap = std::vector>; 11 | 12 | LocalPathPlanner::LocalPathPlanner(wavePropAlg algArg) : alg(algArg) { 13 | using namespace std::placeholders; 14 | if (alg == wavePropAlg::AROUND) { 15 | propagateWave = std::bind(&LocalPathPlanner::propagateWaveAround, this, _1, _2, _3); 16 | } else if (alg == wavePropAlg::TOWARD_TO_AIM) { 17 | propagateWave = std::bind(&LocalPathPlanner::propagateWaveTowardToAim, this, _1, _2, _3); 18 | } else { 19 | throw std::runtime_error("LocalPathPlanner::wavePropAlg type is undefined"); 20 | } 21 | } 22 | 23 | std::vector> 24 | LocalPathPlanner::planPath(const CellMap& cellMap, 25 | const std::vector>& coords) { 26 | if (coords.empty()) { 27 | throw std::runtime_error("no coordinates for path planning"); 28 | } 29 | 30 | rows = cellMap.size(); 31 | cols = rows > 0 ? cellMap[0].size() : 0; 32 | 33 | if (rows == 0 && cols == 0) { 34 | throw std::runtime_error("zero size cell map"); 35 | } 36 | 37 | if (!isInImage(coords[0])) { 38 | throw std::runtime_error("start coordinate is out of range"); 39 | } 40 | 41 | map = std::vector>(rows, std::vector(cols)); 42 | for (int i = 0; i < rows; ++i) { 43 | for (int j = 0; j < cols; ++j) { 44 | map[i][j] = {cellMap[i][j], initOperationId}; 45 | } 46 | } 47 | 48 | Coord start = coords[0]; 49 | if (cellMap[start.y][start.x].type == cell::OBSTACLE && 50 | !findNearNotObstCell(start)) { 51 | throw std::runtime_error("start coordinate is invalid"); 52 | } 53 | 54 | std::vector> path{start}; 55 | for (int i = 1; i < coords.size(); ++i) { 56 | start = path.back(); 57 | Coord end = coords[i]; 58 | 59 | if (!isInImage(end)) { 60 | throw std::runtime_error("coordinate is out of range"); 61 | } 62 | 63 | if (map[end.y][end.x].cell.type == cell::OBSTACLE && 64 | !findNearNotObstCell(end)) { 65 | continue; 66 | } 67 | 68 | addToPath(start, end, path, i); 69 | } 70 | 71 | return path; 72 | } 73 | 74 | bool LocalPathPlanner::isInImage(const Coord& coord) const { 75 | return coord.y >= 0 && coord.y < rows && 76 | coord.x >= 0 && coord.x < cols; 77 | } 78 | 79 | bool LocalPathPlanner::findNearNotObstCell(Coord& base) { 80 | std::queue> q; 81 | q.push(base); 82 | 83 | while (!q.empty()) { 84 | int size = q.size(); 85 | while (size--) { 86 | auto front = q.front(); 87 | q.pop(); 88 | 89 | for (auto sh : shift4) { 90 | Coord coord = front + sh; 91 | 92 | if (!isInImage(coord)) continue; 93 | if (map[coord.y][coord.x].operationId == findNearNotObstCellOperationId) continue; 94 | 95 | map[coord.y][coord.x].operationId = findNearNotObstCellOperationId; 96 | 97 | int dist = round(sqrt((pow(coord.x - (double)base.x, 2) + 98 | pow(coord.y - (double)base.y, 2)))); 99 | 100 | if (dist > searchAreaRadius) continue; 101 | 102 | if (map[coord.y][coord.x].cell.type != cell::OBSTACLE) { 103 | base = coord; 104 | return true; 105 | } 106 | 107 | q.push(coord); 108 | } 109 | } 110 | } 111 | 112 | return false; 113 | } 114 | 115 | bool LocalPathPlanner::propagateWaveAround(const Coord& start, 116 | const Coord& end, 117 | int operationId) { 118 | std::queue> q; 119 | q.push(end); 120 | 121 | map[end.y][end.x].cell.weight = 0; 122 | map[end.y][end.x].operationId = operationId; 123 | 124 | while (!q.empty()) { 125 | int size = q.size(); 126 | while (size--) { 127 | auto base = q.front(); 128 | q.pop(); 129 | if (base == start) { 130 | return true; 131 | } 132 | 133 | for (auto sh : shift4) { 134 | Coord coord = base + sh; 135 | 136 | if (!isInImage(coord)) continue; 137 | if (map[coord.y][coord.x].cell.type == cell::OBSTACLE) continue; 138 | if (map[coord.y][coord.x].operationId == operationId) continue; 139 | 140 | map[coord.y][coord.x].operationId = operationId; 141 | map[coord.y][coord.x].cell.weight = 142 | map[base.y][base.x].cell.weight + 1; 143 | 144 | q.push(coord); 145 | } 146 | } 147 | } 148 | 149 | return false; 150 | } 151 | 152 | bool LocalPathPlanner::propagateWaveTowardToAim(const Coord& start, 153 | const Coord& end, 154 | int operationId) { 155 | auto cmpDistToStart = [&start] (const Coord& lhs, const Coord& rhs) { 156 | int lhsDist = round(sqrt((pow(start.x - (double)lhs.x, 2) + 157 | pow(start.y - (double)lhs.y, 2)))); 158 | int rhsDist = round(sqrt((pow(start.x - (double)rhs.x, 2) + 159 | pow(start.y - (double)rhs.y, 2)))); 160 | return lhsDist > rhsDist; 161 | }; 162 | 163 | std::priority_queue, std::vector>, decltype(cmpDistToStart)> pq(cmpDistToStart); 164 | pq.push(end); 165 | 166 | map[end.y][end.x].cell.weight = 0; 167 | map[end.y][end.x].operationId = operationId; 168 | 169 | while (!pq.empty()) { 170 | auto base = pq.top(); 171 | pq.pop(); 172 | if (base == start) { 173 | return true; 174 | } 175 | 176 | for (auto sh : shift4) { 177 | Coord coord = base + sh; 178 | 179 | if (!isInImage(coord)) continue; 180 | if (map[coord.y][coord.x].cell.type == cell::OBSTACLE) continue; 181 | if (map[coord.y][coord.x].operationId == operationId) continue; 182 | 183 | map[coord.y][coord.x].operationId = operationId; 184 | map[coord.y][coord.x].cell.weight = 185 | map[base.y][base.x].cell.weight + 1; 186 | 187 | pq.push(coord); 188 | } 189 | } 190 | 191 | return false; 192 | } 193 | 194 | void LocalPathPlanner::addToPath(const Coord& start, const Coord& end, 195 | std::vector>& path, int operationId) { 196 | if (!propagateWave(start, end, operationId)) { 197 | std::cout << "wave doesn't reach the aim" << std::endl; 198 | return; 199 | } 200 | 201 | Coord curr = start; 202 | while (curr != end) { 203 | path.push_back(curr); 204 | 205 | Coord minWeightCoord = curr; 206 | for (auto sh : shift8) { 207 | Coord coord = curr + sh; 208 | 209 | if (!isInImage(coord)) continue; 210 | if (map[coord.y][coord.x].cell.type == cell::OBSTACLE) continue; 211 | if (map[coord.y][coord.x].operationId != operationId) continue; 212 | 213 | int minWeight = map[minWeightCoord.y][minWeightCoord.x].cell.weight; 214 | int weight = map[coord.y][coord.x].cell.weight; 215 | if (minWeight > weight) { 216 | minWeightCoord = coord; 217 | } 218 | } 219 | 220 | if (curr == minWeightCoord) { 221 | std::cout << "path not found" << std::endl; 222 | return; 223 | } 224 | curr = minWeightCoord; 225 | } 226 | path.push_back(curr); 227 | } --------------------------------------------------------------------------------