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:234
NavGraphClustersThread::finalize
virtual void finalize()
Finalize the thread.
Definition: navgraph_clusters_thread.cpp:104
fawkes::LockList< fawkes::Position3DInterface * >
fawkes::BlackBoardInterfaceListener
BlackBoard interface listener.
Definition: interface_listener.h:42
NavGraphClustersThread
Block navgraph paths based on laser clusters.
Definition: navgraph_clusters_thread.h:56
NavGraphClustersThread::init
virtual void init()
Initialize the thread.
Definition: navgraph_clusters_thread.cpp:59
fawkes::NavGraphEdgeConstraint
Constraint that can be queried to check if an edge is blocked.
Definition: edge_constraint.h:35
fawkes::NavGraphEdgeCostConstraint
Constraint that can be queried for an edge cost factor.
Definition: edge_cost_constraint.h:34
fawkes::BlackBoardAspect
Thread aspect to access to BlackBoard.
Definition: blackboard.h:34
NavGraphClustersThread::run
virtual void run()
Stub to see name in backtrace for easier debugging.
Definition: navgraph_clusters_thread.h:75
fawkes
Fawkes library namespace.
fawkes::LoggingAspect
Thread aspect to log output.
Definition: logging.h:33
fawkes::TransformAspect
Thread aspect to access the transform system.
Definition: tf.h:39
fawkes::NavGraphAspect
Thread aspect to access NavGraph.
Definition: navgraph.h:36
fawkes::BlackBoardInterfaceObserver
BlackBoard interface observer.
Definition: interface_observer.h:37
fawkes::Interface
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:79
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:214
fawkes::Time
A class for handling time.
Definition: time.h:93
fawkes::Thread
Thread class encapsulation of pthreads.
Definition: thread.h:46
NavGraphClustersThread::robot_pose
bool robot_pose(Eigen::Vector2f &pose)
Determine current robot pose.
Definition: navgraph_clusters_thread.cpp:313
fawkes::ConfigurableAspect
Thread aspect to access configuration data.
Definition: configurable.h:33
NavGraphClustersThread::NavGraphClustersThread
NavGraphClustersThread()
Constructor.
Definition: navgraph_clusters_thread.cpp:47
fawkes::ClockAspect
Thread aspect that allows to obtain the current time from the clock.
Definition: clock.h:34
NavGraphClustersThread::loop
virtual void loop()
Code to execute in the thread.
Definition: navgraph_clusters_thread.cpp:126
NavGraphClustersThread::~NavGraphClustersThread
virtual ~NavGraphClustersThread()
Destructor.
Definition: navgraph_clusters_thread.cpp:54