21 #include "rospub_thread.h" 23 #include <core/threading/mutex_locker.h> 26 #include <fawkes_msgs/NavGraph.h> 27 #include <fawkes_msgs/NavGraphNode.h> 28 #include <fawkes_msgs/NavGraphEdge.h> 39 :
Thread(
"NavGraphROSPubThread",
Thread::OPMODE_WAITFORWAKEUP)
52 pub_ =
rosnode->advertise<fawkes_msgs::NavGraph>(
"navgraph", 10,
true);
68 navgraph->remove_change_listener(
this);
80 }
catch (std::runtime_error &e) {
86 NavGraphROSPubThread::publish_graph()
90 fawkes_msgs::NavGraph ngm;
92 const std::vector<NavGraphNode> &nodes =
navgraph->nodes();
94 fawkes_msgs::NavGraphNode ngn;
95 ngn.name = node.name();
96 ngn.has_orientation = node.has_property(navgraph::PROP_ORIENTATION);
97 ngn.pose.x = node.x();
98 ngn.pose.y = node.y();
99 if (ngn.has_orientation) {
100 ngn.pose.theta = node.property_as_float(navgraph::PROP_ORIENTATION);
102 ngn.unconnected = node.unconnected();
103 const std::map<std::string, std::string> &props = node.properties();
104 for (
const auto p : props) {
105 fawkes_msgs::NavGraphProperty ngp;
107 ngp.value = p.second;
108 ngn.properties.push_back(ngp);
110 ngm.nodes.push_back(ngn);
112 const std::vector<NavGraphEdge> &edges =
navgraph->edges();
114 fawkes_msgs::NavGraphEdge nge;
115 nge.from_node = edge.from();
116 nge.to_node = edge.to();
117 nge.directed = edge.is_directed();
118 const std::map<std::string, std::string> &props = edge.properties();
119 for (
const auto p : props) {
120 fawkes_msgs::NavGraphProperty ngp;
122 ngp.value = p.second;
123 nge.properties.push_back(ngp);
125 ngm.edges.push_back(nge);
NavGraphROSPubThread()
Constructor.
fawkes::LockPtr< NavGraph > navgraph
NavGraph instance shared in framework.
virtual void init()
Initialize the thread.
Fawkes library namespace.
Thread class encapsulation of pthreads.
Logger * logger
This is the Logger member used to access the logger.
virtual void finalize()
Finalize the thread.
Base class for exceptions in Fawkes.
const char * name() const
Get name of thread.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void loop()
Code to execute in the thread.
virtual ~NavGraphROSPubThread()
Destructor.
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
virtual void graph_changed()
Function called if the graph has been changed.