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)",
176 for (
auto o : obstacles_) {
178 name(),
" Adding obstacle %s at (%f,%f)", o.first.c_str(), 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);
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();
366 NavGraphGeneratorThread::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());
547 NavGraphGeneratorThread::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(),
575 NavGraphGeneratorThread::ObstacleMap
576 NavGraphGeneratorThread::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)] =
683 NavGraphGeneratorThread::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)",
725 NavGraphGeneratorThread::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);
751 NavGraphGeneratorThread::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) {
802 " Removing disconnected sub-graph [%s]",
803 str_join(g.begin(), g.end(),
", ").c_str());
804 for (
const std::string &n : g)
809 #ifdef HAVE_VISUALIZATION 811 NavGraphGeneratorThread::publish_visualization()
814 vt_->publish(obstacles_, map_obstacles_, pois_);
char * name() const
Get name value.
void set_error_message(const char *new_error_message)
Set error_message value.
Line information container.
SetFilterParamFloatMessage Fawkes BlackBoard Interface Message.
FilterType filter() const
Get filter value.
RemovePointOfInterestMessage Fawkes BlackBoard Interface Message.
SetAlgorithmMessage Fawkes BlackBoard Interface Message.
float p2_y() const
Get p2_y value.
AddPointOfInterestMessage Fawkes BlackBoard Interface Message.
float value() const
Get value value.
float max_line_point_distance() const
Get max_line_point_distance value.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
AddPointOfInterestWithOriMessage Fawkes BlackBoard Interface Message.
fawkes::LockPtr< NavGraph > navgraph
NavGraph instance shared in framework.
SetGraphDefaultPropertyMessage Fawkes BlackBoard Interface Message.
Cartesian coordinates (2D).
SetCopyGraphDefaultPropertiesMessage Fawkes BlackBoard Interface Message.
virtual void log_error(const char *component, const char *format,...)
Log error message.
char * value() const
Get value value.
float p1_x() const
Get p1_x value.
Generate navgraph using a Grid diagram.
char * name() const
Get name value.
Fawkes library namespace.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
float p2_x() const
Get p2_x value.
void set_unconnected(bool unconnected)
Set unconnected state of the node.
virtual ~NavGraphGeneratorThread()
Destructor.
std::string str_join(const InputIterator &first, const InputIterator &last, char delim='/')
Join list of strings string using given delimiter.
SetFilterMessage Fawkes BlackBoard Interface Message.
SetBoundingBoxMessage Fawkes BlackBoard Interface Message.
void save_yaml_navgraph(std::string filename, NavGraph *graph)
Save navgraph to YAML file.
float x() const
Get x value.
EdgeMode mode() const
Get mode value.
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
Thread class encapsulation of pthreads.
virtual void compute(fawkes::LockPtr< fawkes::NavGraph > graph)=0
Compute graph.
void write()
Write from local copy into BlackBoard memory.
Base class for all Fawkes BlackBoard interfaces.
virtual void set_bounding_box(float bbox_p1_x, float bbox_p1_y, float bbox_p2_x, float bbox_p2_y)
Set bounding box.
SetAlgorithmParameterMessage Fawkes BlackBoard Interface Message.
AddObstacleMessage Fawkes BlackBoard Interface Message.
Logger * logger
This is the Logger member used to access the logger.
float p1_y() const
Get p1_y value.
SetPointOfInterestPropertyMessage Fawkes BlackBoard Interface Message.
float y() const
Get y value.
ConnectionMode mode() const
Get mode value.
Voronoi-based algorithm for navgraph generation.
virtual void loop()
Code to execute in the thread.
const char * tostring_Algorithm(Algorithm value) const
Convert Algorithm constant to string.
char * name() const
Get name value.
FilterType filter() const
Get filter value.
char * name() const
Get name value.
NavGraphGeneratorInterface Fawkes BlackBoard Interface.
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
float y() const
Get y value.
ComputeMessage Fawkes BlackBoard Interface Message.
virtual void finalize()
Finalize the thread.
void bbil_remove_message_interface(Interface *interface)
Remove an interface to the message received watch list.
char * property_name() const
Get property_name value.
Algorithm algorithm() const
Get algorithm value.
Base class for exceptions in Fawkes.
float x() const
Get x value.
NavGraphGeneratorThread()
Constructor.
Generate navgraph using a Voronoi diagram.
void set_final(const bool new_final)
Set final value.
float ori() const
Get ori value.
char * param() const
Get param value.
char * p1() const
Get p1 value.
virtual void log_warn(const char *component, const char *format,...)
Log warning message.
virtual void init()
Initialize the thread.
char * property_value() const
Get property_value value.
char * property_value() const
Get property_value value.
void set_ok(const bool new_ok)
Set ok value.
const char * name() const
Get name of thread.
bool is_enable_copy() const
Get enable_copy value.
MessageType * as_type()
Cast message to given type if possible.
char * p2() const
Get p2 value.
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
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.
Grid-based algorithm with customizable spacing.
Send Marker messages to rviz to show navgraph-generator info.
bool is_enable() const
Get enable value.
ClearMessage Fawkes BlackBoard Interface Message.
ConnectionMode mode() const
Get mode value.
virtual void add_obstacle(float x, float y)
Add an obstacle point.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
AddMapObstaclesMessage Fawkes BlackBoard Interface Message.
char * param() const
Get param value.
float y() const
Get y value.
virtual bool exists(const char *path)=0
Check if a given value exists.
char * name() const
Get name value.
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
AddEdgeMessage Fawkes BlackBoard Interface Message.
const std::vector< std::string > & reachable_nodes() const
Get reachable nodes.
Configuration * config
This is the Configuration member used to access the configuration.
float x() const
Get x value.
void bbil_add_message_interface(Interface *interface)
Add an interface to the message received watch list.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
RemoveObstacleMessage Fawkes BlackBoard Interface Message.
bool is_directed() const
Get directed value.
char * name() const
Get name value.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
BlackBoard interface listener.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
char * property_name() const
Get property_name value.
virtual void close(Interface *interface)=0
Close interface.