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/voronoi.h> 28 #include <plugins/laser-lines/line_func.h> 29 #include <plugins/amcl/amcl_utils.h> 30 #include <utils/misc/string_split.h> 35 #define CFG_PREFIX "/navgraph-generator/" 44 :
Thread(
"NavGraphGeneratorThread",
Thread::OPMODE_WAITFORWAKEUP),
47 #ifdef HAVE_VISUALIZATION 52 #ifdef HAVE_VISUALIZATION 55 :
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_ =
90 cfg_map_line_min_length_ =
92 cfg_map_line_cluster_tolerance_ =
94 cfg_map_line_cluster_quota_ =
99 cfg_visualization_ =
false;
101 cfg_visualization_ =
config->
get_bool(CFG_PREFIX
"visualization/enable");
104 #ifndef HAVE_VISUALIZATION 105 if (cfg_visualization_) {
135 bbox_p1_.
x, bbox_p1_.
y, bbox_p2_.
x, bbox_p2_.
y);
139 for (
auto o : obstacles_) {
141 o.first.c_str(), o.second.x, o.second.y);
145 for (
auto o : map_obstacles_) {
147 o.first.c_str(), o.second.x, o.second.y);
156 navgraph->set_notifications_enabled(
false);
159 std::map<std::string, std::string> default_props =
navgraph->default_properties();
164 if (copy_default_properties_) {
165 navgraph->set_default_properties(default_props);
169 for (
auto p : default_properties_) {
170 navgraph->set_default_property(p.first, p.second);
177 if (filter_[
"FILTER_EDGES_BY_MAP"]) {
179 filter_edges_from_map(filter_params_float_[
"FILTER_EDGES_BY_MAP"][
"distance"]);
181 if (filter_[
"FILTER_ORPHAN_NODES"]) {
183 filter_nodes_orphans();
185 if (filter_[
"FILTER_MULTI_GRAPH"]) {
187 filter_multi_graph();
191 for (
const auto &p : pois_) {
193 NavGraphNode node(p.first, p.second.position.x, p.second.position.y,
194 p.second.properties);
195 switch (p.second.conn_mode) {
196 case NavGraphGeneratorInterface::NOT_CONNECTED:
198 p.first.c_str(), p.second.position.x, p.second.position.y);
202 case NavGraphGeneratorInterface::UNCONNECTED:
204 p.first.c_str(), p.second.position.x, p.second.position.y);
209 case NavGraphGeneratorInterface::CLOSEST_NODE:
211 p.first.c_str(), p.second.position.x, p.second.position.y);
212 navgraph->add_node_and_connect(node, NavGraph::CLOSEST_NODE);
214 case NavGraphGeneratorInterface::CLOSEST_EDGE:
217 p.first.c_str(), p.second.position.x, p.second.position.y);
218 navgraph->add_node_and_connect(node, NavGraph::CLOSEST_EDGE);
225 case NavGraphGeneratorInterface::CLOSEST_EDGE_OR_NODE:
227 p.first.c_str(), p.second.position.x, p.second.position.y);
228 navgraph->add_node_and_connect(node, NavGraph::CLOSEST_EDGE_OR_NODE);
235 for (
const auto &e : edges_) {
236 switch (e.edge_mode) {
237 case NavGraphGeneratorInterface::NO_INTERSECTION:
239 e.p1.c_str(), e.directed ?
">" :
"-", e.p2.c_str());
241 NavGraph::EDGE_NO_INTERSECTION);
244 case NavGraphGeneratorInterface::SPLIT_INTERSECTION:
246 e.p1.c_str(), e.directed ?
">" :
"-", e.p2.c_str());
248 NavGraph::EDGE_SPLIT_INTERSECTION);
251 case NavGraphGeneratorInterface::FORCE:
253 e.p1.c_str(), e.directed ?
">" :
"-", e.p2.c_str());
255 NavGraph::EDGE_FORCE);
283 navgraph->set_notifications_enabled(
true);
291 #ifdef HAVE_VISUALIZATION 292 if (cfg_visualization_) publish_visualization();
298 NavGraphGeneratorThread::bb_interface_message_received(
Interface *interface,
307 map_obstacles_.clear();
309 default_properties_.clear();
311 copy_default_properties_ =
true;
312 filter_params_float_ = filter_params_float_defaults_;
313 for (
auto &f : filter_) {
320 bbox_p1_.
x = msg->
p1_x();
321 bbox_p1_.
y = msg->
p1_y();
322 bbox_p2_.
x = msg->
p2_x();
323 bbox_p2_.
y = msg->
p2_y();
335 std::map<std::string, float> ¶m_float =
338 if (param_float.find(msg->
param()) != param_float.end()) {
354 if (std::isfinite(msg->
x()) && std::isfinite(msg->
x())) {
363 ObstacleMap::iterator f;
364 if ((f = obstacles_.find(msg->
name())) != obstacles_.end()) {
371 if (std::isfinite(msg->
x()) && std::isfinite(msg->
x())) {
374 poi.conn_mode = msg->
mode();
375 pois_[msg->
name()] = poi;
388 edge.edge_mode = msg->
mode();
389 edges_.push_back(edge);
395 if (std::isfinite(msg->
x()) && std::isfinite(msg->
x())) {
398 poi.conn_mode = msg->
mode();
399 if (std::isfinite(msg->
ori())) {
400 poi.properties[
"orientation"] = std::to_string(msg->
ori());
405 pois_[msg->
name()] = poi;
415 if ((f = pois_.find(msg->
name())) != pois_.end()) {
423 if ((f = pois_.find(msg->
name())) != pois_.end()) {
456 NavGraphGeneratorThread::load_map(std::vector<std::pair<int, int>> &free_space_indices)
458 std::string cfg_map_file;
459 float cfg_resolution;
462 float cfg_origin_theta;
463 float cfg_occupied_thresh;
464 float cfg_free_thresh;
466 fawkes::amcl::read_map_config(
config, cfg_map_file, cfg_resolution, cfg_origin_x,
467 cfg_origin_y, cfg_origin_theta, cfg_occupied_thresh,
470 return fawkes::amcl::read_map(cfg_map_file.c_str(),
471 cfg_origin_x, cfg_origin_y, cfg_resolution,
472 cfg_occupied_thresh, cfg_free_thresh, free_space_indices);
475 NavGraphGeneratorThread::ObstacleMap
476 NavGraphGeneratorThread::map_obstacles(
float line_max_dist)
478 ObstacleMap obstacles;
479 unsigned int obstacle_i = 0;
481 std::vector<std::pair<int, int> > free_space_indices;
482 map_t *map = load_map(free_space_indices);
484 logger->
log_info(
name(),
"Map Obstacles: map size: %ux%u (%zu of %u cells free, %.1f%%)",
485 map->size_x, map->size_y, free_space_indices.size(),
486 map->size_x * map->size_y,
487 (float)free_space_indices.size() / (float)(map->size_x * map->size_y) * 100.);
489 size_t occ_cells = map->size_x * map->size_y - free_space_indices.size();
493 map_cloud->points.resize(occ_cells);
495 for (
int x = 0; x < map->size_x; ++x) {
496 for (
int y = 0; y < map->size_y; ++y) {
497 if (map->cells[MAP_INDEX(map, x, y)].occ_state > 0) {
500 p.x = MAP_WXGX(map, x) + 0.5 * map->scale;
501 p.y = MAP_WYGY(map, y) + 0.5 * map->scale;
503 map_cloud->points[pi++] = p;
514 std::vector<LineInfo> linfos =
515 calc_lines<pcl::PointXYZ>(map_cloud,
516 cfg_map_line_segm_min_inliers_, cfg_map_line_segm_max_iterations_,
519 cfg_map_line_cluster_tolerance_, cfg_map_line_cluster_quota_,
520 cfg_map_line_min_length_, -1, -1, -1,
524 linfos.size(), no_line_cloud->points.size());
528 for (
const LineInfo &line : linfos) {
529 const unsigned int num_points = ceilf(line.length / line_max_dist);
530 float distribution = line.length / num_points;
532 obstacles[NavGraph::format_name(
"Map_%u", ++obstacle_i)] =
534 for (
unsigned int i = 1; i <= num_points; ++i) {
535 Eigen::Vector3f p = line.end_point_1 + i * distribution * line.line_direction;
536 obstacles[NavGraph::format_name(
"Map_%d", ++obstacle_i)] =
cart_coord_2d_t(p[0], p[1]);
541 pcl::search::KdTree<pcl::PointXYZ>::Ptr
542 kdtree_cluster(
new pcl::search::KdTree<pcl::PointXYZ>());
544 std::vector<pcl::PointIndices> cluster_indices;
545 pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
546 ec.setClusterTolerance(2 * map->scale);
547 ec.setMinClusterSize(1);
548 ec.setMaxClusterSize(no_line_cloud->points.size());
549 ec.setSearchMethod(kdtree_cluster);
550 ec.setInputCloud(no_line_cloud);
551 ec.extract(cluster_indices);
554 for (
auto cluster : cluster_indices) {
555 Eigen::Vector4f centroid;
556 pcl::compute3DCentroid(*no_line_cloud, cluster.indices, centroid);
559 i, cluster.indices.size(), centroid.x(), centroid.y(), centroid.z());
561 obstacles[NavGraph::format_name(
"MapCluster_%u", ++i)] =
cart_coord_2d_t(centroid.x(), centroid.y());
571 NavGraphGeneratorThread::filter_edges_from_map(
float max_dist)
573 std::vector<std::pair<int, int> > free_space_indices;
574 map_t *map = load_map(free_space_indices);
576 const std::vector<NavGraphEdge> &edges =
navgraph->edges();
578 for (
int x = 0; x < map->size_x; ++x) {
579 for (
int y = 0; y < map->size_y; ++y) {
580 if (map->cells[MAP_INDEX(map, x, y)].occ_state > 0) {
583 gp[0] = MAP_WXGX(map, x) + 0.5 * map->scale;
584 gp[1] = MAP_WYGY(map, y) + 0.5 * map->scale;
590 p[0] = poe.
x; p[1] = poe.
y;
591 if ((gp - p).norm() <= max_dist) {
594 " Removing edge (%s--%s), too close to occupied map cell (%f,%f)",
595 e.from().c_str(), e.to().c_str(), gp[0], gp[1]);
609 NavGraphGeneratorThread::filter_nodes_orphans()
611 const std::vector<NavGraphEdge> &edges =
navgraph->edges();
612 const std::vector<NavGraphNode> &nodes =
navgraph->nodes();
614 std::list<NavGraphNode> remove_nodes;
617 std::string nname = n.name();
618 std::vector<NavGraphEdge>::const_iterator e =
619 std::find_if(edges.begin(), edges.end(),
621 return (e.from() == nname || e.to() == nname);
623 if (e == edges.end() && ! n.unconnected()) {
625 remove_nodes.push_back(n);
637 NavGraphGeneratorThread::filter_multi_graph()
641 std::list<std::set<std::string>> graphs;
643 const std::vector<NavGraphNode> &nodes =
navgraph->nodes();
644 std::set<std::string> nodeset;
645 std::for_each(nodes.begin(), nodes.end(),
646 [&nodeset](
const NavGraphNode &n){ nodeset.insert(n.name()); });
648 while (! nodeset.empty()) {
649 std::queue<std::string> q;
650 q.push(* nodeset.begin());
652 std::set<std::string> traversed;
654 while (! q.empty()) {
655 std::string &nname = q.front();
656 traversed.insert(nname);
662 for (
const std::string &r : reachable) {
663 if (traversed.find(r) == traversed.end()) q.push(r);
669 std::set<std::string> nodediff;
670 std::set_difference(nodeset.begin(), nodeset.end(),
671 traversed.begin(), traversed.end(),
672 std::inserter(nodediff, nodediff.begin()));
673 graphs.push_back(traversed);
678 graphs.sort([](
const std::set<std::string> &a,
const std::set<std::string> &b)->
bool{
679 return b.size() < a.size();
682 std::for_each(std::next(graphs.begin()), graphs.end(),
683 [&](
const std::set<std::string> &g) {
685 str_join(g.begin(), g.end(),
", ").c_str());
686 for (
const std::string &n : g)
navgraph->remove_node(n);
691 #ifdef HAVE_VISUALIZATION 693 NavGraphGeneratorThread::publish_visualization()
696 vt_->publish(obstacles_, map_obstacles_, pois_);
void set_msgid(const uint32_t new_msgid)
Set msgid value.
Thread(const char *name)
Constructor.
char * name() const
Get name value.
Line information container.
SetFilterParamFloatMessage Fawkes BlackBoard Interface Message.
FilterType filter() const
Get filter value.
RemovePointOfInterestMessage 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.
unsigned int id() const
Get message ID.
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.
float p1_x() const
Get p1_x value.
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 set_bounding_box(float bbox_p1_x, float bbox_p1_y, float bbox_p2_x, float bbox_p2_y)
Set bounding box.
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.
const char * tostring_FilterType(FilterType value) const
Convert FilterType constant to string.
void add_obstacle(float x, float y)
Add an obstacle point.
void write()
Write from local copy into BlackBoard memory.
Base class for all Fawkes BlackBoard interfaces.
Mutex * loop_mutex
Mutex that is used to protect a call to loop().
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.
virtual void loop()
Code to execute in the thread.
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.
void wakeup()
Wake up thread.
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 init()
Initialize the thread.
char * property_value() const
Get property_value value.
BlackBoardInterfaceListener(const char *name_format,...)
Constructor.
char * property_value() const
Get property_value 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 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.
Send Marker messages to rviz to show navgraph-generator info.
bool is_of_type()
Check if message has desired type.
bool is_enable() const
Get enable value.
ClearMessage Fawkes BlackBoard Interface Message.
ConnectionMode mode() const
Get mode value.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
AddMapObstaclesMessage Fawkes BlackBoard Interface Message.
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.
virtual void compute(fawkes::LockPtr< fawkes::NavGraph > graph)
Compute graph.
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.
const char * type() const
Get message type.
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.