Fawkes API Fawkes Development Version
grid.cpp
1
2/***************************************************************************
3 * grid.cpp - generate navgraph based on grid
4 *
5 * Created: Sun Apr 23 18:46:12 2017
6 * Copyright 2015-2017 Tim Niemueller [www.niemueller.de]
7 ****************************************************************************/
8
9/* This program is free software; you can redistribute it and/or modify
10 * it under the terms of the GNU General Public License as published by
11 * the Free Software Foundation; either version 2 of the License, or
12 * (at your option) any later version. A runtime exception applies to
13 * this software (see LICENSE.GPL_WRE file mentioned below for details).
14 *
15 * This program is distributed in the hope that it will be useful,
16 * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 * GNU Library General Public License for more details.
19 *
20 * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
21 */
22
23#include <CGAL/Arr_segment_traits_2.h>
24#include <CGAL/Cartesian.h>
25#include <CGAL/MP_Float.h>
26#include <CGAL/Quotient.h>
27#include <CGAL/Simple_cartesian.h>
28#include <CGAL/Sweep_line_2_algorithms.h>
29#include <CGAL/algorithm.h>
30#include <CGAL/point_generators_2.h>
31#include <core/exception.h>
32#include <core/threading/mutex_locker.h>
33#include <navgraph/generators/grid.h>
34
35typedef CGAL::Quotient<CGAL::MP_Float> NT;
36typedef CGAL::Cartesian<NT> Kernel;
37
38typedef Kernel::Point_2 Point;
39typedef Kernel::Segment_2 Segment;
40typedef Kernel::Vector_2 Vector_2;
41typedef CGAL::Points_on_segment_2<Point> PG;
42typedef CGAL::Creator_uniform_2<Point, Segment> Creator;
43typedef CGAL::Join_input_iterator_2<PG, PG, Creator> Segm_iterator;
44typedef CGAL::Counting_iterator<Segm_iterator, Segment> Count_iterator;
45typedef std::vector<Segment> Vector;
46
47typedef CGAL::Arr_segment_traits_2<Kernel> Traits_2;
48typedef Traits_2::Curve_2 Curve_2;
49
50namespace fawkes {
51
52/** @class NavGraphGeneratorGrid <navgraph/generators/grid.h>
53 * Generate navgraph using a Grid diagram.
54 * @author Tim Niemueller
55 */
56
57/** Constructor.
58 * @param params algorithm parameters. Valid parameters are:
59 * - spacing (float): inter-cell midpoint spacing (required).
60 */
61NavGraphGeneratorGrid::NavGraphGeneratorGrid(const std::map<std::string, std::string> &params)
62{
63 if (params.find("spacing") == params.end()) {
64 throw Exception("Grid algorithm requires 'spacing' parameter");
65 }
66 if (params.find("margin") == params.end()) {
67 throw Exception("Grid algorithm requires 'margin' parameter");
68 }
69 add_diagonals_ = false;
70 if (params.find("add-diagonals") != params.end()) {
71 add_diagonals_ = (params.at("add-diagonals") == "true");
72 }
73 try {
74 std::size_t pos;
75 spacing_ = std::stof(params.at("spacing"), &pos);
76 if (pos < params.at("spacing").size()) {
77 throw Exception("Cannot convert spacing '%s' to float", params.at("spacing").c_str());
78 }
79 } catch (std::invalid_argument &e) {
80 throw Exception("%s", e.what());
81 } catch (std::out_of_range &e) {
82 throw Exception("%s", e.what());
83 }
84
85 try {
86 std::size_t pos;
87 margin_ = std::stof(params.at("margin"), &pos);
88 if (pos < params.at("margin").size()) {
89 throw Exception("Cannot convert margin '%s' to float", params.at("margin").c_str());
90 }
91 } catch (std::invalid_argument &e) {
92 throw Exception("%s", e.what());
93 } catch (std::out_of_range &e) {
94 throw Exception("%s", e.what());
95 }
96}
97
98/** Destructor. */
100{
101}
102
103void
105{
106 if (!bbox_enabled_) {
107 throw Exception("Grid algorithm requires bounding box");
108 }
109
110 Point bb_p1(bbox_p1_x_, bbox_p1_y_);
111 Point bb_p2(bbox_p2_x_, bbox_p2_y_);
112
113 if (CGAL::compare_xy<Kernel>(bb_p1, bb_p2) != CGAL::SMALLER) {
114 throw Exception("Bounding box P1 must be smaller than P2 (x1 < x2 or x1 == x2 and y1 < y2)");
115 }
116
117 MutexLocker lock(graph.objmutex_ptr());
118
119 // Calculate number of points in X and Y direction
120 // based on bounding box and grid spacing
121 const Vector_2 diff(bb_p2 - bb_p1);
122 if (diff.x() < spacing_ || diff.y() < spacing_) {
123 throw Exception("Bounding box too small for given spacing");
124 }
125 size_t nx = CGAL::to_double(diff.x()) / spacing_;
126 size_t ny = CGAL::to_double(diff.y()) / spacing_;
127
128 // Spacing half, will be used to make sure grid intersection points
129 // have at least half spacing margin from bounding box.
130 const float spacing_2 = spacing_ / 2;
131
132 // Generate HORIZONTAL lines of grid (along x-axis)
133 PG p1(Point(bb_p1.x(), bb_p1.y() + spacing_2), Point(bb_p1.x(), bb_p2.y() - spacing_2), ny);
134 PG p2(Point(bb_p2.x(), bb_p1.y() + spacing_2), Point(bb_p2.x(), bb_p2.y() - spacing_2), ny);
135 Segm_iterator t1(p1, p2); // Segment generator.
136 Count_iterator t1_begin(t1); // Finite range.
137 Count_iterator t1_end(t1, ny);
138
139 // std::for_each(t1_begin, t1_end,
140 // [](const Segment &s) {
141 // printf("Segment H (%f,%f) -> (%f,%f)\n",
142 // CGAL::to_double(s.source().x()), CGAL::to_double(s.source().y()),
143 // CGAL::to_double(s.target().x()), CGAL::to_double(s.target().y()));
144 // });
145
146 // Generate VERTICAL lines of grid (along y-axis)
147 PG p3(Point(bb_p1.x() + spacing_2, bb_p1.y()), Point(bb_p2.x() - spacing_2, bb_p1.y()), nx);
148 PG p4(Point(bb_p1.x() + spacing_2, bb_p2.y()), Point(bb_p2.x() - spacing_2, bb_p2.y()), nx);
149 Segm_iterator t2(p3, p4); // Segment generator.
150 Count_iterator t2_begin(t2); // Finite range.
151 Count_iterator t2_end(t2, nx);
152
153 // std::for_each(t2_begin, t2_end,
154 // [](const Segment &s) {
155 // printf("Segment V (%f,%f) -> (%f,%f)\n",
156 // CGAL::to_double(s.source().x()), CGAL::to_double(s.source().y()),
157 // CGAL::to_double(s.target().x()), CGAL::to_double(s.target().y()));
158 // });
159
160 // Transform segments to curves for intersection calculation
161 std::vector<Curve_2> segs;
162 segs.reserve(nx + ny);
163 std::transform(t1_begin, t1_end, std::back_inserter(segs), [](const Segment &s) -> Curve_2 {
164 return Curve_2(s.source(), s.target());
165 });
166 std::transform(t2_begin, t2_end, std::back_inserter(segs), [](const Segment &s) -> Curve_2 {
167 return Curve_2(s.source(), s.target());
168 });
169
170 // Compute intersection points on grid, these will be the navgraph nodes
171 std::vector<Point> ipts;
172 ipts.reserve(nx * ny);
173 CGAL::compute_intersection_points(segs.begin(), segs.end(), std::back_inserter(ipts));
174
175 // Sort by Y, then by X (relevant for edge generation later to use
176 // simple index operations to find predecessor row to connect to)
177 std::sort(ipts.begin(), ipts.end(), [](const Point &p1, const Point &p2) {
178 return CGAL::compare_yx<Kernel>(p1, p2) == CGAL::SMALLER;
179 });
180
181 // printf("Num intersections: %zu\n", ipts.size());
182 // std::for_each(ipts.begin(), ipts.end(),
183 // [](const Point &p) {
184 // printf("Intersection (%f,%f)\n",
185 // CGAL::to_double(p.x()), CGAL::to_double(p.y()));
186 // });
187
188 std::map<std::string, std::string> props_gen = {{"generated", "true"}};
189
190 // Add nodes to graph
191 std::vector<std::pair<std::string, Point>> points;
192 points.reserve(ipts.size());
193 for (size_t i = 0; i < ipts.size(); ++i) {
194 std::string name = "G-" + std::to_string((i % nx) + 1) + "-" + std::to_string((i / nx) + 1);
195 points.push_back(std::make_pair(name, ipts[i]));
196 graph->add_node(
197 NavGraphNode(name, CGAL::to_double(ipts[i].x()), CGAL::to_double(ipts[i].y()), props_gen));
198 }
199
200 // std::for_each(points.begin(), points.end(),
201 // [](const auto &p) {
202 // printf("Point '%s' (%f,%f)\n", p.first.c_str(),
203 // CGAL::to_double(p.second.x()), CGAL::to_double(p.second.y()));
204 // });
205
206 // Add edges to graph
207 for (size_t i = 0; i < points.size(); ++i) {
208 if (i % nx > 0) {
209 // connect to previous node unless it's the first node in the row
210 graph->add_edge(NavGraphEdge(points[i - 1].first, points[i].first, props_gen));
211 }
212
213 if (i >= nx) {
214 // connect to other row, unless it's the first row
215 const size_t prev_i = (((i / nx) - 1) * nx) + (i % nx);
216 graph->add_edge(NavGraphEdge(points[prev_i].first, points[i].first));
217
218 if (add_diagonals_) {
219 if (prev_i % nx > 0) {
220 graph->add_edge(NavGraphEdge(points[prev_i - 1].first, points[i].first),
222 }
223 if (prev_i % nx < (nx - 1)) {
224 graph->add_edge(NavGraphEdge(points[prev_i + 1].first, points[i].first),
226 }
227 }
228 }
229 }
230
231 // Eliminate edges near obstacles
232 const std::vector<NavGraphEdge> &edges = graph->edges();
233
234 for (const auto &o : obstacles_) {
235 std::list<NavGraphEdge> to_remove;
236 for (const NavGraphEdge &e : edges) {
237 try {
238 if (e.distance(o.first, o.second) <= margin_) {
239 to_remove.push_back(e);
240 }
241 } catch (Exception &e) {
242 } // ignore, point is not on edge
243 }
244 std::for_each(to_remove.begin(), to_remove.end(), [&graph](const NavGraphEdge &e) {
245 graph->remove_edge(e);
246 });
247 }
248
249 // Orphan nodes are removed by the navgraph-generator thread
250}
251
252} // end of namespace fawkes
Base class for exceptions in Fawkes.
Definition: exception.h:36
Mutex * objmutex_ptr() const
Get object mutex.
Definition: lockptr.h:284
Mutex locking helper.
Definition: mutex_locker.h:34
Topological graph edge.
Definition: navgraph_edge.h:38
virtual void compute(fawkes::LockPtr< fawkes::NavGraph > graph)
Compute graph.
Definition: grid.cpp:104
virtual ~NavGraphGeneratorGrid()
Destructor.
Definition: grid.cpp:99
NavGraphGeneratorGrid(const std::map< std::string, std::string > &params)
Constructor.
Definition: grid.cpp:61
float bbox_p1_x_
X part of P1 for bounding box.
Definition: generator.h:47
float bbox_p2_x_
X part of P2 for bounding box.
Definition: generator.h:49
std::list< std::pair< float, float > > obstacles_
Obstacles to consider during navgraph generation.
Definition: generator.h:54
bool bbox_enabled_
True if bounding box requested, false otherwise.
Definition: generator.h:46
float bbox_p1_y_
Y part of P1 for bounding box.
Definition: generator.h:48
float bbox_p2_y_
Y part of P2 for bounding box.
Definition: generator.h:50
Topological graph node.
Definition: navgraph_node.h:36
void add_edge(const NavGraphEdge &edge, EdgeMode mode=EDGE_NO_INTERSECTION, bool allow_existing=false)
Add an edge.
Definition: navgraph.cpp:584
void add_node(const NavGraphNode &node)
Add a node.
Definition: navgraph.cpp:460
@ EDGE_SPLIT_INTERSECTION
Add the edge, but if it intersects with an existing edges add new points at the intersection points f...
Definition: navgraph.h:65
const std::vector< NavGraphEdge > & edges() const
Get edges of the graph.
Definition: navgraph.cpp:142
Fawkes library namespace.