Fawkes API  Fawkes Development Version
navgraph_generator_thread.h
1 /***************************************************************************
2  * navgraph_generator_thread.h - Graph-based global path planning
3  *
4  * Created: Tue Sep 18 15:56:35 2012
5  * Copyright 2012 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 #ifndef _PLUGINS_NAVGRAPH_GENERATOR_NAVGRAPH_GENERATOR_THREAD_H_
22 #define _PLUGINS_NAVGRAPH_GENERATOR_NAVGRAPH_GENERATOR_THREAD_H_
23 
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>
34 
35 #ifdef HAVE_VISUALIZATION
37 #endif
38 
40  public fawkes::LoggingAspect,
45 {
46 #ifdef HAVE_VISUALIZATION
48 #endif
49 public:
51 #ifdef HAVE_VISUALIZATION
53 #endif
54  virtual ~NavGraphGeneratorThread();
55 
56  virtual void init();
57  virtual void loop();
58  virtual void finalize();
59 
60  /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
61 protected:
62  virtual void
63  run()
64  {
65  Thread::run();
66  }
67 
68 private:
69  typedef struct
70  {
71  fawkes::cart_coord_2d_t position;
73  std::map<std::string, std::string> properties;
74  } PointOfInterest;
75 
76  typedef struct
77  {
78  std::string p1;
79  std::string p2;
80  bool directed;
82  } Edge;
83 
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;
87 
88  virtual bool bb_interface_message_received(fawkes::Interface *interface,
89  fawkes::Message * message) throw();
90 
91  ObstacleMap map_obstacles(float line_max_dist);
92  map_t * load_map(std::vector<std::pair<int, int>> &free_space_indices);
93 
94  void filter_edges_from_map(float max_dist);
95  void filter_nodes_orphans();
96  void filter_multi_graph();
97 
98 #ifdef HAVE_VISUALIZATION
99  void publish_visualization();
100 #endif
101 
102 private:
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_;
110 
111  bool cfg_save_to_file_;
112  std::string cfg_save_filename_;
113 
115 
116  PoiMap pois_;
117  ObstacleMap obstacles_;
118  ObstacleMap map_obstacles_;
119  EdgeList edges_;
120 
121  bool copy_default_properties_;
122  std::map<std::string, std::string> default_properties_;
123 
125  std::map<std::string, std::string> algorithm_params_;
126 
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_;
130 
131  bool bbox_set_;
132  fawkes::cart_coord_2d_t bbox_p1_;
133  fawkes::cart_coord_2d_t bbox_p2_;
134 
135 #ifdef HAVE_VISUALIZATION
137 #endif
138 };
139 
140 #endif
fawkes::Message
Definition: message.h:41
NavGraphGeneratorThread::NavGraphGeneratorThread
NavGraphGeneratorThread()
Constructor.
Definition: navgraph_generator_thread.cpp:44
fawkes::BlackBoardInterfaceListener
Definition: interface_listener.h:47
NavGraphGeneratorThread::finalize
virtual void finalize()
Finalize the thread.
Definition: navgraph_generator_thread.cpp:131
NavGraphGeneratorThread::init
virtual void init()
Initialize the thread.
Definition: navgraph_generator_thread.cpp:69
fawkes::NavGraphGeneratorInterface::EdgeMode
EdgeMode
When adding edges, the mode defines how to add edges.
Definition: NavGraphGeneratorInterface.h:120
fawkes::BlackBoardAspect
Definition: blackboard.h:38
NavGraphGeneratorVisualizationThread
Definition: visualization_thread.h:35
fawkes::LoggingAspect
Definition: logging.h:38
NavGraphGeneratorThread::loop
virtual void loop()
Code to execute in the thread.
Definition: navgraph_generator_thread.cpp:139
fawkes::NavGraphGeneratorInterface::Algorithm
Algorithm
Available generator algorithms.
Definition: NavGraphGeneratorInterface.h:140
fawkes::NavGraphAspect
Definition: navgraph.h:41
fawkes::Interface
Definition: interface.h:78
NavGraphGeneratorThread::run
virtual void run()
Stub to see name in backtrace for easier debugging.
Definition: navgraph_generator_thread.h:68
NavGraphGeneratorThread
Definition: navgraph_generator_thread.h:39
fawkes::cart_coord_2d_struct
Cartesian coordinates (2D).
Definition: types.h:64
fawkes::Thread
Definition: thread.h:45
fawkes::NavGraphGeneratorInterface::ConnectionMode
ConnectionMode
Describe how to connect nodes to the graph.
Definition: NavGraphGeneratorInterface.h:54
fawkes::ConfigurableAspect
Definition: configurable.h:38
fawkes::NavGraphGeneratorInterface
Definition: NavGraphGeneratorInterface.h:39
NavGraphGeneratorThread::~NavGraphGeneratorThread
virtual ~NavGraphGeneratorThread()
Destructor.
Definition: navgraph_generator_thread.cpp:64