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 
35 typedef CGAL::Quotient<CGAL::MP_Float> NT;
36 typedef CGAL::Cartesian<NT> Kernel;
37 
38 typedef Kernel::Point_2 Point;
39 typedef Kernel::Segment_2 Segment;
40 typedef Kernel::Vector_2 Vector_2;
41 typedef CGAL::Points_on_segment_2<Point> PG;
42 typedef CGAL::Creator_uniform_2<Point, Segment> Creator;
43 typedef CGAL::Join_input_iterator_2<PG, PG, Creator> Segm_iterator;
44 typedef CGAL::Counting_iterator<Segm_iterator, Segment> Count_iterator;
45 typedef std::vector<Segment> Vector;
46 
47 typedef CGAL::Arr_segment_traits_2<Kernel> Traits_2;
48 typedef Traits_2::Curve_2 Curve_2;
49 
50 namespace 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  */
61 NavGraphGeneratorGrid::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 
103 void
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
fawkes::NavGraphNode
Definition: navgraph_node.h:38
fawkes::LockPtr< fawkes::NavGraph >
fawkes::NavGraphGeneratorGrid::~NavGraphGeneratorGrid
virtual ~NavGraphGeneratorGrid()
Destructor.
Definition: grid.cpp:98
fawkes::NavGraphGenerator::bbox_p2_y_
float bbox_p2_y_
Y part of P2 for bounding box.
Definition: generator.h:57
fawkes::NavGraph::edges
const std::vector< NavGraphEdge > & edges() const
Get edges of the graph.
Definition: navgraph.cpp:145
fawkes::NavGraphEdge
Definition: navgraph_edge.h:40
fawkes::MutexLocker
Definition: mutex_locker.h:37
fawkes::LockPtr::objmutex_ptr
Mutex * objmutex_ptr() const
Get object mutex.
Definition: lockptr.h:297
fawkes::NavGraphGenerator::bbox_p1_y_
float bbox_p1_y_
Y part of P1 for bounding box.
Definition: generator.h:55
fawkes::NavGraph::add_edge
void add_edge(const NavGraphEdge &edge, EdgeMode mode=EDGE_NO_INTERSECTION, bool allow_existing=false)
Add an edge.
Definition: navgraph.cpp:587
fawkes::NavGraphGeneratorGrid::NavGraphGeneratorGrid
NavGraphGeneratorGrid(const std::map< std::string, std::string > &params)
Constructor.
Definition: grid.cpp:60
fawkes::NavGraphGenerator::bbox_enabled_
bool bbox_enabled_
True if bounding box requested, false otherwise.
Definition: generator.h:53
fawkes::NavGraph::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:68
fawkes::NavGraph::remove_edge
void remove_edge(const NavGraphEdge &edge)
Remove an edge.
Definition: navgraph.cpp:680
fawkes
fawkes::NavGraphGeneratorGrid::compute
virtual void compute(fawkes::LockPtr< fawkes::NavGraph > graph)
Definition: grid.cpp:103
fawkes::NavGraphGenerator::obstacles_
std::list< std::pair< float, float > > obstacles_
Obstacles to consider during navgraph generation.
Definition: generator.h:61
fawkes::NavGraphGenerator::bbox_p1_x_
float bbox_p1_x_
X part of P1 for bounding box.
Definition: generator.h:54
fawkes::NavGraph::add_node
void add_node(const NavGraphNode &node)
Add a node.
Definition: navgraph.cpp:463
fawkes::NavGraphGenerator::bbox_p2_x_
float bbox_p2_x_
X part of P2 for bounding box.
Definition: generator.h:56
fawkes::Exception
Definition: exception.h:39