21 #ifndef __PLUGINS_NAVGRAPH_NAVGRAPH_THREAD_H_ 22 #define __PLUGINS_NAVGRAPH_NAVGRAPH_THREAD_H_ 24 #ifdef HAVE_VISUALIZATION 25 # include "visualization_thread.h" 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> 38 #include <interfaces/NavigatorInterface.h> 39 #include <interfaces/NavPathInterface.h> 41 #include <navgraph/navgraph.h> 42 #include <utils/system/fam.h> 44 #include <navgraph/constraints/constraint_repo.h> 62 #ifdef HAVE_VISUALIZATION 73 virtual void fam_event(
const char *filename,
unsigned int mask);
76 protected:
virtual void run() { Thread::run();}
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);
87 void send_next_goal();
89 bool node_ori_reached();
91 size_t shortcut_possible();
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_;
110 bool cfg_monitor_file_;
111 float cfg_target_time_;
112 float cfg_target_ori_time_;
114 bool cfg_abort_on_error_;
115 bool cfg_enable_path_execution_;
125 bool target_reached_;
126 bool target_ori_reached_;
127 bool target_rotating_;
130 std::string last_node_;
133 bool constrained_plan_;
141 std::string error_reason_;
146 #ifdef HAVE_VISUALIZATION Thread aspect to access to BlackBoard.
Thread aspect that allows to obtain the current time from the clock.
virtual ~NavGraphThread()
Destructor.
NavGraphThread()
Constructor.
Fawkes library namespace.
A class for handling time.
Class representing a path for a NavGraph.
Send Marker messages to rviz to show navgraph info.
Thread class encapsulation of pthreads.
virtual void init()
Initialize the thread.
Thread aspect to use blocked timing.
Thread to perform graph-based path planning.
virtual void once()
Execute an action exactly once.
File Alteration Monitor Listener.
Monitors files for changes.
Thread aspect to log output.
Sub-class representing a navgraph path traversal.
virtual void loop()
Code to execute in the thread.
virtual void finalize()
Finalize the thread.
Thread aspect provide a new aspect.
Thread aspect to access configuration data.
NavGraphAspect initializer/finalizer.
virtual void fam_event(const char *filename, unsigned int mask)
Event has been raised.
NavPathInterface Fawkes BlackBoard Interface.
virtual void run()
Stub to see name in backtrace for easier debugging.
void start(bool wait=true)
Call this method to start the thread.
NavigatorInterface Fawkes BlackBoard Interface.