Fawkes API  Fawkes Development Version
navgraph_clusters_thread.h
1 /***************************************************************************
2  * navgraph_clusters_thread.h - block paths based on laser clusters
3  *
4  * Created: Sun Jul 13 15:30:03 2014
5  * Copyright 2014 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_CLUSTERS_NAVGRAPH_CLUSTERS_THREAD_H_
22 #define __PLUGINS_NAVGRAPH_CLUSTERS_NAVGRAPH_CLUSTERS_THREAD_H_
23 
24 #include <core/threading/thread.h>
25 #include <core/utils/lock_list.h>
26 #include <aspect/clock.h>
27 #include <aspect/configurable.h>
28 #include <aspect/logging.h>
29 #include <aspect/tf.h>
30 #include <aspect/blackboard.h>
31 #include <navgraph/aspect/navgraph.h>
32 #include <blackboard/interface_listener.h>
33 #include <blackboard/interface_observer.h>
34 
35 #include <Eigen/Geometry>
36 
37 #include <list>
38 #include <string>
39 #include <tuple>
40 
41 namespace fawkes {
42  class Position3DInterface;
43  class Time;
44  class NavGraphEdgeConstraint;
45  class NavGraphEdgeCostConstraint;
46 }
47 
49 : public fawkes::Thread,
50  public fawkes::ClockAspect,
51  public fawkes::LoggingAspect,
58 {
59  public:
61  virtual ~NavGraphClustersThread();
62 
63  virtual void init();
64  virtual void loop();
65  virtual void finalize();
66 
67  std::list<std::pair<std::string, std::string>> blocked_edges() throw();
68 
69  std::list<std::tuple<std::string, std::string, Eigen::Vector2f>>
70  blocked_edges_centroids() throw();
71 
72  bool robot_pose(Eigen::Vector2f &pose) throw();
73 
74  /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
75  protected: virtual void run() { Thread::run();}
76 
77  private:
78  // for BlackBoardInterfaceObserver
79  virtual void bb_interface_created(const char *type, const char *id) throw();
80 
81  // for BlackBoardInterfaceListener
82  virtual void bb_interface_writer_removed(fawkes::Interface *interface,
83  unsigned int instance_serial) throw();
84  virtual void bb_interface_reader_removed(fawkes::Interface *interface,
85  unsigned int instance_serial) throw();
86 
87  void conditional_close(fawkes::Interface *interface) throw();
88 
89  Eigen::Vector2f fixed_frame_pose(std::string frame, const fawkes::Time &timestamp,
90  float x, float y);
91 
92  private:
93  std::string cfg_iface_prefix_;
94  float cfg_close_threshold_;
95  std::string cfg_fixed_frame_;
96  std::string cfg_base_frame_;
97  int cfg_min_vishistory_;
98  std::string cfg_mode_;
99 
101 
102  fawkes::NavGraphEdgeConstraint *edge_constraint_;
103  fawkes::NavGraphEdgeCostConstraint *edge_cost_constraint_;
104 };
105 
106 #endif
Thread aspect to access to BlackBoard.
Definition: blackboard.h:34
Constraint that can be queried to check if an edge is blocked.
virtual void run()
Stub to see name in backtrace for easier debugging.
Thread aspect that allows to obtain the current time from the clock.
Definition: clock.h:36
Fawkes library namespace.
virtual void run()
Code to execute in the thread.
Definition: thread.cpp:939
A class for handling time.
Definition: time.h:91
Thread class encapsulation of pthreads.
Definition: thread.h:42
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:79
Constraint that can be queried for an edge cost factor.
Thread aspect to access the transform system.
Definition: tf.h:42
Block navgraph paths based on laser clusters.
List with a lock.
Definition: thread.h:40
Thread aspect to log output.
Definition: logging.h:35
BlackBoard interface observer.
Thread aspect to access configuration data.
Definition: configurable.h:35
BlackBoard interface listener.
Thread aspect to access NavGraph.
Definition: navgraph.h:37