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
43namespace fawkes {
44class Time;
45}
46
56{
57public:
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() */
72protected:
73 virtual void
75 {
76 Thread::run();
77 }
78
79private:
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
96private:
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_;
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
Thread to perform graph-based path planning.
virtual void fam_event(const char *filename, unsigned int mask)
Event has been raised.
virtual void run()
Stub to see name in backtrace for easier debugging.
virtual void once()
Execute an action exactly once.
virtual void finalize()
Finalize the thread.
virtual void init()
Initialize the thread.
virtual ~NavGraphThread()
Destructor.
virtual void loop()
Code to execute in the thread.
NavGraphThread()
Constructor.
Send Marker messages to rviz to show navgraph info.
Thread aspect provide a new aspect.
Thread aspect to access to BlackBoard.
Definition: blackboard.h:34
Thread aspect to use blocked timing.
Thread aspect that allows to obtain the current time from the clock.
Definition: clock.h:34
Thread aspect to access configuration data.
Definition: configurable.h:33
File Alteration Monitor Listener.
Definition: fam.h:36
Monitors files for changes.
Definition: fam.h:71
Thread aspect to log output.
Definition: logging.h:33
NavGraphAspect initializer/finalizer.
Topological graph node.
Definition: navgraph_node.h:36
Sub-class representing a navgraph path traversal.
Definition: navgraph_path.h:40
Class representing a path for a NavGraph.
Definition: navgraph_path.h:37
NavPathInterface Fawkes BlackBoard Interface.
NavigatorInterface Fawkes BlackBoard Interface.
Thread class encapsulation of pthreads.
Definition: thread.h:46
void start(bool wait=true)
Call this method to start the thread.
Definition: thread.cpp:499
A class for handling time.
Definition: time.h:93
Thread aspect to access the transform system.
Definition: tf.h:39
Fawkes library namespace.