Fawkes API  Fawkes Development Version
rospub_thread.cpp
1 /***************************************************************************
2  * navgraph_interactive_thread.cpp - ROSPub navgraph editing
3  *
4  * Created: Thu Jan 15 16:26:40 2015
5  * Copyright 2015 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 #include "rospub_thread.h"
22 
23 #include <core/threading/mutex_locker.h>
24 
25 #include <ros/ros.h>
26 #include <fawkes_msgs/NavGraph.h>
27 #include <fawkes_msgs/NavGraphNode.h>
28 #include <fawkes_msgs/NavGraphEdge.h>
29 
30 using namespace fawkes;
31 
32 /** @class NavGraphROSPubThread "rospub_thread.h"
33  * Publish navgaraph to ROS.
34  * @author Tim Niemueller
35  */
36 
37 /** Constructor. */
39  : Thread("NavGraphROSPubThread", Thread::OPMODE_WAITFORWAKEUP)
40 {
41 }
42 
43 /** Destructor. */
45 {
46 }
47 
48 
49 void
51 {
52  pub_ = rosnode->advertise<fawkes_msgs::NavGraph>("navgraph", 10, /* latching */ true);
53 
54  publish_graph();
55 
56  navgraph->add_change_listener(this);
57 }
58 
59 void
61 {
62  pub_.shutdown();
63 }
64 
65 void
67 {
68  navgraph->remove_change_listener(this);
69 }
70 
71 
72 void
74 {
75  try {
76  publish_graph();
77  } catch (fawkes::Exception &e) {
78  logger->log_warn(name(), "Failed to publish graph, exception follows");
79  logger->log_warn(name(), e);
80  } catch (std::runtime_error &e) {
81  logger->log_warn(name(), "Failed to publish graph: %s", e.what());
82  }
83 }
84 
85 void
86 NavGraphROSPubThread::publish_graph()
87 {
88  MutexLocker lock(navgraph.objmutex_ptr());
89 
90  fawkes_msgs::NavGraph ngm;
91 
92  const std::vector<NavGraphNode> &nodes = navgraph->nodes();
93  for (const NavGraphNode &node : 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);
101  }
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;
106  ngp.key = p.first;
107  ngp.value = p.second;
108  ngn.properties.push_back(ngp);
109  }
110  ngm.nodes.push_back(ngn);
111  }
112  const std::vector<NavGraphEdge> &edges = navgraph->edges();
113  for (const NavGraphEdge &edge : 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;
121  ngp.key = p.first;
122  ngp.value = p.second;
123  nge.properties.push_back(ngp);
124  }
125  ngm.edges.push_back(nge);
126  }
127 
128  pub_.publish(ngm);
129 }
NavGraphROSPubThread()
Constructor.
fawkes::LockPtr< NavGraph > navgraph
NavGraph instance shared in framework.
Definition: navgraph.h:46
virtual void init()
Initialize the thread.
Fawkes library namespace.
Mutex locking helper.
Definition: mutex_locker.h:33
Thread class encapsulation of pthreads.
Definition: thread.h:42
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
virtual void finalize()
Finalize the thread.
Base class for exceptions in Fawkes.
Definition: exception.h:36
const char * name() const
Get name of thread.
Definition: thread.h:95
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void loop()
Code to execute in the thread.
Topological graph edge.
Definition: navgraph_edge.h:39
Topological graph node.
Definition: navgraph_node.h:38
virtual ~NavGraphROSPubThread()
Destructor.
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
Definition: ros.h:48
virtual void graph_changed()
Function called if the graph has been changed.