21#include "navgraph_generator_thread.h"
22#ifdef HAVE_VISUALIZATION
23# include "visualization_thread.h"
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>
36#define CFG_PREFIX "/navgraph-generator/"
45:
Thread(
"NavGraphGeneratorThread",
Thread::OPMODE_WAITFORWAKEUP),
48#ifdef HAVE_VISUALIZATION
53#ifdef HAVE_VISUALIZATION
56:
Thread(
"NavGraphGeneratorThread",
Thread::OPMODE_WAITFORWAKEUP),
72 copy_default_properties_ =
true;
74 filter_[
"FILTER_EDGES_BY_MAP"] =
false;
75 filter_[
"FILTER_ORPHAN_NODES"] =
false;
76 filter_[
"FILTER_MULTI_GRAPH"] =
false;
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"] =
84 filter_params_float_ = filter_params_float_defaults_;
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");
95 cfg_visualization_ =
false;
97 cfg_visualization_ =
config->
get_bool(CFG_PREFIX
"visualization/enable");
101 cfg_save_to_file_ =
false;
103 cfg_save_to_file_ =
config->
get_bool(CFG_PREFIX
"save-to-file/enable");
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");
111 if (cfg_save_filename_.find(
"..") != std::string::npos) {
112 throw Exception(
"navgraph-generator: filename may not contains two consecutive dots (..)");
114 if (cfg_save_filename_[0] !=
'/') {
115 cfg_save_filename_ = std::string(CONFDIR) +
"/" + cfg_save_filename_;
119#ifndef HAVE_VISUALIZATION
120 if (cfg_visualization_) {
141 std::shared_ptr<NavGraphGenerator> ng;
144 switch (algorithm_) {
152 "Failed to initialize algorithm %s, exception follows",
155 navgen_if_->
set_ok(
false);
163 "Calculating new graph (%s)",
168 " Setting bound box (%f,%f) to (%f,%f)",
173 ng->set_bounding_box(bbox_p1_.
x, bbox_p1_.
y, bbox_p2_.
x, bbox_p2_.
y);
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);
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);
193 navgraph->set_notifications_enabled(
false);
196 std::map<std::string, std::string> default_props =
navgraph->default_properties();
201 if (copy_default_properties_) {
202 navgraph->set_default_properties(default_props);
206 for (
auto p : default_properties_) {
207 navgraph->set_default_property(p.first, p.second);
216 navgen_if_->
set_ok(
false);
219 navgraph->set_notifications_enabled(
true);
224 if (filter_[
"FILTER_EDGES_BY_MAP"]) {
226 filter_edges_from_map(filter_params_float_[
"FILTER_EDGES_BY_MAP"][
"distance"]);
228 if (filter_[
"FILTER_ORPHAN_NODES"]) {
230 filter_nodes_orphans();
232 if (filter_[
"FILTER_MULTI_GRAPH"]) {
234 filter_multi_graph();
238 for (
const auto &p : pois_) {
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)",
247 p.second.position.y);
251 case NavGraphGeneratorInterface::UNCONNECTED:
253 " Unconnected POI %s at (%f,%f)",
256 p.second.position.y);
261 case NavGraphGeneratorInterface::CLOSEST_NODE:
263 " Connecting POI %s at (%f,%f) to closest node",
266 p.second.position.y);
267 navgraph->add_node_and_connect(node, NavGraph::CLOSEST_NODE);
269 case NavGraphGeneratorInterface::CLOSEST_EDGE:
272 " Connecting POI %s at (%f,%f) to closest edge",
275 p.second.position.y);
276 navgraph->add_node_and_connect(node, NavGraph::CLOSEST_EDGE);
282 case NavGraphGeneratorInterface::CLOSEST_EDGE_OR_NODE:
284 " Connecting POI %s at (%f,%f) to closest edge or node",
287 p.second.position.y);
288 navgraph->add_node_and_connect(node, NavGraph::CLOSEST_EDGE_OR_NODE);
294 for (
const auto &e : edges_) {
295 switch (e.edge_mode) {
296 case NavGraphGeneratorInterface::NO_INTERSECTION:
298 " Edge %s-%s%s (no intersection)",
300 e.directed ?
">" :
"-",
305 case NavGraphGeneratorInterface::SPLIT_INTERSECTION:
307 " Edge %s-%s%s (split intersection)",
309 e.directed ?
">" :
"-",
314 case NavGraphGeneratorInterface::FORCE:
316 name(),
" Edge %s-%s%s (force)", e.p1.c_str(), e.directed ?
">" :
"-", e.p2.c_str());
344 if (cfg_save_to_file_) {
350 navgraph->set_notifications_enabled(
true);
359#ifdef HAVE_VISUALIZATION
360 if (cfg_visualization_)
361 publish_visualization();
366NavGraphGeneratorThread::bb_interface_message_received(
Interface *interface,
375 map_obstacles_.clear();
377 default_properties_.clear();
379 copy_default_properties_ =
true;
381 algorithm_params_.clear();
382 filter_params_float_ = filter_params_float_defaults_;
383 for (
auto &f : filter_) {
397 algorithm_params_[msg->
param()] = msg->
value();
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();
412 filter_[navgen_if_->tostring_FilterType(msg->
filter())] = msg->
is_enable();
418 std::map<std::string, float> ¶m_float =
419 filter_params_float_[navgen_if_->tostring_FilterType(msg->
filter())];
421 if (param_float.find(msg->
param()) != param_float.end()) {
425 "Filter %s has no float parameter named %s, ignoring",
426 navgen_if_->tostring_FilterType(msg->
filter()),
438 if (std::isfinite(msg->
x()) && std::isfinite(msg->
y())) {
442 "Received non-finite obstacle (%.2f,%.2f), ignoring",
449 ObstacleMap::iterator f;
450 if ((f = obstacles_.find(msg->
name())) != obstacles_.end()) {
457 if (std::isfinite(msg->
x()) && std::isfinite(msg->
y())) {
460 poi.conn_mode = msg->
mode();
461 pois_[msg->
name()] = poi;
464 "Received non-finite POI (%.2f,%.2f), ignoring",
476 edge.edge_mode = msg->
mode();
477 edges_.push_back(edge);
483 if (std::isfinite(msg->
x()) && std::isfinite(msg->
y())) {
486 poi.conn_mode = msg->
mode();
487 if (std::isfinite(msg->
ori())) {
488 poi.properties[
"orientation"] = std::to_string(msg->
ori());
490 logger->
log_warn(name(),
"Received POI with non-finite ori %f, ignoring ori", msg->
ori());
492 pois_[msg->
name()] = poi;
495 "Received non-finite POI (%.2f,%.2f), ignoring ori",
504 if ((f = pois_.find(msg->
name())) != pois_.end()) {
512 if ((f = pois_.find(msg->
name())) != pois_.end()) {
516 "POI %s unknown, cannot set property %s=%s",
528 ->is_of_type<NavGraphGeneratorInterface::SetCopyGraphDefaultPropertiesMessage>()) {
534 navgen_if_->set_msgid(message->id());
535 navgen_if_->set_final(
false);
540 logger->
log_error(name(),
"Received unknown message of type %s, ignoring", message->type());
547NavGraphGeneratorThread::load_map(std::vector<std::pair<int, int>> &free_space_indices)
549 std::string cfg_map_file;
550 float cfg_resolution;
553 float cfg_origin_theta;
554 float cfg_occupied_thresh;
555 float cfg_free_thresh;
557 fawkes::amcl::read_map_config(
config,
566 return fawkes::amcl::read_map(cfg_map_file.c_str(),
575NavGraphGeneratorThread::ObstacleMap
576NavGraphGeneratorThread::map_obstacles(
float line_max_dist)
578 ObstacleMap obstacles;
579 unsigned int obstacle_i = 0;
581 std::vector<std::pair<int, int>> free_space_indices;
582 map_t * map = load_map(free_space_indices);
585 "Map Obstacles: map size: %ux%u (%zu of %u cells free, %.1f%%)",
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.);
592 size_t occ_cells = (size_t)map->size_x * map->size_y - free_space_indices.size();
596 map_cloud->points.resize(occ_cells);
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) {
603 p.x = MAP_WXGX(map, x) + 0.5 * map->scale;
604 p.y = MAP_WYGY(map, y) + 0.5 * map->scale;
606 map_cloud->points[pi++] = p;
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_,
622 cfg_map_line_cluster_tolerance_,
623 cfg_map_line_cluster_quota_,
624 cfg_map_line_min_length_,
631 "Map Obstacles: found %zu lines, %zu points remaining",
633 no_line_cloud->points.size());
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;
640 obstacles[NavGraph::format_name(
"Map_%u", ++obstacle_i)] =
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]);
649 pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree_cluster(
new pcl::search::KdTree<pcl::PointXYZ>());
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);
661 for (
auto cluster : cluster_indices) {
662 Eigen::Vector4f centroid;
663 pcl::compute3DCentroid(*no_line_cloud, cluster.indices, centroid);
666 "Map Obstacles: Cluster %u with %zu points at (%f, %f, %f)",
668 cluster.indices.size(),
673 obstacles[NavGraph::format_name(
"MapCluster_%u", ++i)] =
683NavGraphGeneratorThread::filter_edges_from_map(
float max_dist)
685 std::vector<std::pair<int, int>> free_space_indices;
686 map_t * map = load_map(free_space_indices);
688 const std::vector<NavGraphEdge> &edges =
navgraph->edges();
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) {
695 gp[0] = MAP_WXGX(map, x) + 0.5 * map->scale;
696 gp[1] = MAP_WYGY(map, y) + 0.5 * map->scale;
704 if ((gp - p).norm() <= max_dist) {
707 " Removing edge (%s--%s), too close to occupied map cell (%f,%f)",
725NavGraphGeneratorThread::filter_nodes_orphans()
727 const std::vector<NavGraphEdge> &edges =
navgraph->edges();
728 const std::vector<NavGraphNode> &nodes =
navgraph->nodes();
730 std::list<NavGraphNode> remove_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);
738 if (e == edges.end() && !n.unconnected()) {
740 remove_nodes.push_back(n);
751NavGraphGeneratorThread::filter_multi_graph()
755 std::list<std::set<std::string>> graphs;
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());
763 while (!nodeset.empty()) {
764 std::queue<std::string> q;
765 q.push(*nodeset.begin());
767 std::set<std::string> traversed;
770 std::string &nname = q.front();
771 traversed.insert(nname);
777 for (
const std::string &r : reachable) {
778 if (traversed.find(r) == traversed.end())
785 std::set<std::string> nodediff;
786 std::set_difference(nodeset.begin(),
790 std::inserter(nodediff, nodediff.begin()));
791 graphs.push_back(traversed);
796 graphs.sort([](
const std::set<std::string> &a,
const std::set<std::string> &b) ->
bool {
797 return b.size() < a.size();
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);
809#ifdef HAVE_VISUALIZATION
811NavGraphGeneratorThread::publish_visualization()
814 vt_->publish(obstacles_, map_obstacles_, pois_);
Line information container.
virtual void finalize()
Finalize the thread.
virtual void init()
Initialize the thread.
NavGraphGeneratorThread()
Constructor.
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.
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.
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.
virtual void close(Interface *interface)=0
Close interface.
Configuration * config
This is the Configuration member used to access the configuration.
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.
virtual const char * what_no_backtrace() const noexcept
Get primary string (does not implicitly print the back trace).
Base class for all Fawkes BlackBoard interfaces.
void write()
Write from local copy into BlackBoard memory.
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.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
MessageType * as_type()
Cast message to given type if possible.
virtual void log_warn(const char *component, const char *format,...)
Log warning message.
virtual void log_error(const char *component, const char *format,...)
Log error message.
fawkes::LockPtr< NavGraph > navgraph
NavGraph instance shared in framework.
Generate navgraph using a Grid diagram.
AddEdgeMessage Fawkes BlackBoard Interface Message.
char * p2() const
Get p2 value.
char * p1() const
Get p1 value.
EdgeMode mode() const
Get mode value.
bool is_directed() const
Get directed value.
AddMapObstaclesMessage Fawkes BlackBoard Interface Message.
float max_line_point_distance() const
Get max_line_point_distance value.
AddObstacleMessage Fawkes BlackBoard Interface Message.
float y() const
Get y value.
char * name() const
Get name value.
float x() const
Get x value.
AddPointOfInterestMessage Fawkes BlackBoard Interface Message.
char * name() const
Get name value.
float x() const
Get x value.
ConnectionMode mode() const
Get mode value.
float y() const
Get y value.
AddPointOfInterestWithOriMessage Fawkes BlackBoard Interface Message.
ConnectionMode mode() const
Get mode value.
float ori() const
Get ori value.
float x() const
Get x value.
float y() const
Get y value.
char * name() const
Get name value.
ClearMessage Fawkes BlackBoard Interface Message.
ComputeMessage Fawkes BlackBoard Interface Message.
RemoveObstacleMessage Fawkes BlackBoard Interface Message.
char * name() const
Get name value.
RemovePointOfInterestMessage Fawkes BlackBoard Interface Message.
char * name() const
Get name value.
SetAlgorithmMessage Fawkes BlackBoard Interface Message.
Algorithm algorithm() const
Get algorithm value.
SetAlgorithmParameterMessage Fawkes BlackBoard Interface Message.
char * param() const
Get param value.
char * value() const
Get value value.
SetBoundingBoxMessage Fawkes BlackBoard Interface Message.
float p2_x() const
Get p2_x value.
float p2_y() const
Get p2_y value.
float p1_y() const
Get p1_y value.
float p1_x() const
Get p1_x value.
SetCopyGraphDefaultPropertiesMessage Fawkes BlackBoard Interface Message.
bool is_enable_copy() const
Get enable_copy value.
SetFilterMessage Fawkes BlackBoard Interface Message.
bool is_enable() const
Get enable value.
FilterType filter() const
Get filter value.
SetFilterParamFloatMessage Fawkes BlackBoard Interface Message.
float value() const
Get value value.
char * param() const
Get param value.
FilterType filter() const
Get filter value.
SetGraphDefaultPropertyMessage Fawkes BlackBoard Interface Message.
char * property_value() const
Get property_value value.
char * property_name() const
Get property_name value.
SetPointOfInterestPropertyMessage Fawkes BlackBoard Interface Message.
char * property_name() const
Get property_name value.
char * name() const
Get name value.
char * property_value() const
Get property_value value.
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.
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.
const char * name() const
Get name of thread.
Fawkes library namespace.
void save_yaml_navgraph(std::string filename, NavGraph *graph)
Save navgraph to YAML file.
Cartesian coordinates (2D).