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 <core/threading/thread.h>
25 #include <aspect/configurable.h>
26 #include <aspect/logging.h>
27 #include <aspect/blackboard.h>
28 #include <navgraph/aspect/navgraph.h>
29 #include <navgraph/navgraph.h>
30 #include <blackboard/interface_listener.h>
31 #include <utils/math/types.h>
32 #include <plugins/amcl/map/map.h>
33 
34 #include <interfaces/NavGraphGeneratorInterface.h>
35 
36 #ifdef HAVE_VISUALIZATION
38 #endif
39 
41 : public fawkes::Thread,
42  public fawkes::LoggingAspect,
47 {
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: virtual void run() { Thread::run();}
62 
63  private:
64  typedef struct {
65  fawkes::cart_coord_2d_t position;
67  std::map<std::string, std::string> properties;
68  } PointOfInterest;
69 
70  typedef struct {
71  std::string p1;
72  std::string p2;
73  bool directed;
75  } Edge;
76 
77  typedef std::map<std::string, PointOfInterest> PoiMap;
78  typedef std::map<std::string, fawkes::cart_coord_2d_t> ObstacleMap;
79  typedef std::list<Edge> EdgeList;
80 
81  virtual bool bb_interface_message_received(fawkes::Interface *interface,
82  fawkes::Message *message) throw();
83 
84  ObstacleMap map_obstacles(float line_max_dist);
85  map_t * load_map(std::vector<std::pair<int, int>> &free_space_indices);
86 
87  void filter_edges_from_map(float max_dist);
88  void filter_nodes_orphans();
89  void filter_multi_graph();
90 
91 #ifdef HAVE_VISUALIZATION
92  void publish_visualization();
93 #endif
94 
95  private:
96  std::string cfg_global_frame_;
97  unsigned int cfg_map_line_segm_max_iterations_;
98  float cfg_map_line_min_length_;
99  unsigned int cfg_map_line_segm_min_inliers_;
100  float cfg_map_line_cluster_tolerance_;
101  float cfg_map_line_cluster_quota_;
102  bool cfg_visualization_;
103 
105 
106  PoiMap pois_;
107  ObstacleMap obstacles_;
108  ObstacleMap map_obstacles_;
109  EdgeList edges_;
110 
111  bool copy_default_properties_;
112  std::map<std::string, std::string> default_properties_;
113 
114  std::map<std::string, bool> filter_;
115  std::map<std::string, std::map<std::string, float>> filter_params_float_;
116  std::map<std::string, std::map<std::string, float>> filter_params_float_defaults_;
117 
118  bool bbox_set_;
119  fawkes::cart_coord_2d_t bbox_p1_;
120  fawkes::cart_coord_2d_t bbox_p2_;
121 
122 #ifdef HAVE_VISUALIZATION
124 #endif
125 };
126 
127 #endif
Thread to perform graph-based path planning.
EdgeMode
When adding edges, the mode defines how to add edges.
Thread aspect to access to BlackBoard.
Definition: blackboard.h:34
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:44
Cartesian coordinates (2D).
Definition: types.h:59
virtual ~NavGraphGeneratorThread()
Destructor.
Thread class encapsulation of pthreads.
Definition: thread.h:42
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:79
virtual void run()
Stub to see name in backtrace for easier debugging.
virtual void loop()
Code to execute in the thread.
NavGraphGeneratorInterface Fawkes BlackBoard Interface.
virtual void finalize()
Finalize the thread.
virtual void init()
Initialize the thread.
Thread aspect to log output.
Definition: logging.h:35
Send Marker messages to rviz to show navgraph-generator info.
Thread aspect to access configuration data.
Definition: configurable.h:35
BlackBoard interface listener.
Thread aspect to access NavGraph.
Definition: navgraph.h:37