Fawkes API  Fawkes Development Version
navgraph_thread.h
1 /***************************************************************************
2  * navgraph_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_NAVGRAPH_THREAD_H_
22 #define __PLUGINS_NAVGRAPH_NAVGRAPH_THREAD_H_
23 
24 #ifdef HAVE_VISUALIZATION
25 # include "visualization_thread.h"
26 #endif
27 
28 #include <core/threading/thread.h>
29 #include <aspect/blocked_timing.h>
30 #include <aspect/clock.h>
31 #include <aspect/configurable.h>
32 #include <aspect/logging.h>
33 #include <aspect/tf.h>
34 #include <aspect/blackboard.h>
35 #include <aspect/aspect_provider.h>
36 #include <navgraph/aspect/navgraph_inifin.h>
37 
38 #include <interfaces/NavigatorInterface.h>
39 #include <interfaces/NavPathInterface.h>
40 
41 #include <navgraph/navgraph.h>
42 #include <utils/system/fam.h>
43 
44 #include <navgraph/constraints/constraint_repo.h>
45 
46 namespace fawkes {
47  class Time;
48 }
49 
51 : public fawkes::Thread,
52  public fawkes::ClockAspect,
53  public fawkes::LoggingAspect,
59  public fawkes::FamListener
60 {
61  public:
62 #ifdef HAVE_VISUALIZATION
64 #endif
66  virtual ~NavGraphThread();
67 
68  virtual void init();
69  virtual void once();
70  virtual void loop();
71  virtual void finalize();
72 
73  virtual void fam_event(const char *filename, unsigned int mask);
74 
75  /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
76  protected: virtual void run() { Thread::run();}
77 
78  private:
79  bool generate_plan(std::string goal);
80  bool generate_plan(std::string goal, float ori);
81  bool generate_plan(float x, float y, float ori);
82  bool replan(const fawkes::NavGraphNode &start,
83  const fawkes::NavGraphNode &goal);
84  void optimize_plan();
85  void stop_motion();
86  void start_plan();
87  void send_next_goal();
88  bool node_reached();
89  bool node_ori_reached();
90  bool node_ori_reached(const fawkes::NavGraphNode &node);
91  size_t shortcut_possible();
92  fawkes::LockPtr<fawkes::NavGraph> load_graph(std::string filename);
93  void log_graph();
94  void publish_path();
95 
96 
97  private:
98  fawkes::NavGraphAspectIniFin navgraph_aspect_inifin_;
99 
100  std::string cfg_graph_file_;
101  std::string cfg_base_frame_;
102  std::string cfg_global_frame_;
103  std::string cfg_nav_if_id_;
104  float cfg_resend_interval_;
105  float cfg_replan_interval_;
106  float cfg_replan_factor_;
107 #ifdef HAVE_VISUALIZATION
108  float cfg_visual_interval_;
109 #endif
110  bool cfg_monitor_file_;
111  float cfg_target_time_;
112  float cfg_target_ori_time_;
113  bool cfg_log_graph_;
114  bool cfg_abort_on_error_;
115  bool cfg_enable_path_execution_;
116 
118  fawkes::NavigatorInterface *pp_nav_if_;
119  fawkes::NavPathInterface *path_if_;
120 
122 
124  bool exec_active_;
125  bool target_reached_;
126  bool target_ori_reached_;
127  bool target_rotating_;
128  float target_time_;
129  fawkes::Time *target_reached_at_;
130  std::string last_node_;
131  fawkes::NavGraphPath path_;
133  bool constrained_plan_;
134 
136 
137  fawkes::Time *cmd_sent_at_;
138  fawkes::Time *path_planned_at_;
139 
140  fawkes::Time *error_at_;
141  std::string error_reason_;
142 
144 
145 
146 #ifdef HAVE_VISUALIZATION
147  fawkes::Time *visualized_at_;
149 #endif
150 };
151 
152 #endif
Thread aspect to access to BlackBoard.
Definition: blackboard.h:34
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
Class representing a path for a NavGraph.
Definition: navgraph_path.h:39
Send Marker messages to rviz to show navgraph info.
Thread class encapsulation of pthreads.
Definition: thread.h:42
Thread aspect to access the transform system.
Definition: tf.h:42
Thread aspect to use blocked timing.
Thread to perform graph-based path planning.
File Alteration Monitor Listener.
Definition: fam.h:35
Monitors files for changes.
Definition: fam.h:71
Thread aspect to log output.
Definition: logging.h:35
Sub-class representing a navgraph path traversal.
Definition: navgraph_path.h:41
Thread aspect provide a new aspect.
Thread aspect to access configuration data.
Definition: configurable.h:35
NavGraphAspect initializer/finalizer.
Topological graph node.
Definition: navgraph_node.h:38
NavPathInterface Fawkes BlackBoard Interface.
virtual void run()
Stub to see name in backtrace for easier debugging.
NavigatorInterface Fawkes BlackBoard Interface.