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
Line information container.
Definition: line_info.h:40
virtual void finalize()
Finalize the thread.
virtual void init()
Initialize the thread.
virtual ~NavGraphGeneratorThread()
Destructor.
virtual void loop()
Code to execute in the thread.
Send Marker messages to rviz to show navgraph-generator info.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
BlackBoard interface listener.
void bbil_add_message_interface(Interface *interface)
Add an interface to the message received watch list.
void bbil_remove_message_interface(Interface *interface)
Remove an interface to the message received watch list.
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
Definition: blackboard.cpp:212
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
Definition: blackboard.cpp:185
virtual void close(Interface *interface)=0
Close interface.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual bool exists(const char *path)=0
Check if a given value exists.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
Definition: exception.cpp:663
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:79
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:494
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
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.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:45
MessageType * as_type()
Cast message to given type if possible.
Definition: message.h:150
virtual void log_warn(const char *component, const char *format,...)
Log warning message.
Definition: multi.cpp:216
virtual void log_error(const char *component, const char *format,...)
Log error message.
Definition: multi.cpp:237
Mutex locking helper.
Definition: mutex_locker.h:34
fawkes::LockPtr< NavGraph > navgraph
NavGraph instance shared in framework.
Definition: navgraph.h:44
Topological graph edge.
Definition: navgraph_edge.h:38
Generate navgraph using a Grid diagram.
Definition: grid.h:33
AddEdgeMessage Fawkes BlackBoard Interface Message.
AddMapObstaclesMessage Fawkes BlackBoard Interface Message.
float max_line_point_distance() const
Get max_line_point_distance value.
AddObstacleMessage Fawkes BlackBoard Interface Message.
AddPointOfInterestMessage Fawkes BlackBoard Interface Message.
AddPointOfInterestWithOriMessage Fawkes BlackBoard Interface Message.
ClearMessage Fawkes BlackBoard Interface Message.
ComputeMessage Fawkes BlackBoard Interface Message.
RemoveObstacleMessage Fawkes BlackBoard Interface Message.
RemovePointOfInterestMessage Fawkes BlackBoard Interface Message.
SetAlgorithmMessage Fawkes BlackBoard Interface Message.
SetAlgorithmParameterMessage Fawkes BlackBoard Interface Message.
SetBoundingBoxMessage Fawkes BlackBoard Interface Message.
SetCopyGraphDefaultPropertiesMessage Fawkes BlackBoard Interface Message.
SetFilterMessage Fawkes BlackBoard Interface Message.
SetFilterParamFloatMessage Fawkes BlackBoard Interface Message.
SetGraphDefaultPropertyMessage Fawkes BlackBoard Interface Message.
SetPointOfInterestPropertyMessage Fawkes BlackBoard Interface Message.
NavGraphGeneratorInterface Fawkes BlackBoard Interface.
void set_ok(const bool new_ok)
Set ok value.
void set_error_message(const char *new_error_message)
Set error_message value.
const char * tostring_Algorithm(Algorithm value) const
Convert Algorithm constant to string.
@ ALGORITHM_VORONOI
Voronoi-based algorithm for navgraph generation.
@ ALGORITHM_GRID
Grid-based algorithm with customizable spacing.
void set_final(const bool new_final)
Set final value.
Generate navgraph using a Voronoi diagram.
Definition: voronoi.h:33
Topological graph node.
Definition: navgraph_node.h:36
const std::vector< std::string > & reachable_nodes() const
Get reachable nodes.
void set_unconnected(bool unconnected)
Set unconnected state of the node.
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
Fawkes library namespace.
void save_yaml_navgraph(std::string filename, NavGraph *graph)
Save navgraph to YAML file.
Cartesian coordinates (2D).
Definition: types.h:65
float y
y coordinate
Definition: types.h:67
float x
x coordinate
Definition: types.h:66