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-2017 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/grid.h>
28 #include <navgraph/generators/voronoi.h>
29 #include <navgraph/yaml_navgraph.h>
30 #include <plugins/amcl/amcl_utils.h>
31 #include <plugins/laser-lines/line_func.h>
32 #include <utils/misc/string_split.h>
33 
34 using namespace fawkes;
35 
36 #define CFG_PREFIX "/navgraph-generator/"
37 
38 /** @class NavGraphGeneratorThread "navgraph_generator_thread.h"
39  * Thread to perform graph-based path planning.
40  * @author Tim Niemueller
41  */
42 
43 /** Constructor. */
45 : Thread("NavGraphGeneratorThread", Thread::OPMODE_WAITFORWAKEUP),
46  BlackBoardInterfaceListener("NavGraphGeneratorThread")
47 {
48 #ifdef HAVE_VISUALIZATION
49  vt_ = NULL;
50 #endif
51 }
52 
53 #ifdef HAVE_VISUALIZATION
54 /** Constructor. */
56 : Thread("NavGraphGeneratorThread", Thread::OPMODE_WAITFORWAKEUP),
57  BlackBoardInterfaceListener("NavGraphGeneratorThread")
58 {
59  vt_ = vt;
60 }
61 #endif
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_ = config->get_uint(CFG_PREFIX "map/line_segmentation_min_inliers");
89  cfg_map_line_min_length_ = config->get_float(CFG_PREFIX "map/line_min_length");
90  cfg_map_line_cluster_tolerance_ = config->get_float(CFG_PREFIX "map/line_cluster_tolerance");
91  cfg_map_line_cluster_quota_ = config->get_float(CFG_PREFIX "map/line_cluster_quota");
92 
93  cfg_global_frame_ = config->get_string("/frames/fixed");
94 
95  cfg_visualization_ = false;
96  try {
97  cfg_visualization_ = config->get_bool(CFG_PREFIX "visualization/enable");
98  } catch (Exception &e) {
99  } // ignore, use default
100 
101  cfg_save_to_file_ = false;
102  try {
103  cfg_save_to_file_ = config->get_bool(CFG_PREFIX "save-to-file/enable");
104  } catch (Exception &e) {
105  } // ignore, use default
106  if (cfg_save_to_file_) {
107  cfg_save_filename_ = config->get_string(CFG_PREFIX "save-to-file/filename");
108  if (cfg_save_filename_.empty()) {
109  throw Exception("navgraph-generator: invalid empty filename");
110  }
111  if (cfg_save_filename_.find("..") != std::string::npos) {
112  throw Exception("navgraph-generator: filename may not contains two consecutive dots (..)");
113  }
114  if (cfg_save_filename_[0] != '/') {
115  cfg_save_filename_ = std::string(CONFDIR) + "/" + cfg_save_filename_;
116  }
117  }
118 
119 #ifndef HAVE_VISUALIZATION
120  if (cfg_visualization_) {
121  logger->log_warn(name(), "Visualization enabled, but support not compiled in");
122  }
123 #endif
124 
125  navgen_if_ = blackboard->open_for_writing<NavGraphGeneratorInterface>("/navgraph-generator");
126  bbil_add_message_interface(navgen_if_);
127  blackboard->register_listener(this, BlackBoard::BBIL_FLAG_MESSAGES);
128 }
129 
130 void
132 {
134  bbil_remove_message_interface(navgen_if_);
135  blackboard->close(navgen_if_);
136 }
137 
138 void
140 {
141  std::shared_ptr<NavGraphGenerator> ng;
142 
143  try {
144  switch (algorithm_) {
146  ng.reset(new NavGraphGeneratorGrid(algorithm_params_));
147  break;
148  default: ng.reset(new NavGraphGeneratorVoronoi());
149  }
150  } catch (Exception &e) {
151  logger->log_error(name(),
152  "Failed to initialize algorithm %s, exception follows",
153  navgen_if_->tostring_Algorithm(algorithm_));
154  logger->log_error(name(), e);
155  navgen_if_->set_ok(false);
156  navgen_if_->set_error_message(e.what_no_backtrace());
157  navgen_if_->set_final(true);
158  navgen_if_->write();
159  return;
160  }
161 
162  logger->log_debug(name(),
163  "Calculating new graph (%s)",
164  navgen_if_->tostring_Algorithm(algorithm_));
165 
166  if (bbox_set_) {
167  logger->log_debug(name(),
168  " Setting bound box (%f,%f) to (%f,%f)",
169  bbox_p1_.x,
170  bbox_p1_.y,
171  bbox_p2_.x,
172  bbox_p2_.y);
173  ng->set_bounding_box(bbox_p1_.x, bbox_p1_.y, bbox_p2_.x, bbox_p2_.y);
174  }
175 
176  for (auto o : obstacles_) {
177  logger->log_debug(
178  name(), " Adding obstacle %s at (%f,%f)", o.first.c_str(), o.second.x, o.second.y);
179  ng->add_obstacle(o.second.x, o.second.y);
180  }
181 
182  for (auto o : map_obstacles_) {
183  logger->log_debug(
184  name(), " Adding map obstacle %s at (%f,%f)", o.first.c_str(), o.second.x, o.second.y);
185  ng->add_obstacle(o.second.x, o.second.y);
186  }
187 
188  // Acquire lock on navgraph, no more searches/modifications until we are done
189  MutexLocker lock(navgraph.objmutex_ptr());
190 
191  // disable notifications as to not trigger one for all the many
192  // operations we are going to perform
193  navgraph->set_notifications_enabled(false);
194 
195  // remember default properties
196  std::map<std::string, std::string> default_props = navgraph->default_properties();
197 
198  navgraph->clear();
199 
200  // restore default properties
201  if (copy_default_properties_) {
202  navgraph->set_default_properties(default_props);
203  }
204 
205  // set properties received as message
206  for (auto p : default_properties_) {
207  navgraph->set_default_property(p.first, p.second);
208  }
209 
210  logger->log_debug(name(), " Computing navgraph");
211  try {
212  ng->compute(navgraph);
213  } catch (Exception &e) {
214  logger->log_error(name(), "Failed to compute navgraph, exception follows");
215  logger->log_error(name(), e);
216  navgen_if_->set_ok(false);
217  navgen_if_->set_error_message(e.what_no_backtrace());
218  navgen_if_->write();
219  navgraph->set_notifications_enabled(true);
220  return;
221  }
222 
223  // post-processing
224  if (filter_["FILTER_EDGES_BY_MAP"]) {
225  logger->log_debug(name(), " Applying FILTER_EDGES_BY_MAP");
226  filter_edges_from_map(filter_params_float_["FILTER_EDGES_BY_MAP"]["distance"]);
227  }
228  if (filter_["FILTER_ORPHAN_NODES"]) {
229  logger->log_debug(name(), " Applying FILTER_ORPHAN_NODES");
230  filter_nodes_orphans();
231  }
232  if (filter_["FILTER_MULTI_GRAPH"]) {
233  logger->log_debug(name(), " Applying FILTER_MULTI_GRAPH");
234  filter_multi_graph();
235  }
236 
237  // add POIs
238  for (const auto &p : pois_) {
239  // add poi
240  NavGraphNode node(p.first, p.second.position.x, p.second.position.y, p.second.properties);
241  switch (p.second.conn_mode) {
242  case NavGraphGeneratorInterface::NOT_CONNECTED:
243  logger->log_debug(name(),
244  " POI without initial connection %s at (%f,%f)",
245  p.first.c_str(),
246  p.second.position.x,
247  p.second.position.y);
248  navgraph->add_node(node);
249  break;
250 
251  case NavGraphGeneratorInterface::UNCONNECTED:
252  logger->log_debug(name(),
253  " Unconnected POI %s at (%f,%f)",
254  p.first.c_str(),
255  p.second.position.x,
256  p.second.position.y);
257  node.set_unconnected(true);
258  navgraph->add_node(node);
259  break;
260 
261  case NavGraphGeneratorInterface::CLOSEST_NODE:
262  logger->log_debug(name(),
263  " Connecting POI %s at (%f,%f) to closest node",
264  p.first.c_str(),
265  p.second.position.x,
266  p.second.position.y);
267  navgraph->add_node_and_connect(node, NavGraph::CLOSEST_NODE);
268  break;
269  case NavGraphGeneratorInterface::CLOSEST_EDGE:
270  try {
271  logger->log_debug(name(),
272  " Connecting POI %s at (%f,%f) to closest edge",
273  p.first.c_str(),
274  p.second.position.x,
275  p.second.position.y);
276  navgraph->add_node_and_connect(node, NavGraph::CLOSEST_EDGE);
277  } catch (Exception &e) {
278  logger->log_error(name(), " Failed to add POI %s, exception follows", p.first.c_str());
279  logger->log_error(name(), e);
280  }
281  break;
282  case NavGraphGeneratorInterface::CLOSEST_EDGE_OR_NODE:
283  logger->log_debug(name(),
284  " Connecting POI %s at (%f,%f) to closest edge or node",
285  p.first.c_str(),
286  p.second.position.x,
287  p.second.position.y);
288  navgraph->add_node_and_connect(node, NavGraph::CLOSEST_EDGE_OR_NODE);
289  break;
290  }
291  }
292 
293  // add edges
294  for (const auto &e : edges_) {
295  switch (e.edge_mode) {
296  case NavGraphGeneratorInterface::NO_INTERSECTION:
297  logger->log_debug(name(),
298  " Edge %s-%s%s (no intersection)",
299  e.p1.c_str(),
300  e.directed ? ">" : "-",
301  e.p2.c_str());
302  navgraph->add_edge(NavGraphEdge(e.p1, e.p2, e.directed), NavGraph::EDGE_NO_INTERSECTION);
303  break;
304 
305  case NavGraphGeneratorInterface::SPLIT_INTERSECTION:
306  logger->log_debug(name(),
307  " Edge %s-%s%s (split intersection)",
308  e.p1.c_str(),
309  e.directed ? ">" : "-",
310  e.p2.c_str());
311  navgraph->add_edge(NavGraphEdge(e.p1, e.p2, e.directed), NavGraph::EDGE_SPLIT_INTERSECTION);
312  break;
313 
314  case NavGraphGeneratorInterface::FORCE:
315  logger->log_debug(
316  name(), " Edge %s-%s%s (force)", e.p1.c_str(), e.directed ? ">" : "-", e.p2.c_str());
317  navgraph->add_edge(NavGraphEdge(e.p1, e.p2, e.directed), NavGraph::EDGE_FORCE);
318  break;
319  }
320  }
321 
322  /*
323  // Add POIs in free areas
324  unsigned int ci = 0;
325  const std::list<Polygon2D> &polygons = nggv.face_polygons();
326  for (const auto &p : polygons) {
327  Eigen::Vector2f centroid(polygon_centroid(p));
328  navgraph->add_node_and_connect(NavGraphNode(navgraph->format_name("AREA-%u", ++ci),
329  centroid.x(), centroid.y()),
330  NavGraph::CLOSEST_EDGE_OR_NODE);
331 
332  }
333  */
334 
335  // Finalize graph setup
336  try {
337  logger->log_debug(name(), " Calculate reachability relations");
338  navgraph->calc_reachability();
339  } catch (Exception &e) {
340  logger->log_error(name(), "Failed to finalize graph setup, exception follows");
341  logger->log_error(name(), e);
342  }
343 
344  if (cfg_save_to_file_) {
345  logger->log_debug(name(), " Writing to file '%s'", cfg_save_filename_.c_str());
346  save_yaml_navgraph(cfg_save_filename_, *navgraph);
347  }
348 
349  // re-enable notifications
350  navgraph->set_notifications_enabled(true);
351 
352  logger->log_debug(name(), " Graph computed, notifying listeners");
353  navgraph->notify_of_change();
354 
355  navgen_if_->set_ok(true);
356  navgen_if_->set_final(true);
357  navgen_if_->write();
358 
359 #ifdef HAVE_VISUALIZATION
360  if (cfg_visualization_)
361  publish_visualization();
362 #endif
363 }
364 
365 bool
366 NavGraphGeneratorThread::bb_interface_message_received(Interface *interface,
367  Message * message) throw()
368 {
369  // in continuous mode wait for signal if disabled
370  MutexLocker lock(loop_mutex);
371 
372  if (message->is_of_type<NavGraphGeneratorInterface::ClearMessage>()) {
373  pois_.clear();
374  obstacles_.clear();
375  map_obstacles_.clear();
376  edges_.clear();
377  default_properties_.clear();
378  bbox_set_ = false;
379  copy_default_properties_ = true;
381  algorithm_params_.clear();
382  filter_params_float_ = filter_params_float_defaults_;
383  for (auto &f : filter_) {
384  f.second = false;
385  }
386 
387  } else if (message->is_of_type<NavGraphGeneratorInterface::SetAlgorithmMessage>()) {
390 
391  algorithm_ = msg->algorithm();
392 
393  } else if (message->is_of_type<NavGraphGeneratorInterface::SetAlgorithmParameterMessage>()) {
396 
397  algorithm_params_[msg->param()] = msg->value();
398 
399  } else if (message->is_of_type<NavGraphGeneratorInterface::SetBoundingBoxMessage>()) {
402  bbox_set_ = true;
403  bbox_p1_.x = msg->p1_x();
404  bbox_p1_.y = msg->p1_y();
405  bbox_p2_.x = msg->p2_x();
406  bbox_p2_.y = msg->p2_y();
407 
408  } else if (message->is_of_type<NavGraphGeneratorInterface::SetFilterMessage>()) {
411 
412  filter_[navgen_if_->tostring_FilterType(msg->filter())] = msg->is_enable();
413 
414  } else if (message->is_of_type<NavGraphGeneratorInterface::SetFilterParamFloatMessage>()) {
417 
418  std::map<std::string, float> &param_float =
419  filter_params_float_[navgen_if_->tostring_FilterType(msg->filter())];
420 
421  if (param_float.find(msg->param()) != param_float.end()) {
422  param_float[msg->param()] = msg->value();
423  } else {
424  logger->log_warn(name(),
425  "Filter %s has no float parameter named %s, ignoring",
426  navgen_if_->tostring_FilterType(msg->filter()),
427  msg->param());
428  }
429 
430  } else if (message->is_of_type<NavGraphGeneratorInterface::AddMapObstaclesMessage>()) {
433  map_obstacles_ = map_obstacles(msg->max_line_point_distance());
434 
435  } else if (message->is_of_type<NavGraphGeneratorInterface::AddObstacleMessage>()) {
438  if (std::isfinite(msg->x()) && std::isfinite(msg->y())) {
439  obstacles_[msg->name()] = cart_coord_2d_t(msg->x(), msg->y());
440  } else {
441  logger->log_error(name(),
442  "Received non-finite obstacle (%.2f,%.2f), ignoring",
443  msg->x(),
444  msg->y());
445  }
446  } else if (message->is_of_type<NavGraphGeneratorInterface::RemoveObstacleMessage>()) {
449  ObstacleMap::iterator f;
450  if ((f = obstacles_.find(msg->name())) != obstacles_.end()) {
451  obstacles_.erase(f);
452  }
453 
454  } else if (message->is_of_type<NavGraphGeneratorInterface::AddPointOfInterestMessage>()) {
457  if (std::isfinite(msg->x()) && std::isfinite(msg->y())) {
458  PointOfInterest poi;
459  poi.position = cart_coord_2d_t(msg->x(), msg->y());
460  poi.conn_mode = msg->mode();
461  pois_[msg->name()] = poi;
462  } else {
463  logger->log_error(name(),
464  "Received non-finite POI (%.2f,%.2f), ignoring",
465  msg->x(),
466  msg->y());
467  }
468 
469  } else if (message->is_of_type<NavGraphGeneratorInterface::AddEdgeMessage>()) {
472  Edge edge;
473  edge.p1 = msg->p1();
474  edge.p2 = msg->p2();
475  edge.directed = msg->is_directed();
476  edge.edge_mode = msg->mode();
477  edges_.push_back(edge);
478 
479  } else if (message->is_of_type<NavGraphGeneratorInterface::AddPointOfInterestWithOriMessage>()) {
482 
483  if (std::isfinite(msg->x()) && std::isfinite(msg->y())) {
484  PointOfInterest poi;
485  poi.position = cart_coord_2d_t(msg->x(), msg->y());
486  poi.conn_mode = msg->mode();
487  if (std::isfinite(msg->ori())) {
488  poi.properties["orientation"] = std::to_string(msg->ori());
489  } else {
490  logger->log_warn(name(), "Received POI with non-finite ori %f, ignoring ori", msg->ori());
491  }
492  pois_[msg->name()] = poi;
493  } else {
494  logger->log_error(name(),
495  "Received non-finite POI (%.2f,%.2f), ignoring ori",
496  msg->x(),
497  msg->y());
498  }
499 
500  } else if (message->is_of_type<NavGraphGeneratorInterface::RemovePointOfInterestMessage>()) {
503  PoiMap::iterator f;
504  if ((f = pois_.find(msg->name())) != pois_.end()) {
505  pois_.erase(f);
506  }
507 
508  } else if (message->is_of_type<NavGraphGeneratorInterface::SetPointOfInterestPropertyMessage>()) {
511  PoiMap::iterator f;
512  if ((f = pois_.find(msg->name())) != pois_.end()) {
513  f->second.properties[msg->property_name()] = msg->property_value();
514  } else {
515  logger->log_warn(name(),
516  "POI %s unknown, cannot set property %s=%s",
517  msg->name(),
518  msg->property_name(),
519  msg->property_value());
520  }
521 
522  } else if (message->is_of_type<NavGraphGeneratorInterface::SetGraphDefaultPropertyMessage>()) {
525  default_properties_[msg->property_name()] = msg->property_value();
526 
527  } else if (message
528  ->is_of_type<NavGraphGeneratorInterface::SetCopyGraphDefaultPropertiesMessage>()) {
531  copy_default_properties_ = msg->is_enable_copy();
532 
533  } else if (message->is_of_type<NavGraphGeneratorInterface::ComputeMessage>()) {
534  navgen_if_->set_msgid(message->id());
535  navgen_if_->set_final(false);
536  navgen_if_->write();
537  wakeup();
538 
539  } else {
540  logger->log_error(name(), "Received unknown message of type %s, ignoring", message->type());
541  }
542 
543  return false;
544 }
545 
546 map_t *
547 NavGraphGeneratorThread::load_map(std::vector<std::pair<int, int>> &free_space_indices)
548 {
549  std::string cfg_map_file;
550  float cfg_resolution;
551  float cfg_origin_x;
552  float cfg_origin_y;
553  float cfg_origin_theta;
554  float cfg_occupied_thresh;
555  float cfg_free_thresh;
556 
557  fawkes::amcl::read_map_config(config,
558  cfg_map_file,
559  cfg_resolution,
560  cfg_origin_x,
561  cfg_origin_y,
562  cfg_origin_theta,
563  cfg_occupied_thresh,
564  cfg_free_thresh);
565 
566  return fawkes::amcl::read_map(cfg_map_file.c_str(),
567  cfg_origin_x,
568  cfg_origin_y,
569  cfg_resolution,
570  cfg_occupied_thresh,
571  cfg_free_thresh,
572  free_space_indices);
573 }
574 
575 NavGraphGeneratorThread::ObstacleMap
576 NavGraphGeneratorThread::map_obstacles(float line_max_dist)
577 {
578  ObstacleMap obstacles;
579  unsigned int obstacle_i = 0;
580 
581  std::vector<std::pair<int, int>> free_space_indices;
582  map_t * map = load_map(free_space_indices);
583 
584  logger->log_info(name(),
585  "Map Obstacles: map size: %ux%u (%zu of %u cells free, %.1f%%)",
586  map->size_x,
587  map->size_y,
588  free_space_indices.size(),
589  map->size_x * map->size_y,
590  (float)free_space_indices.size() / (float)(map->size_x * map->size_y) * 100.);
591 
592  size_t occ_cells = (size_t)map->size_x * map->size_y - free_space_indices.size();
593 
594  // convert map to point cloud
596  map_cloud->points.resize(occ_cells);
597  size_t pi = 0;
598  for (int x = 0; x < map->size_x; ++x) {
599  for (int y = 0; y < map->size_y; ++y) {
600  if (map->cells[MAP_INDEX(map, x, y)].occ_state > 0) {
601  // cell is occupied, generate point in cloud
602  pcl::PointXYZ p;
603  p.x = MAP_WXGX(map, x) + 0.5 * map->scale;
604  p.y = MAP_WYGY(map, y) + 0.5 * map->scale;
605  p.z = 0.;
606  map_cloud->points[pi++] = p;
607  }
608  }
609  }
610 
611  logger->log_info(name(), "Map Obstacles: filled %zu/%zu points", pi, occ_cells);
612 
614 
615  // determine lines
616  std::vector<LineInfo> linfos =
617  calc_lines<pcl::PointXYZ>(map_cloud,
618  cfg_map_line_segm_min_inliers_,
619  cfg_map_line_segm_max_iterations_,
620  /* segm distance threshold */ 2 * map->scale,
621  /* segm sample max dist */ 2 * map->scale,
622  cfg_map_line_cluster_tolerance_,
623  cfg_map_line_cluster_quota_,
624  cfg_map_line_min_length_,
625  -1,
626  -1,
627  -1,
628  no_line_cloud);
629 
630  logger->log_info(name(),
631  "Map Obstacles: found %zu lines, %zu points remaining",
632  linfos.size(),
633  no_line_cloud->points.size());
634 
635  // determine line obstacle points
636  for (const LineInfo &line : linfos) {
637  const unsigned int num_points = ceilf(line.length / line_max_dist);
638  float distribution = line.length / num_points;
639 
640  obstacles[NavGraph::format_name("Map_%u", ++obstacle_i)] =
641  cart_coord_2d_t(line.end_point_1[0], line.end_point_1[1]);
642  for (unsigned int i = 1; i <= num_points; ++i) {
643  Eigen::Vector3f p = line.end_point_1 + i * distribution * line.line_direction;
644  obstacles[NavGraph::format_name("Map_%d", ++obstacle_i)] = cart_coord_2d_t(p[0], p[1]);
645  }
646  }
647 
648  // cluster in remaining points to find more points of interest
649  pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree_cluster(new pcl::search::KdTree<pcl::PointXYZ>());
650 
651  std::vector<pcl::PointIndices> cluster_indices;
652  pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
653  ec.setClusterTolerance(2 * map->scale);
654  ec.setMinClusterSize(1);
655  ec.setMaxClusterSize(no_line_cloud->points.size());
656  ec.setSearchMethod(kdtree_cluster);
657  ec.setInputCloud(no_line_cloud);
658  ec.extract(cluster_indices);
659 
660  unsigned int i = 0;
661  for (auto cluster : cluster_indices) {
662  Eigen::Vector4f centroid;
663  pcl::compute3DCentroid(*no_line_cloud, cluster.indices, centroid);
664 
665  logger->log_info(name(),
666  "Map Obstacles: Cluster %u with %zu points at (%f, %f, %f)",
667  i,
668  cluster.indices.size(),
669  centroid.x(),
670  centroid.y(),
671  centroid.z());
672 
673  obstacles[NavGraph::format_name("MapCluster_%u", ++i)] =
674  cart_coord_2d_t(centroid.x(), centroid.y());
675  }
676 
677  map_free(map);
678 
679  return obstacles;
680 }
681 
682 void
683 NavGraphGeneratorThread::filter_edges_from_map(float max_dist)
684 {
685  std::vector<std::pair<int, int>> free_space_indices;
686  map_t * map = load_map(free_space_indices);
687 
688  const std::vector<NavGraphEdge> &edges = navgraph->edges();
689 
690  for (int x = 0; x < map->size_x; ++x) {
691  for (int y = 0; y < map->size_y; ++y) {
692  if (map->cells[MAP_INDEX(map, x, y)].occ_state > 0) {
693  // cell is occupied, generate point in cloud
694  Eigen::Vector2f gp;
695  gp[0] = MAP_WXGX(map, x) + 0.5 * map->scale;
696  gp[1] = MAP_WYGY(map, y) + 0.5 * map->scale;
697 
698  for (const NavGraphEdge &e : edges) {
699  try {
700  cart_coord_2d_t poe = e.closest_point_on_edge(gp[0], gp[1]);
701  Eigen::Vector2f p;
702  p[0] = poe.x;
703  p[1] = poe.y;
704  if ((gp - p).norm() <= max_dist) {
705  // edge too close, remove it!
706  logger->log_debug(name(),
707  " Removing edge (%s--%s), too close to occupied map cell (%f,%f)",
708  e.from().c_str(),
709  e.to().c_str(),
710  gp[0],
711  gp[1]);
712  navgraph->remove_edge(e);
713  break;
714  }
715  } catch (Exception &e) {
716  } // alright, not close
717  }
718  }
719  }
720  }
721  map_free(map);
722 }
723 
724 void
725 NavGraphGeneratorThread::filter_nodes_orphans()
726 {
727  const std::vector<NavGraphEdge> &edges = navgraph->edges();
728  const std::vector<NavGraphNode> &nodes = navgraph->nodes();
729 
730  std::list<NavGraphNode> remove_nodes;
731 
732  for (const NavGraphNode &n : nodes) {
733  std::string nname = n.name();
734  std::vector<NavGraphEdge>::const_iterator e =
735  std::find_if(edges.begin(), edges.end(), [nname](const NavGraphEdge &e) -> bool {
736  return (e.from() == nname || e.to() == nname);
737  });
738  if (e == edges.end() && !n.unconnected()) {
739  // node is not connected to any other node -> remove
740  remove_nodes.push_back(n);
741  }
742  }
743 
744  for (const NavGraphNode &n : remove_nodes) {
745  logger->log_debug(name(), " Removing unconnected node %s", n.name().c_str());
746  navgraph->remove_node(n);
747  }
748 }
749 
750 void
751 NavGraphGeneratorThread::filter_multi_graph()
752 {
753  navgraph->calc_reachability(/* allow multi graph*/ true);
754 
755  std::list<std::set<std::string>> graphs;
756 
757  const std::vector<NavGraphNode> &nodes = navgraph->nodes();
758  std::set<std::string> nodeset;
759  std::for_each(nodes.begin(), nodes.end(), [&nodeset](const NavGraphNode &n) {
760  nodeset.insert(n.name());
761  });
762 
763  while (!nodeset.empty()) {
764  std::queue<std::string> q;
765  q.push(*nodeset.begin());
766 
767  std::set<std::string> traversed;
768 
769  while (!q.empty()) {
770  std::string &nname = q.front();
771  traversed.insert(nname);
772 
773  NavGraphNode n = navgraph->node(nname);
774  if (n) {
775  const std::vector<std::string> &reachable = n.reachable_nodes();
776 
777  for (const std::string &r : reachable) {
778  if (traversed.find(r) == traversed.end())
779  q.push(r);
780  }
781  }
782  q.pop();
783  }
784 
785  std::set<std::string> nodediff;
786  std::set_difference(nodeset.begin(),
787  nodeset.end(),
788  traversed.begin(),
789  traversed.end(),
790  std::inserter(nodediff, nodediff.begin()));
791  graphs.push_back(traversed);
792  nodeset = nodediff;
793  }
794 
795  // reverse sort, largest set first
796  graphs.sort([](const std::set<std::string> &a, const std::set<std::string> &b) -> bool {
797  return b.size() < a.size();
798  });
799 
800  std::for_each(std::next(graphs.begin()), graphs.end(), [&](const std::set<std::string> &g) {
801  logger->log_debug(name(),
802  " Removing disconnected sub-graph [%s]",
803  str_join(g.begin(), g.end(), ", ").c_str());
804  for (const std::string &n : g)
805  navgraph->remove_node(n);
806  });
807 }
808 
809 #ifdef HAVE_VISUALIZATION
810 void
811 NavGraphGeneratorThread::publish_visualization()
812 {
813  if (vt_) {
814  vt_->publish(obstacles_, map_obstacles_, pois_);
815  }
816 }
817 #endif
fawkes::NavGraphGeneratorVoronoi
Generate navgraph using a Voronoi diagram.
Definition: voronoi.h:33
fawkes::NavGraphGenerator::set_bounding_box
virtual void set_bounding_box(float bbox_p1_x, float bbox_p1_y, float bbox_p2_x, float bbox_p2_y)
Set bounding box.
Definition: generator.cpp:111
fawkes::NavGraphNode
Topological graph node.
Definition: navgraph_node.h:36
fawkes::MultiLogger::log_error
virtual void log_error(const char *component, const char *format,...)
Log error message.
Definition: multi.cpp:237
fawkes::NavGraphGeneratorInterface::AddPointOfInterestWithOriMessage
AddPointOfInterestWithOriMessage Fawkes BlackBoard Interface Message.
Definition: NavGraphGeneratorInterface.h:534
fawkes::NavGraphGeneratorInterface::SetGraphDefaultPropertyMessage::property_value
char * property_value() const
Get property_value value.
Definition: NavGraphGeneratorInterface.cpp:2748
fawkes::NavGraphGeneratorInterface::AddPointOfInterestWithOriMessage::name
char * name() const
Get name value.
Definition: NavGraphGeneratorInterface.cpp:2050
fawkes::NavGraphGeneratorInterface::AddPointOfInterestWithOriMessage::x
float x() const
Get x value.
Definition: NavGraphGeneratorInterface.cpp:2082
fawkes::NavGraphGeneratorInterface::AddPointOfInterestMessage::x
float x() const
Get x value.
Definition: NavGraphGeneratorInterface.cpp:1851
fawkes::NavGraphGeneratorInterface::RemoveObstacleMessage
RemoveObstacleMessage Fawkes BlackBoard Interface Message.
Definition: NavGraphGeneratorInterface.h:458
fawkes::NavGraphGeneratorInterface::SetBoundingBoxMessage::p1_x
float p1_x() const
Get p1_x value.
Definition: NavGraphGeneratorInterface.cpp:821
fawkes::NavGraphNode::set_unconnected
void set_unconnected(bool unconnected)
Set unconnected state of the node.
Definition: navgraph_node.cpp:104
fawkes::BlackBoard::register_listener
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
Definition: blackboard.cpp:185
fawkes::NavGraphGeneratorInterface::SetAlgorithmParameterMessage::value
char * value() const
Get value value.
Definition: NavGraphGeneratorInterface.cpp:690
fawkes::NavGraphGeneratorInterface::AddPointOfInterestWithOriMessage::mode
ConnectionMode mode() const
Get mode value.
Definition: NavGraphGeneratorInterface.cpp:2174
fawkes::NavGraphGeneratorInterface::AddPointOfInterestMessage::name
char * name() const
Get name value.
Definition: NavGraphGeneratorInterface.cpp:1819
fawkes::NavGraphGeneratorInterface::SetAlgorithmMessage::algorithm
Algorithm algorithm() const
Get algorithm value.
Definition: NavGraphGeneratorInterface.cpp:531
fawkes::NavGraphGeneratorInterface::set_error_message
void set_error_message(const char *new_error_message)
Set error_message value.
Definition: NavGraphGeneratorInterface.cpp:308
fawkes::NavGraphGeneratorInterface::AddObstacleMessage::x
float x() const
Get x value.
Definition: NavGraphGeneratorInterface.cpp:1537
fawkes::NavGraphGeneratorInterface::SetFilterParamFloatMessage::param
char * param() const
Get param value.
Definition: NavGraphGeneratorInterface.cpp:1223
fawkes::NavGraphGeneratorInterface::AddObstacleMessage::y
float y() const
Get y value.
Definition: NavGraphGeneratorInterface.cpp:1567
fawkes::Message
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:45
fawkes::NavGraphGenerator::compute
virtual void compute(fawkes::LockPtr< fawkes::NavGraph > graph)=0
Compute graph.
fawkes::Configuration::get_bool
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
fawkes::NavGraphGeneratorInterface::SetGraphDefaultPropertyMessage
SetGraphDefaultPropertyMessage Fawkes BlackBoard Interface Message.
Definition: NavGraphGeneratorInterface.h:667
fawkes::NavGraphEdge
Topological graph edge.
Definition: navgraph_edge.h:38
fawkes::NavGraphGenerator::add_obstacle
virtual void add_obstacle(float x, float y)
Add an obstacle point.
Definition: generator.cpp:141
NavGraphGeneratorThread::NavGraphGeneratorThread
NavGraphGeneratorThread()
Constructor.
Definition: navgraph_generator_thread.cpp:44
fawkes::Logger::log_info
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
fawkes::MutexLocker
Mutex locking helper.
Definition: mutex_locker.h:34
fawkes::BlackBoard::unregister_listener
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
Definition: blackboard.cpp:212
fawkes::BlackBoardInterfaceListener
BlackBoard interface listener.
Definition: interface_listener.h:42
fawkes::NavGraphGeneratorInterface::SetPointOfInterestPropertyMessage::name
char * name() const
Get name value.
Definition: NavGraphGeneratorInterface.cpp:2305
fawkes::NavGraphGeneratorInterface::AddObstacleMessage::name
char * name() const
Get name value.
Definition: NavGraphGeneratorInterface.cpp:1505
fawkes::NavGraphGeneratorInterface::AddPointOfInterestWithOriMessage::ori
float ori() const
Get ori value.
Definition: NavGraphGeneratorInterface.cpp:2142
fawkes::NavGraphGeneratorGrid
Generate navgraph using a Grid diagram.
Definition: grid.h:33
NavGraphGeneratorThread::finalize
virtual void finalize()
Finalize the thread.
Definition: navgraph_generator_thread.cpp:131
fawkes::NavGraphGeneratorInterface::SetCopyGraphDefaultPropertiesMessage
SetCopyGraphDefaultPropertiesMessage Fawkes BlackBoard Interface Message.
Definition: NavGraphGeneratorInterface.h:701
NavGraphGeneratorThread::init
virtual void init()
Initialize the thread.
Definition: navgraph_generator_thread.cpp:69
fawkes::Thread::name
const char * name() const
Get name of thread.
Definition: thread.h:100
fawkes::NavGraphGeneratorInterface::ClearMessage
ClearMessage Fawkes BlackBoard Interface Message.
Definition: NavGraphGeneratorInterface.h:180
fawkes::NavGraphGeneratorInterface::ALGORITHM_GRID
@ ALGORITHM_GRID
Grid-based algorithm with customizable spacing.
Definition: NavGraphGeneratorInterface.h:133
fawkes::cart_coord_2d_struct::y
float y
y coordinate
Definition: types.h:67
fawkes::NavGraphGeneratorInterface::SetBoundingBoxMessage::p2_x
float p2_x() const
Get p2_x value.
Definition: NavGraphGeneratorInterface.cpp:881
fawkes::NavGraphGeneratorInterface::AddEdgeMessage::is_directed
bool is_directed() const
Get directed value.
Definition: NavGraphGeneratorInterface.cpp:2562
fawkes::NavGraphGeneratorInterface::AddEdgeMessage::mode
EdgeMode mode() const
Get mode value.
Definition: NavGraphGeneratorInterface.cpp:2595
fawkes::MultiLogger::log_warn
virtual void log_warn(const char *component, const char *format,...)
Log warning message.
Definition: multi.cpp:216
fawkes::NavGraphGeneratorInterface::SetFilterMessage::filter
FilterType filter() const
Get filter value.
Definition: NavGraphGeneratorInterface.cpp:1032
fawkes::NavGraphGeneratorInterface::RemovePointOfInterestMessage
RemovePointOfInterestMessage Fawkes BlackBoard Interface Message.
Definition: NavGraphGeneratorInterface.h:731
fawkes::LoggingAspect::logger
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
fawkes::NavGraphGeneratorInterface::SetBoundingBoxMessage::p2_y
float p2_y() const
Get p2_y value.
Definition: NavGraphGeneratorInterface.cpp:911
fawkes::BlackBoard::close
virtual void close(Interface *interface)=0
Close interface.
fawkes::save_yaml_navgraph
void save_yaml_navgraph(std::string filename, NavGraph *graph)
Save navgraph to YAML file.
Definition: yaml_navgraph.cpp:386
fawkes::Logger::log_error
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
fawkes::NavGraphGeneratorInterface::AddPointOfInterestMessage::y
float y() const
Get y value.
Definition: NavGraphGeneratorInterface.cpp:1881
fawkes::NavGraphGeneratorInterface::ALGORITHM_VORONOI
@ ALGORITHM_VORONOI
Voronoi-based algorithm for navgraph generation.
Definition: NavGraphGeneratorInterface.h:129
fawkes::NavGraphGeneratorInterface::SetFilterParamFloatMessage::value
float value() const
Get value value.
Definition: NavGraphGeneratorInterface.cpp:1255
fawkes::NavGraphGeneratorInterface::AddPointOfInterestMessage
AddPointOfInterestMessage Fawkes BlackBoard Interface Message.
Definition: NavGraphGeneratorInterface.h:489
NavGraphGeneratorVisualizationThread
Send Marker messages to rviz to show navgraph-generator info.
Definition: visualization_thread.h:39
fawkes
Fawkes library namespace.
fawkes::NavGraphGeneratorInterface::SetGraphDefaultPropertyMessage::property_name
char * property_name() const
Get property_name value.
Definition: NavGraphGeneratorInterface.cpp:2717
fawkes::NavGraphNode::reachable_nodes
const std::vector< std::string > & reachable_nodes() const
Get reachable nodes.
Definition: navgraph_node.h:206
LineInfo
Line information container.
Definition: line_info.h:40
NavGraphGeneratorThread::loop
virtual void loop()
Code to execute in the thread.
Definition: navgraph_generator_thread.cpp:139
fawkes::BlackBoardInterfaceListener::bbil_remove_message_interface
void bbil_remove_message_interface(Interface *interface)
Remove an interface to the message received watch list.
Definition: interface_listener.cpp:292
fawkes::NavGraphGeneratorInterface::AddEdgeMessage::p1
char * p1() const
Get p1 value.
Definition: NavGraphGeneratorInterface.cpp:2499
pcl::PointCloud
Definition: pointcloud.h:32
fawkes::NavGraphGeneratorInterface::set_ok
void set_ok(const bool new_ok)
Set ok value.
Definition: NavGraphGeneratorInterface.cpp:272
fawkes::Logger::log_warn
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
fawkes::NavGraphGeneratorInterface::SetPointOfInterestPropertyMessage::property_name
char * property_name() const
Get property_name value.
Definition: NavGraphGeneratorInterface.cpp:2337
fawkes::NavGraphGeneratorInterface::AddEdgeMessage::p2
char * p2() const
Get p2 value.
Definition: NavGraphGeneratorInterface.cpp:2529
fawkes::NavGraphGeneratorInterface::AddMapObstaclesMessage::max_line_point_distance
float max_line_point_distance() const
Get max_line_point_distance value.
Definition: NavGraphGeneratorInterface.cpp:1375
fawkes::BlackBoardInterfaceListener::bbil_add_message_interface
void bbil_add_message_interface(Interface *interface)
Add an interface to the message received watch list.
Definition: interface_listener.cpp:241
fawkes::Interface
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:79
fawkes::NavGraphGeneratorInterface::AddMapObstaclesMessage
AddMapObstaclesMessage Fawkes BlackBoard Interface Message.
Definition: NavGraphGeneratorInterface.h:387
fawkes::ConfigurableAspect::config
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
fawkes::NavGraphGeneratorInterface::AddPointOfInterestWithOriMessage::y
float y() const
Get y value.
Definition: NavGraphGeneratorInterface.cpp:2112
fawkes::NavGraphGeneratorInterface::AddPointOfInterestMessage::mode
ConnectionMode mode() const
Get mode value.
Definition: NavGraphGeneratorInterface.cpp:1913
fawkes::NavGraphGeneratorInterface::ComputeMessage
ComputeMessage Fawkes BlackBoard Interface Message.
Definition: NavGraphGeneratorInterface.h:762
fawkes::Exception::what_no_backtrace
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
Definition: exception.cpp:663
fawkes::cart_coord_2d_struct
Cartesian coordinates (2D).
Definition: types.h:65
fawkes::cart_coord_2d_struct::x
float x
x coordinate
Definition: types.h:66
fawkes::NavGraphGeneratorInterface::SetAlgorithmMessage
SetAlgorithmMessage Fawkes BlackBoard Interface Message.
Definition: NavGraphGeneratorInterface.h:204
fawkes::NavGraphGeneratorInterface::AddEdgeMessage
AddEdgeMessage Fawkes BlackBoard Interface Message.
Definition: NavGraphGeneratorInterface.h:623
fawkes::NavGraphGeneratorInterface::RemoveObstacleMessage::name
char * name() const
Get name value.
Definition: NavGraphGeneratorInterface.cpp:1686
fawkes::NavGraphGeneratorInterface::SetFilterMessage
SetFilterMessage Fawkes BlackBoard Interface Message.
Definition: NavGraphGeneratorInterface.h:312
fawkes::NavGraphGeneratorInterface::RemovePointOfInterestMessage::name
char * name() const
Get name value.
Definition: NavGraphGeneratorInterface.cpp:2986
fawkes::Configuration::get_float
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
fawkes::NavGraphGeneratorInterface::SetFilterMessage::is_enable
bool is_enable() const
Get enable value.
Definition: NavGraphGeneratorInterface.cpp:1064
fawkes::Thread
Thread class encapsulation of pthreads.
Definition: thread.h:46
fawkes::Message::as_type
MessageType * as_type()
Cast message to given type if possible.
Definition: message.h:150
fawkes::BlackBoardAspect::blackboard
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
fawkes::NavGraphAspect::navgraph
fawkes::LockPtr< NavGraph > navgraph
NavGraph instance shared in framework.
Definition: navgraph.h:44
fawkes::Configuration::get_uint
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
fawkes::Configuration::get_string
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
fawkes::NavGraphGeneratorInterface::SetFilterParamFloatMessage::filter
FilterType filter() const
Get filter value.
Definition: NavGraphGeneratorInterface.cpp:1191
fawkes::NavGraphGeneratorInterface::SetAlgorithmParameterMessage
SetAlgorithmParameterMessage Fawkes BlackBoard Interface Message.
Definition: NavGraphGeneratorInterface.h:233
fawkes::NavGraphGeneratorInterface::SetCopyGraphDefaultPropertiesMessage::is_enable_copy
bool is_enable_copy() const
Get enable_copy value.
Definition: NavGraphGeneratorInterface.cpp:2866
fawkes::NavGraphGeneratorInterface::AddObstacleMessage
AddObstacleMessage Fawkes BlackBoard Interface Message.
Definition: NavGraphGeneratorInterface.h:419
fawkes::NavGraphGeneratorInterface::tostring_Algorithm
const char * tostring_Algorithm(Algorithm value) const
Convert Algorithm constant to string.
Definition: NavGraphGeneratorInterface.cpp:158
fawkes::Configuration::exists
virtual bool exists(const char *path)=0
Check if a given value exists.
fawkes::NavGraphGeneratorInterface::SetPointOfInterestPropertyMessage
SetPointOfInterestPropertyMessage Fawkes BlackBoard Interface Message.
Definition: NavGraphGeneratorInterface.h:583
fawkes::Logger::log_debug
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
fawkes::Interface::write
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:494
fawkes::NavGraphGeneratorInterface::SetAlgorithmParameterMessage::param
char * param() const
Get param value.
Definition: NavGraphGeneratorInterface.cpp:655
fawkes::BlackBoard::open_for_writing
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
fawkes::NavGraphGeneratorInterface::set_final
void set_final(const bool new_final)
Set final value.
Definition: NavGraphGeneratorInterface.cpp:236
fawkes::NavGraphGeneratorInterface::SetPointOfInterestPropertyMessage::property_value
char * property_value() const
Get property_value value.
Definition: NavGraphGeneratorInterface.cpp:2368
fawkes::NavGraphGeneratorInterface
NavGraphGeneratorInterface Fawkes BlackBoard Interface.
Definition: NavGraphGeneratorInterface.h:34
fawkes::NavGraphGeneratorInterface::SetBoundingBoxMessage
SetBoundingBoxMessage Fawkes BlackBoard Interface Message.
Definition: NavGraphGeneratorInterface.h:271
NavGraphGeneratorThread::~NavGraphGeneratorThread
virtual ~NavGraphGeneratorThread()
Destructor.
Definition: navgraph_generator_thread.cpp:64
fawkes::NavGraphGeneratorInterface::SetBoundingBoxMessage::p1_y
float p1_y() const
Get p1_y value.
Definition: NavGraphGeneratorInterface.cpp:851
fawkes::NavGraphGeneratorInterface::SetFilterParamFloatMessage
SetFilterParamFloatMessage Fawkes BlackBoard Interface Message.
Definition: NavGraphGeneratorInterface.h:347
fawkes::Exception
Base class for exceptions in Fawkes.
Definition: exception.h:36