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 <aspect/aspect_provider.h>
29 #include <aspect/blackboard.h>
30 #include <aspect/blocked_timing.h>
31 #include <aspect/clock.h>
32 #include <aspect/configurable.h>
33 #include <aspect/logging.h>
34 #include <aspect/tf.h>
35 #include <core/threading/thread.h>
36 #include <interfaces/NavPathInterface.h>
37 #include <interfaces/NavigatorInterface.h>
38 #include <navgraph/aspect/navgraph_inifin.h>
39 #include <navgraph/constraints/constraint_repo.h>
40 #include <navgraph/navgraph.h>
41 #include <utils/system/fam.h>
42 
43 namespace fawkes {
44 class Time;
45 }
46 
48  public fawkes::ClockAspect,
49  public fawkes::LoggingAspect,
55  public fawkes::FamListener
56 {
57 public:
58 #ifdef HAVE_VISUALIZATION
60 #endif
62  virtual ~NavGraphThread();
63 
64  virtual void init();
65  virtual void once();
66  virtual void loop();
67  virtual void finalize();
68 
69  virtual void fam_event(const char *filename, unsigned int mask);
70 
71  /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
72 protected:
73  virtual void
74  run()
75  {
76  Thread::run();
77  }
78 
79 private:
80  bool generate_plan(const std::string &goal);
81  bool generate_plan(const std::string &goal, float ori);
82  bool generate_plan(float x, float y, float ori, const std::string &target_name = "free-target");
83  bool replan(const fawkes::NavGraphNode &start, 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 private:
97  fawkes::NavGraphAspectIniFin navgraph_aspect_inifin_;
98 
99  std::string cfg_graph_file_;
100  std::string cfg_base_frame_;
101  std::string cfg_global_frame_;
102  std::string cfg_nav_if_id_;
103  float cfg_resend_interval_;
104  float cfg_replan_interval_;
105  float cfg_replan_factor_;
106 #ifdef HAVE_VISUALIZATION
107  float cfg_visual_interval_;
108 #endif
109  bool cfg_monitor_file_;
110  float cfg_target_time_;
111  float cfg_target_ori_time_;
112  bool cfg_log_graph_;
113  bool cfg_abort_on_error_;
114  bool cfg_enable_path_execution_;
115  bool cfg_allow_multi_graph_;
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  unsigned int cmd_msgid_;
138  fawkes::Time *cmd_sent_at_;
139  fawkes::Time *path_planned_at_;
140 
141  fawkes::Time *error_at_;
142  std::string error_reason_;
143 
145 
146 #ifdef HAVE_VISUALIZATION
147  fawkes::Time * visualized_at_;
149 #endif
150 };
151 
152 #endif
fawkes::NavGraphNode
Definition: navgraph_node.h:40
fawkes::LockPtr< fawkes::NavGraph >
NavGraphThread::fam_event
virtual void fam_event(const char *filename, unsigned int mask)
Definition: navgraph_thread.cpp:1002
fawkes::FileAlterationMonitor
Definition: fam.h:76
fawkes::FamListener
Definition: fam.h:41
fawkes::AspectProviderAspect
Definition: aspect_provider.h:41
fawkes::BlockedTimingAspect
Definition: blocked_timing.h:56
NavGraphThread::init
virtual void init()
Initialize the thread.
Definition: navgraph_thread.cpp:68
fawkes::NavGraphAspectIniFin
Definition: navgraph_inifin.h:40
fawkes::NavPathInterface
Definition: NavPathInterface.h:39
fawkes::tf::Stamped< fawkes::tf::Pose >
NavGraphVisualizationThread
Definition: visualization_thread.h:36
NavGraphThread::NavGraphThread
NavGraphThread()
Constructor.
Definition: navgraph_thread.cpp:41
NavGraphThread::loop
virtual void loop()
Code to execute in the thread.
Definition: navgraph_thread.cpp:229
fawkes::BlackBoardAspect
Definition: blackboard.h:38
fawkes
fawkes::LoggingAspect
Definition: logging.h:38
fawkes::TransformAspect
Definition: tf.h:43
NavGraphThread::run
virtual void run()
Stub to see name in backtrace for easier debugging.
Definition: navgraph_thread.h:74
NavGraphThread::~NavGraphThread
virtual ~NavGraphThread()
Destructor.
Definition: navgraph_thread.cpp:63
fawkes::Time
Definition: time.h:98
fawkes::Thread
Definition: thread.h:45
fawkes::Thread::start
void start(bool wait=true)
Call this method to start the thread.
Definition: thread.cpp:505
fawkes::NavGraphPath
Definition: navgraph_path.h:41
fawkes::ConfigurableAspect
Definition: configurable.h:38
NavGraphThread
Definition: navgraph_thread.h:47
fawkes::NavigatorInterface
Definition: NavigatorInterface.h:39
fawkes::ClockAspect
Definition: clock.h:40
fawkes::NavGraphPath::Traversal
Definition: navgraph_path.h:44
NavGraphThread::finalize
virtual void finalize()
Finalize the thread.
Definition: navgraph_thread.cpp:197
NavGraphThread::once
virtual void once()
Execute an action exactly once.
Definition: navgraph_thread.cpp:218