Fawkes API  Fawkes Development Version
navgraph_generator_thread.cpp
1 /***************************************************************************
2  * navgraph_generator_thread.cpp - Plugin to generate navgraphs
3  *
4  * Created: Mon Feb 09 17:37:30 2015
5  * Copyright 2015 Tim Niemueller [www.niemueller.de]
6  ****************************************************************************/
7 
8 /* This program is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2 of the License, or
11  * (at your option) any later version.
12  *
13  * This program is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU Library General Public License for more details.
17  *
18  * Read the full text in the LICENSE.GPL file in the doc directory.
19  */
20 
21 #include "navgraph_generator_thread.h"
22 #ifdef HAVE_VISUALIZATION
23 # include "visualization_thread.h"
24 #endif
25 
26 #include <core/threading/mutex_locker.h>
27 #include <navgraph/generators/voronoi.h>
28 #include <plugins/laser-lines/line_func.h>
29 #include <plugins/amcl/amcl_utils.h>
30 #include <utils/misc/string_split.h>
31 
32 
33 using namespace fawkes;
34 
35 #define CFG_PREFIX "/navgraph-generator/"
36 
37 /** @class NavGraphGeneratorThread "navgraph_generator_thread.h"
38  * Thread to perform graph-based path planning.
39  * @author Tim Niemueller
40  */
41 
42 /** Constructor. */
44  : Thread("NavGraphGeneratorThread", Thread::OPMODE_WAITFORWAKEUP),
45  BlackBoardInterfaceListener("NavGraphGeneratorThread")
46 {
47 #ifdef HAVE_VISUALIZATION
48  vt_ = NULL;
49 #endif
50 }
51 
52 #ifdef HAVE_VISUALIZATION
53 /** Constructor. */
55  : Thread("NavGraphGeneratorThread", Thread::OPMODE_WAITFORWAKEUP),
56  BlackBoardInterfaceListener("NavGraphGeneratorThread")
57 {
58  vt_ = vt;
59 }
60 #endif
61 
62 
63 /** Destructor. */
65 {
66 }
67 
68 void
70 {
71  bbox_set_ = false;
72  copy_default_properties_ = true;
73 
74  filter_["FILTER_EDGES_BY_MAP"] = false;
75  filter_["FILTER_ORPHAN_NODES"] = false;
76  filter_["FILTER_MULTI_GRAPH"] = false;
77 
78  filter_params_float_defaults_["FILTER_EDGES_BY_MAP"]["distance"] = 0.3;
79  if (config->exists(CFG_PREFIX"filters/edges_by_map/distance")) {
80  filter_params_float_defaults_["FILTER_EDGES_BY_MAP"]["distance"] =
81  config->get_float(CFG_PREFIX"filters/edges_by_map/distance");
82  }
83 
84  filter_params_float_ = filter_params_float_defaults_;
85 
86  cfg_map_line_segm_max_iterations_ =
87  config->get_uint(CFG_PREFIX"map/line_segmentation_max_iterations");
88  cfg_map_line_segm_min_inliers_ =
89  config->get_uint(CFG_PREFIX"map/line_segmentation_min_inliers");
90  cfg_map_line_min_length_ =
91  config->get_float(CFG_PREFIX"map/line_min_length");
92  cfg_map_line_cluster_tolerance_ =
93  config->get_float(CFG_PREFIX"map/line_cluster_tolerance");
94  cfg_map_line_cluster_quota_ =
95  config->get_float(CFG_PREFIX"map/line_cluster_quota");
96 
97  cfg_global_frame_ = config->get_string("/frames/fixed");
98 
99  cfg_visualization_ = false;
100  try {
101  cfg_visualization_ = config->get_bool(CFG_PREFIX"visualization/enable");
102  } catch (Exception &e) {} // ignore, use default
103 
104 #ifndef HAVE_VISUALIZATION
105  if (cfg_visualization_) {
106  logger->log_warn(name(), "Visualization enabled, but support not compiled in");
107  }
108 #endif
109 
110  navgen_if_ =
111  blackboard->open_for_writing<NavGraphGeneratorInterface>("/navgraph-generator");
112  bbil_add_message_interface(navgen_if_);
113  blackboard->register_listener(this, BlackBoard::BBIL_FLAG_MESSAGES);
114 
115 }
116 
117 void
119 {
121  bbil_remove_message_interface(navgen_if_);
122  blackboard->close(navgen_if_);
123 }
124 
125 
126 void
128 {
130 
131  logger->log_debug(name(), "Calculating new graph");
132 
133  if (bbox_set_) {
134  logger->log_debug(name(), " Setting bound box (%f,%f) to (%f,%f)",
135  bbox_p1_.x, bbox_p1_.y, bbox_p2_.x, bbox_p2_.y);
136  nggv.set_bounding_box(bbox_p1_.x, bbox_p1_.y, bbox_p2_.x, bbox_p2_.y);
137  }
138 
139  for (auto o : obstacles_) {
140  logger->log_debug(name(), " Adding obstacle %s at (%f,%f)",
141  o.first.c_str(), o.second.x, o.second.y);
142  nggv.add_obstacle(o.second.x, o.second.y);
143  }
144 
145  for (auto o : map_obstacles_) {
146  logger->log_debug(name(), " Adding map obstacle %s at (%f,%f)",
147  o.first.c_str(), o.second.x, o.second.y);
148  nggv.add_obstacle(o.second.x, o.second.y);
149  }
150 
151  // Acquire lock on navgraph, no more searches/modifications until we are done
152  MutexLocker lock(navgraph.objmutex_ptr());
153 
154  // disable notifications as to not trigger one for all the many
155  // operations we are going to perform
156  navgraph->set_notifications_enabled(false);
157 
158  // remember default properties
159  std::map<std::string, std::string> default_props = navgraph->default_properties();
160 
161  navgraph->clear();
162 
163  // restore default properties
164  if (copy_default_properties_) {
165  navgraph->set_default_properties(default_props);
166  }
167 
168  // set properties received as message
169  for (auto p : default_properties_) {
170  navgraph->set_default_property(p.first, p.second);
171  }
172 
173  logger->log_debug(name(), " Computing Voronoi");
174  nggv.compute(navgraph);
175 
176  // post-processing
177  if (filter_["FILTER_EDGES_BY_MAP"]) {
178  logger->log_debug(name(), " Applying FILTER_EDGES_BY_MAP");
179  filter_edges_from_map(filter_params_float_["FILTER_EDGES_BY_MAP"]["distance"]);
180  }
181  if (filter_["FILTER_ORPHAN_NODES"]) {
182  logger->log_debug(name(), " Applying FILTER_ORPHAN_NODES");
183  filter_nodes_orphans();
184  }
185  if (filter_["FILTER_MULTI_GRAPH"]) {
186  logger->log_debug(name(), " Applying FILTER_MULTI_GRAPH");
187  filter_multi_graph();
188  }
189 
190  // add POIs
191  for (const auto &p : pois_) {
192  // add poi
193  NavGraphNode node(p.first, p.second.position.x, p.second.position.y,
194  p.second.properties);
195  switch (p.second.conn_mode) {
196  case NavGraphGeneratorInterface::NOT_CONNECTED:
197  logger->log_debug(name(), " POI without initial connection %s at (%f,%f)",
198  p.first.c_str(), p.second.position.x, p.second.position.y);
199  navgraph->add_node(node);
200  break;
201 
202  case NavGraphGeneratorInterface::UNCONNECTED:
203  logger->log_debug(name(), " Unconnected POI %s at (%f,%f)",
204  p.first.c_str(), p.second.position.x, p.second.position.y);
205  node.set_unconnected(true);
206  navgraph->add_node(node);
207  break;
208 
209  case NavGraphGeneratorInterface::CLOSEST_NODE:
210  logger->log_debug(name(), " Connecting POI %s at (%f,%f) to closest node",
211  p.first.c_str(), p.second.position.x, p.second.position.y);
212  navgraph->add_node_and_connect(node, NavGraph::CLOSEST_NODE);
213  break;
214  case NavGraphGeneratorInterface::CLOSEST_EDGE:
215  try {
216  logger->log_debug(name(), " Connecting POI %s at (%f,%f) to closest edge",
217  p.first.c_str(), p.second.position.x, p.second.position.y);
218  navgraph->add_node_and_connect(node, NavGraph::CLOSEST_EDGE);
219  } catch (Exception &e) {
220  logger->log_error(name(), " Failed to add POI %s, exception follows",
221  p.first.c_str());
222  logger->log_error(name(), e);
223  }
224  break;
225  case NavGraphGeneratorInterface::CLOSEST_EDGE_OR_NODE:
226  logger->log_debug(name(), " Connecting POI %s at (%f,%f) to closest edge or node",
227  p.first.c_str(), p.second.position.x, p.second.position.y);
228  navgraph->add_node_and_connect(node, NavGraph::CLOSEST_EDGE_OR_NODE);
229  break;
230  }
231  }
232 
233 
234  // add edges
235  for (const auto &e : edges_) {
236  switch (e.edge_mode) {
237  case NavGraphGeneratorInterface::NO_INTERSECTION:
238  logger->log_debug(name(), " Edge %s-%s%s (no intersection)",
239  e.p1.c_str(), e.directed ? ">" : "-", e.p2.c_str());
240  navgraph->add_edge(NavGraphEdge(e.p1, e.p2, e.directed),
241  NavGraph::EDGE_NO_INTERSECTION);
242  break;
243 
244  case NavGraphGeneratorInterface::SPLIT_INTERSECTION:
245  logger->log_debug(name(), " Edge %s-%s%s (split intersection)",
246  e.p1.c_str(), e.directed ? ">" : "-", e.p2.c_str());
247  navgraph->add_edge(NavGraphEdge(e.p1, e.p2, e.directed),
248  NavGraph::EDGE_SPLIT_INTERSECTION);
249  break;
250 
251  case NavGraphGeneratorInterface::FORCE:
252  logger->log_debug(name(), " Edge %s-%s%s (force)",
253  e.p1.c_str(), e.directed ? ">" : "-", e.p2.c_str());
254  navgraph->add_edge(NavGraphEdge(e.p1, e.p2, e.directed),
255  NavGraph::EDGE_FORCE);
256  break;
257  }
258  }
259 
260  /*
261  // Add POIs in free areas
262  unsigned int ci = 0;
263  const std::list<Polygon2D> &polygons = nggv.face_polygons();
264  for (const auto &p : polygons) {
265  Eigen::Vector2f centroid(polygon_centroid(p));
266  navgraph->add_node_and_connect(NavGraphNode(navgraph->format_name("AREA-%u", ++ci),
267  centroid.x(), centroid.y()),
268  NavGraph::CLOSEST_EDGE_OR_NODE);
269 
270  }
271  */
272 
273  // Finalize graph setup
274  try {
275  logger->log_debug(name(), " Calculate reachability relations");
276  navgraph->calc_reachability();
277  } catch (Exception &e) {
278  logger->log_error(name(), "Failed to finalize graph setup, exception follows");
279  logger->log_error(name(), e);
280  }
281 
282  // re-enable notifications
283  navgraph->set_notifications_enabled(true);
284 
285  logger->log_debug(name(), " Graph computed, notifying listeners");
286  navgraph->notify_of_change();
287 
288  navgen_if_->set_final(true);
289  navgen_if_->write();
290 
291 #ifdef HAVE_VISUALIZATION
292  if (cfg_visualization_) publish_visualization();
293 #endif
294 }
295 
296 
297 bool
298 NavGraphGeneratorThread::bb_interface_message_received(Interface *interface,
299  Message *message) throw()
300 {
301  // in continuous mode wait for signal if disabled
302  MutexLocker lock(loop_mutex);
303 
305  pois_.clear();
306  obstacles_.clear();
307  map_obstacles_.clear();
308  edges_.clear();
309  default_properties_.clear();
310  bbox_set_ = false;
311  copy_default_properties_ = true;
312  filter_params_float_ = filter_params_float_defaults_;
313  for (auto &f : filter_) {
314  f.second = false;
315  }
319  bbox_set_ = true;
320  bbox_p1_.x = msg->p1_x();
321  bbox_p1_.y = msg->p1_y();
322  bbox_p2_.x = msg->p2_x();
323  bbox_p2_.y = msg->p2_y();
324 
328 
329  filter_[navgen_if_->tostring_FilterType(msg->filter())] = msg->is_enable();
330 
334 
335  std::map<std::string, float> &param_float =
336  filter_params_float_[navgen_if_->tostring_FilterType(msg->filter())];
337 
338  if (param_float.find(msg->param()) != param_float.end()) {
339  param_float[msg->param()] = msg->value();
340  } else {
341  logger->log_warn(name(), "Filter %s has no float parameter named %s, ignoring",
342  navgen_if_->tostring_FilterType(msg->filter()),
343  msg->param());
344  }
345 
349  map_obstacles_ = map_obstacles(msg->max_line_point_distance());
350 
354  if (std::isfinite(msg->x()) && std::isfinite(msg->x())) {
355  obstacles_[msg->name()] = cart_coord_2d_t(msg->x(), msg->y());
356  } else {
357  logger->log_error(name(), "Received non-finite obstacle (%.2f,%.2f), ignoring",
358  msg->x(), msg->y());
359  }
363  ObstacleMap::iterator f;
364  if ((f = obstacles_.find(msg->name())) != obstacles_.end()) {
365  obstacles_.erase(f);
366  }
367 
371  if (std::isfinite(msg->x()) && std::isfinite(msg->x())) {
372  PointOfInterest poi;
373  poi.position = cart_coord_2d_t(msg->x(), msg->y());
374  poi.conn_mode = msg->mode();
375  pois_[msg->name()] = poi;
376  } else {
377  logger->log_error(name(), "Received non-finite POI (%.2f,%.2f), ignoring",
378  msg->x(), msg->y());
379  }
380 
384  Edge edge;
385  edge.p1 = msg->p1();
386  edge.p2 = msg->p2();
387  edge.directed = msg->is_directed();
388  edge.edge_mode = msg->mode();
389  edges_.push_back(edge);
390 
394 
395  if (std::isfinite(msg->x()) && std::isfinite(msg->x())) {
396  PointOfInterest poi;
397  poi.position = cart_coord_2d_t(msg->x(), msg->y());
398  poi.conn_mode = msg->mode();
399  if (std::isfinite(msg->ori())) {
400  poi.properties["orientation"] = std::to_string(msg->ori());
401  } else {
402  logger->log_warn(name(), "Received POI with non-finite ori %f, ignoring ori",
403  msg->ori());
404  }
405  pois_[msg->name()] = poi;
406  } else {
407  logger->log_error(name(), "Received non-finite POI (%.2f,%.2f), ignoring ori",
408  msg->x(), msg->y());
409  }
410 
414  PoiMap::iterator f;
415  if ((f = pois_.find(msg->name())) != pois_.end()) {
416  pois_.erase(f);
417  }
418 
422  PoiMap::iterator f;
423  if ((f = pois_.find(msg->name())) != pois_.end()) {
424  f->second.properties[msg->property_name()] = msg->property_value();
425  } else {
426  logger->log_warn(name(), "POI %s unknown, cannot set property %s=%s",
427  msg->name(), msg->property_name(), msg->property_value());
428  }
429 
433  default_properties_[msg->property_name()] = msg->property_value();
434 
438  copy_default_properties_ = msg->is_enable_copy();
439 
441  navgen_if_->set_msgid(message->id());
442  navgen_if_->set_final(false);
443  navgen_if_->write();
444  wakeup();
445 
446  } else {
447  logger->log_error(name(), "Received unknown message of type %s, ignoring",
448  message->type());
449  }
450 
451  return false;
452 }
453 
454 
455 map_t *
456 NavGraphGeneratorThread::load_map(std::vector<std::pair<int, int>> &free_space_indices)
457 {
458  std::string cfg_map_file;
459  float cfg_resolution;
460  float cfg_origin_x;
461  float cfg_origin_y;
462  float cfg_origin_theta;
463  float cfg_occupied_thresh;
464  float cfg_free_thresh;
465 
466  fawkes::amcl::read_map_config(config, cfg_map_file, cfg_resolution, cfg_origin_x,
467  cfg_origin_y, cfg_origin_theta, cfg_occupied_thresh,
468  cfg_free_thresh);
469 
470  return fawkes::amcl::read_map(cfg_map_file.c_str(),
471  cfg_origin_x, cfg_origin_y, cfg_resolution,
472  cfg_occupied_thresh, cfg_free_thresh, free_space_indices);
473 }
474 
475 NavGraphGeneratorThread::ObstacleMap
476 NavGraphGeneratorThread::map_obstacles(float line_max_dist)
477 {
478  ObstacleMap obstacles;
479  unsigned int obstacle_i = 0;
480 
481  std::vector<std::pair<int, int> > free_space_indices;
482  map_t *map = load_map(free_space_indices);
483 
484  logger->log_info(name(), "Map Obstacles: map size: %ux%u (%zu of %u cells free, %.1f%%)",
485  map->size_x, map->size_y, free_space_indices.size(),
486  map->size_x * map->size_y,
487  (float)free_space_indices.size() / (float)(map->size_x * map->size_y) * 100.);
488 
489  size_t occ_cells = map->size_x * map->size_y - free_space_indices.size();
490 
491  // convert map to point cloud
493  map_cloud->points.resize(occ_cells);
494  size_t pi = 0;
495  for (int x = 0; x < map->size_x; ++x) {
496  for (int y = 0; y < map->size_y; ++y) {
497  if (map->cells[MAP_INDEX(map, x, y)].occ_state > 0) {
498  // cell is occupied, generate point in cloud
499  pcl::PointXYZ p;
500  p.x = MAP_WXGX(map, x) + 0.5 * map->scale;
501  p.y = MAP_WYGY(map, y) + 0.5 * map->scale;
502  p.z = 0.;
503  map_cloud->points[pi++] = p;
504  }
505  }
506  }
507 
508  logger->log_info(name(), "Map Obstacles: filled %zu/%zu points", pi, occ_cells);
509 
511  no_line_cloud(new pcl::PointCloud<pcl::PointXYZ>());
512 
513  // determine lines
514  std::vector<LineInfo> linfos =
515  calc_lines<pcl::PointXYZ>(map_cloud,
516  cfg_map_line_segm_min_inliers_, cfg_map_line_segm_max_iterations_,
517  /* segm distance threshold */ 2 * map->scale,
518  /* segm sample max dist */ 2 * map->scale,
519  cfg_map_line_cluster_tolerance_, cfg_map_line_cluster_quota_,
520  cfg_map_line_min_length_, -1, -1, -1,
521  no_line_cloud);
522 
523  logger->log_info(name(), "Map Obstacles: found %zu lines, %zu points remaining",
524  linfos.size(), no_line_cloud->points.size());
525 
526 
527  // determine line obstacle points
528  for (const LineInfo &line : linfos) {
529  const unsigned int num_points = ceilf(line.length / line_max_dist);
530  float distribution = line.length / num_points;
531 
532  obstacles[NavGraph::format_name("Map_%u", ++obstacle_i)] =
533  cart_coord_2d_t(line.end_point_1[0], line.end_point_1[1]);
534  for (unsigned int i = 1; i <= num_points; ++i) {
535  Eigen::Vector3f p = line.end_point_1 + i * distribution * line.line_direction;
536  obstacles[NavGraph::format_name("Map_%d", ++obstacle_i)] = cart_coord_2d_t(p[0], p[1]);
537  }
538  }
539 
540  // cluster in remaining points to find more points of interest
541  pcl::search::KdTree<pcl::PointXYZ>::Ptr
542  kdtree_cluster(new pcl::search::KdTree<pcl::PointXYZ>());
543 
544  std::vector<pcl::PointIndices> cluster_indices;
545  pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
546  ec.setClusterTolerance(2 * map->scale);
547  ec.setMinClusterSize(1);
548  ec.setMaxClusterSize(no_line_cloud->points.size());
549  ec.setSearchMethod(kdtree_cluster);
550  ec.setInputCloud(no_line_cloud);
551  ec.extract(cluster_indices);
552 
553  unsigned int i = 0;
554  for (auto cluster : cluster_indices) {
555  Eigen::Vector4f centroid;
556  pcl::compute3DCentroid(*no_line_cloud, cluster.indices, centroid);
557 
558  logger->log_info(name(), "Map Obstacles: Cluster %u with %zu points at (%f, %f, %f)",
559  i, cluster.indices.size(), centroid.x(), centroid.y(), centroid.z());
560 
561  obstacles[NavGraph::format_name("MapCluster_%u", ++i)] = cart_coord_2d_t(centroid.x(), centroid.y());
562  }
563 
564  map_free(map);
565 
566  return obstacles;
567 }
568 
569 
570 void
571 NavGraphGeneratorThread::filter_edges_from_map(float max_dist)
572 {
573  std::vector<std::pair<int, int> > free_space_indices;
574  map_t *map = load_map(free_space_indices);
575 
576  const std::vector<NavGraphEdge> &edges = navgraph->edges();
577 
578  for (int x = 0; x < map->size_x; ++x) {
579  for (int y = 0; y < map->size_y; ++y) {
580  if (map->cells[MAP_INDEX(map, x, y)].occ_state > 0) {
581  // cell is occupied, generate point in cloud
582  Eigen::Vector2f gp;
583  gp[0] = MAP_WXGX(map, x) + 0.5 * map->scale;
584  gp[1] = MAP_WYGY(map, y) + 0.5 * map->scale;
585 
586  for (const NavGraphEdge &e : edges) {
587  try {
588  cart_coord_2d_t poe = e.closest_point_on_edge(gp[0], gp[1]);
589  Eigen::Vector2f p;
590  p[0] = poe.x; p[1] = poe.y;
591  if ((gp - p).norm() <= max_dist) {
592  // edge too close, remove it!
593  logger->log_debug(name(),
594  " Removing edge (%s--%s), too close to occupied map cell (%f,%f)",
595  e.from().c_str(), e.to().c_str(), gp[0], gp[1]);
596  navgraph->remove_edge(e);
597  break;
598  }
599  } catch (Exception &e) {} // alright, not close
600  }
601  }
602  }
603  }
604  map_free(map);
605 }
606 
607 
608 void
609 NavGraphGeneratorThread::filter_nodes_orphans()
610 {
611  const std::vector<NavGraphEdge> &edges = navgraph->edges();
612  const std::vector<NavGraphNode> &nodes = navgraph->nodes();
613 
614  std::list<NavGraphNode> remove_nodes;
615 
616  for (const NavGraphNode &n : nodes) {
617  std::string nname = n.name();
618  std::vector<NavGraphEdge>::const_iterator e =
619  std::find_if(edges.begin(), edges.end(),
620  [nname](const NavGraphEdge &e)->bool{
621  return (e.from() == nname || e.to() == nname);
622  });
623  if (e == edges.end() && ! n.unconnected()) {
624  // node is not connected to any other node -> remove
625  remove_nodes.push_back(n);
626  }
627  }
628 
629  for (const NavGraphNode &n : remove_nodes) {
630  logger->log_debug(name(), " Removing unconnected node %s", n.name().c_str());
631  navgraph->remove_node(n);
632  }
633 }
634 
635 
636 void
637 NavGraphGeneratorThread::filter_multi_graph()
638 {
639  navgraph->calc_reachability(/* allow multi graph*/ true);
640 
641  std::list<std::set<std::string>> graphs;
642 
643  const std::vector<NavGraphNode> &nodes = navgraph->nodes();
644  std::set<std::string> nodeset;
645  std::for_each(nodes.begin(), nodes.end(),
646  [&nodeset](const NavGraphNode &n){ nodeset.insert(n.name()); });
647 
648  while (! nodeset.empty()) {
649  std::queue<std::string> q;
650  q.push(* nodeset.begin());
651 
652  std::set<std::string> traversed;
653 
654  while (! q.empty()) {
655  std::string &nname = q.front();
656  traversed.insert(nname);
657 
658  NavGraphNode n = navgraph->node(nname);
659  if (n) {
660  const std::vector<std::string> & reachable = n.reachable_nodes();
661 
662  for (const std::string &r : reachable) {
663  if (traversed.find(r) == traversed.end()) q.push(r);
664  }
665  }
666  q.pop();
667  }
668 
669  std::set<std::string> nodediff;
670  std::set_difference(nodeset.begin(), nodeset.end(),
671  traversed.begin(), traversed.end(),
672  std::inserter(nodediff, nodediff.begin()));
673  graphs.push_back(traversed);
674  nodeset = nodediff;
675  }
676 
677  // reverse sort, largest set first
678  graphs.sort([](const std::set<std::string> &a, const std::set<std::string> &b)->bool{
679  return b.size() < a.size();
680  });
681 
682  std::for_each(std::next(graphs.begin()), graphs.end(),
683  [&](const std::set<std::string> &g) {
684  logger->log_debug(name(), " Removing disconnected sub-graph [%s]",
685  str_join(g.begin(), g.end(), ", ").c_str());
686  for (const std::string &n : g) navgraph->remove_node(n);
687  });
688 }
689 
690 
691 #ifdef HAVE_VISUALIZATION
692 void
693 NavGraphGeneratorThread::publish_visualization()
694 {
695  if (vt_) {
696  vt_->publish(obstacles_, map_obstacles_, pois_);
697  }
698 }
699 #endif
void set_msgid(const uint32_t new_msgid)
Set msgid value.
Thread(const char *name)
Constructor.
Definition: thread.cpp:205
Line information container.
Definition: line_info.h:38
SetFilterParamFloatMessage Fawkes BlackBoard Interface Message.
RemovePointOfInterestMessage Fawkes BlackBoard Interface Message.
AddPointOfInterestMessage Fawkes BlackBoard Interface Message.
float max_line_point_distance() const
Get max_line_point_distance value.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:44
unsigned int id() const
Get message ID.
Definition: message.cpp:197
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
AddPointOfInterestWithOriMessage Fawkes BlackBoard Interface Message.
fawkes::LockPtr< NavGraph > navgraph
NavGraph instance shared in framework.
Definition: navgraph.h:46
SetGraphDefaultPropertyMessage Fawkes BlackBoard Interface Message.
Cartesian coordinates (2D).
Definition: types.h:59
SetCopyGraphDefaultPropertiesMessage Fawkes BlackBoard Interface Message.
Fawkes library namespace.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
void set_unconnected(bool unconnected)
Set unconnected state of the node.
Mutex locking helper.
Definition: mutex_locker.h:33
virtual ~NavGraphGeneratorThread()
Destructor.
std::string str_join(const InputIterator &first, const InputIterator &last, char delim='/')
Join list of strings string using given delimiter.
Definition: string_split.h:134
SetFilterMessage Fawkes BlackBoard Interface Message.
SetBoundingBoxMessage Fawkes BlackBoard Interface Message.
void set_bounding_box(float bbox_p1_x, float bbox_p1_y, float bbox_p2_x, float bbox_p2_y)
Set bounding box.
Definition: voronoi.cpp:153
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
Definition: blackboard.cpp:218
Thread class encapsulation of pthreads.
Definition: thread.h:42
const char * tostring_FilterType(FilterType value) const
Convert FilterType constant to string.
void add_obstacle(float x, float y)
Add an obstacle point.
Definition: voronoi.cpp:181
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:500
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:79
Mutex * loop_mutex
Mutex that is used to protect a call to loop().
Definition: thread.h:139
AddObstacleMessage Fawkes BlackBoard Interface Message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
SetPointOfInterestPropertyMessage Fawkes BlackBoard Interface Message.
virtual void loop()
Code to execute in the thread.
NavGraphGeneratorInterface Fawkes BlackBoard Interface.
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
Definition: blackboard.cpp:190
ComputeMessage Fawkes BlackBoard Interface Message.
virtual void finalize()
Finalize the thread.
void bbil_remove_message_interface(Interface *interface)
Remove an interface to the message received watch list.
void wakeup()
Wake up thread.
Definition: thread.cpp:1000
Base class for exceptions in Fawkes.
Definition: exception.h:36
Generate navgraph using a Voronoi diagram.
Definition: voronoi.h:35
void set_final(const bool new_final)
Set final value.
virtual void init()
Initialize the thread.
BlackBoardInterfaceListener(const char *name_format,...)
Constructor.
const char * name() const
Get name of thread.
Definition: thread.h:95
MessageType * as_type()
Cast message to given type if possible.
Definition: message.h:146
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
Send Marker messages to rviz to show navgraph-generator info.
bool is_of_type()
Check if message has desired type.
Definition: message.h:138
float y
y coordinate
Definition: types.h:61
ClearMessage Fawkes BlackBoard Interface Message.
Topological graph edge.
Definition: navgraph_edge.h:39
Topological graph node.
Definition: navgraph_node.h:38
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
AddMapObstaclesMessage Fawkes BlackBoard Interface Message.
virtual bool exists(const char *path)=0
Check if a given value exists.
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
AddEdgeMessage Fawkes BlackBoard Interface Message.
const std::vector< std::string > & reachable_nodes() const
Get reachable nodes.
virtual void compute(fawkes::LockPtr< fawkes::NavGraph > graph)
Compute graph.
Definition: voronoi.cpp:202
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:44
void bbil_add_message_interface(Interface *interface)
Add an interface to the message received watch list.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
RemoveObstacleMessage Fawkes BlackBoard Interface Message.
const char * type() const
Get message type.
Definition: message.cpp:378
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
BlackBoard interface listener.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
virtual void close(Interface *interface)=0
Close interface.
float x
x coordinate
Definition: types.h:60