21 #ifndef _PLUGINS_NAVGRAPH_GENERATOR_NAVGRAPH_GENERATOR_THREAD_H_
22 #define _PLUGINS_NAVGRAPH_GENERATOR_NAVGRAPH_GENERATOR_THREAD_H_
24 #include <aspect/blackboard.h>
25 #include <aspect/configurable.h>
26 #include <aspect/logging.h>
27 #include <blackboard/interface_listener.h>
28 #include <core/threading/thread.h>
29 #include <interfaces/NavGraphGeneratorInterface.h>
30 #include <navgraph/aspect/navgraph.h>
31 #include <navgraph/navgraph.h>
32 #include <plugins/amcl/map/map.h>
33 #include <utils/math/types.h>
35 #ifdef HAVE_VISUALIZATION
46 #ifdef HAVE_VISUALIZATION
51 #ifdef HAVE_VISUALIZATION
73 std::map<std::string, std::string> properties;
84 typedef std::map<std::string, PointOfInterest> PoiMap;
85 typedef std::map<std::string, fawkes::cart_coord_2d_t> ObstacleMap;
86 typedef std::list<Edge> EdgeList;
91 ObstacleMap map_obstacles(
float line_max_dist);
92 map_t * load_map(std::vector<std::pair<int, int>> &free_space_indices);
94 void filter_edges_from_map(
float max_dist);
95 void filter_nodes_orphans();
96 void filter_multi_graph();
98 #ifdef HAVE_VISUALIZATION
99 void publish_visualization();
103 std::string cfg_global_frame_;
104 unsigned int cfg_map_line_segm_max_iterations_;
105 float cfg_map_line_min_length_;
106 unsigned int cfg_map_line_segm_min_inliers_;
107 float cfg_map_line_cluster_tolerance_;
108 float cfg_map_line_cluster_quota_;
109 bool cfg_visualization_;
111 bool cfg_save_to_file_;
112 std::string cfg_save_filename_;
117 ObstacleMap obstacles_;
118 ObstacleMap map_obstacles_;
121 bool copy_default_properties_;
122 std::map<std::string, std::string> default_properties_;
125 std::map<std::string, std::string> algorithm_params_;
127 std::map<std::string, bool> filter_;
128 std::map<std::string, std::map<std::string, float>> filter_params_float_;
129 std::map<std::string, std::map<std::string, float>> filter_params_float_defaults_;
135 #ifdef HAVE_VISUALIZATION