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
34using 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
68void
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
130void
132{
135 blackboard->close(navgen_if_);
136}
137
138void
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) {
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
163 "Calculating new graph (%s)",
164 navgen_if_->tostring_Algorithm(algorithm_));
165
166 if (bbox_set_) {
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_) {
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_) {
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:
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:
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:
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 {
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:
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:
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:
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:
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
365bool
366NavGraphGeneratorThread::bb_interface_message_received(Interface *interface,
367 Message * message) noexcept
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
546map_t *
547NavGraphGeneratorThread::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
575NavGraphGeneratorThread::ObstacleMap
576NavGraphGeneratorThread::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
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
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
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
682void
683NavGraphGeneratorThread::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!
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
724void
725NavGraphGeneratorThread::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
750void
751NavGraphGeneratorThread::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
810void
811NavGraphGeneratorThread::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 noexcept
Get primary string (does not implicitly print the back trace).
Definition: exception.cpp:663
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:80
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:501
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:44
MessageType * as_type()
Cast message to given type if possible.
Definition: message.h:168
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
void set_unconnected(bool unconnected)
Set unconnected state of the node.
const std::vector< std::string > & reachable_nodes() const
Get reachable nodes.
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