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 <aspect/blackboard.h>
25 #include <aspect/clock.h>
26 #include <aspect/configurable.h>
27 #include <aspect/logging.h>
28 #include <aspect/tf.h>
29 #include <blackboard/interface_listener.h>
30 #include <blackboard/interface_observer.h>
31 #include <core/threading/thread.h>
32 #include <core/utils/lock_list.h>
33 #include <navgraph/aspect/navgraph.h>
34 
35 #include <Eigen/Geometry>
36 #include <list>
37 #include <string>
38 #include <tuple>
39 
40 namespace fawkes {
41 class Position3DInterface;
42 class Time;
43 class NavGraphEdgeConstraint;
44 class NavGraphEdgeCostConstraint;
45 } // namespace fawkes
46 
48  public fawkes::ClockAspect,
49  public fawkes::LoggingAspect,
56 {
57 public:
59  virtual ~NavGraphClustersThread();
60 
61  virtual void init();
62  virtual void loop();
63  virtual void finalize();
64 
65  std::list<std::pair<std::string, std::string>> blocked_edges() throw();
66 
67  std::list<std::tuple<std::string, std::string, Eigen::Vector2f>>
68  blocked_edges_centroids() throw();
69 
70  bool robot_pose(Eigen::Vector2f &pose) throw();
71 
72  /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
73 protected:
74  virtual void
75  run()
76  {
77  Thread::run();
78  }
79 
80 private:
81  // for BlackBoardInterfaceObserver
82  virtual void bb_interface_created(const char *type, const char *id) throw();
83 
84  // for BlackBoardInterfaceListener
85  virtual void bb_interface_writer_removed(fawkes::Interface *interface,
86  unsigned int instance_serial) throw();
87  virtual void bb_interface_reader_removed(fawkes::Interface *interface,
88  unsigned int instance_serial) throw();
89 
90  void conditional_close(fawkes::Interface *interface) throw();
91 
92  Eigen::Vector2f
93  fixed_frame_pose(std::string frame, const fawkes::Time &timestamp, float x, float y);
94 
95 private:
96  std::string cfg_iface_prefix_;
97  float cfg_close_threshold_;
98  std::string cfg_fixed_frame_;
99  std::string cfg_base_frame_;
100  int cfg_min_vishistory_;
101  std::string cfg_mode_;
102 
104 
105  fawkes::NavGraphEdgeConstraint * edge_constraint_;
106  fawkes::NavGraphEdgeCostConstraint *edge_cost_constraint_;
107 };
108 
109 #endif
NavGraphClustersThread::blocked_edges_centroids
std::list< std::tuple< std::string, std::string, Eigen::Vector2f > > blocked_edges_centroids()
Get a list of edges close to a clusters and its centroid considered blocked.
Definition: navgraph_clusters_thread.cpp:233
NavGraphClustersThread::finalize
virtual void finalize()
Finalize the thread.
Definition: navgraph_clusters_thread.cpp:103
fawkes::LockList< fawkes::Position3DInterface * >
fawkes::BlackBoardInterfaceListener
Definition: interface_listener.h:45
NavGraphClustersThread
Definition: navgraph_clusters_thread.h:46
NavGraphClustersThread::init
virtual void init()
Initialize the thread.
Definition: navgraph_clusters_thread.cpp:58
fawkes::NavGraphEdgeConstraint
Definition: edge_constraint.h:38
fawkes::NavGraphEdgeCostConstraint
Definition: edge_cost_constraint.h:36
fawkes::BlackBoardAspect
Definition: blackboard.h:36
NavGraphClustersThread::run
virtual void run()
Stub to see name in backtrace for easier debugging.
Definition: navgraph_clusters_thread.h:74
fawkes
fawkes::LoggingAspect
Definition: logging.h:36
fawkes::TransformAspect
Definition: tf.h:41
fawkes::NavGraphAspect
Definition: navgraph.h:39
fawkes::BlackBoardInterfaceObserver
Definition: interface_observer.h:40
fawkes::Interface
Definition: interface.h:77
NavGraphClustersThread::blocked_edges
std::list< std::pair< std::string, std::string > > blocked_edges()
Get a list of edges close to a clusters considered blocked.
Definition: navgraph_clusters_thread.cpp:213
fawkes::Time
Definition: time.h:96
fawkes::Thread
Definition: thread.h:44
NavGraphClustersThread::robot_pose
bool robot_pose(Eigen::Vector2f &pose)
Determine current robot pose.
Definition: navgraph_clusters_thread.cpp:312
fawkes::ConfigurableAspect
Definition: configurable.h:36
NavGraphClustersThread::NavGraphClustersThread
NavGraphClustersThread()
Constructor.
Definition: navgraph_clusters_thread.cpp:46
fawkes::ClockAspect
Definition: clock.h:38
NavGraphClustersThread::loop
virtual void loop()
Code to execute in the thread.
Definition: navgraph_clusters_thread.cpp:125
NavGraphClustersThread::~NavGraphClustersThread
virtual ~NavGraphClustersThread()
Destructor.
Definition: navgraph_clusters_thread.cpp:53