21 #ifndef _PLUGINS_NAVGRAPH_NAVGRAPH_ROSPUB_THREAD_H_
22 #define _PLUGINS_NAVGRAPH_NAVGRAPH_ROSPUB_THREAD_H_
24 #include <aspect/clock.h>
25 #include <aspect/configurable.h>
26 #include <aspect/logging.h>
27 #include <aspect/tf.h>
28 #include <core/threading/thread.h>
29 #include <fawkes_msgs/NavGraphGetPairwiseCosts.h>
30 #include <fawkes_msgs/NavGraphSearchPath.h>
31 #include <navgraph/aspect/navgraph.h>
32 #include <navgraph/navgraph.h>
33 #include <plugins/ros/aspect/ros.h>
34 #include <ros/publisher.h>
35 #include <ros/service_server.h>
66 void convert_nodes(
const std::vector<fawkes::NavGraphNode> &nodes,
67 std::vector<fawkes_msgs::NavGraphNode> & out);
69 bool svs_search_path_cb(fawkes_msgs::NavGraphSearchPath::Request & req,
70 fawkes_msgs::NavGraphSearchPath::Response &res);
71 bool svs_get_pwcosts_cb(fawkes_msgs::NavGraphGetPairwiseCosts::Request & req,
72 fawkes_msgs::NavGraphGetPairwiseCosts::Response &res);
75 std::string cfg_base_frame_;
76 std::string cfg_global_frame_;
79 ros::ServiceServer svs_search_path_;
80 ros::ServiceServer svs_get_pwcosts_;
Publish navgaraph to ROS.
NavGraphROSPubThread()
Constructor.
virtual void run()
Stub to see name in backtrace for easier debugging.
virtual void init()
Initialize the thread.
virtual ~NavGraphROSPubThread()
Destructor.
virtual void loop()
Code to execute in the thread.
virtual void finalize()
Finalize the thread.
virtual void graph_changed() noexcept
Function called if the graph has been changed.
Thread aspect that allows to obtain the current time from the clock.
Thread aspect to access configuration data.
Thread aspect to log output.
Thread aspect to access NavGraph.
Topological graph change listener.
Thread aspect to get access to a ROS node handle.
Thread class encapsulation of pthreads.