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#include <fawkes_msgs/NavGraph.h>
25#include <fawkes_msgs/NavGraphEdge.h>
26#include <fawkes_msgs/NavGraphNode.h>
27#include <ros/ros.h>
28
29#include <cmath>
30
31using namespace fawkes;
32
33/** @class NavGraphROSPubThread "rospub_thread.h"
34 * Publish navgaraph to ROS.
35 * @author Tim Niemueller
36 */
37
38/** Constructor. */
40: Thread("NavGraphROSPubThread", Thread::OPMODE_WAITFORWAKEUP)
41{
42}
43
44/** Destructor. */
46{
47}
48
49void
51{
52 cfg_base_frame_ = config->get_string("/frames/base");
53 cfg_global_frame_ = config->get_string("/frames/fixed");
54
55 pub_ = rosnode->advertise<fawkes_msgs::NavGraph>("navgraph", 10, /* latching */ true);
56 svs_search_path_ = rosnode->advertiseService("navgraph/search_path",
57 &NavGraphROSPubThread::svs_search_path_cb,
58 this);
59 svs_get_pwcosts_ = rosnode->advertiseService("navgraph/get_pairwise_costs",
60 &NavGraphROSPubThread::svs_get_pwcosts_cb,
61 this);
62
63 publish_graph();
64
65 navgraph->add_change_listener(this);
66}
67
68void
70{
71 navgraph->remove_change_listener(this);
72 pub_.shutdown();
73 svs_search_path_.shutdown();
74 svs_get_pwcosts_.shutdown();
75}
76
77void
79{
80}
81
82void
84{
85 try {
86 publish_graph();
87 } catch (fawkes::Exception &e) {
88 logger->log_warn(name(), "Failed to publish graph, exception follows");
89 logger->log_warn(name(), e);
90 } catch (std::runtime_error &e) {
91 logger->log_warn(name(), "Failed to publish graph: %s", e.what());
92 }
93}
94
95void
96NavGraphROSPubThread::convert_nodes(const std::vector<fawkes::NavGraphNode> &nodes,
97 std::vector<fawkes_msgs::NavGraphNode> & out)
98{
99 for (const NavGraphNode &node : nodes) {
100 fawkes_msgs::NavGraphNode ngn;
101 ngn.name = node.name();
102 ngn.has_orientation = node.has_property(navgraph::PROP_ORIENTATION);
103 ngn.pose.x = node.x();
104 ngn.pose.y = node.y();
105 if (ngn.has_orientation) {
106 ngn.pose.theta = node.property_as_float(navgraph::PROP_ORIENTATION);
107 }
108 ngn.unconnected = node.unconnected();
109 const std::map<std::string, std::string> &props = node.properties();
110 for (const auto p : props) {
111 fawkes_msgs::NavGraphProperty ngp;
112 ngp.key = p.first;
113 ngp.value = p.second;
114 ngn.properties.push_back(ngp);
115 }
116 out.push_back(ngn);
117 }
118}
119
120void
121NavGraphROSPubThread::publish_graph()
122{
123 MutexLocker lock(navgraph.objmutex_ptr());
124
125 fawkes_msgs::NavGraph ngm;
126
127 const std::vector<NavGraphNode> &nodes = navgraph->nodes();
128 convert_nodes(nodes, ngm.nodes);
129
130 const std::vector<NavGraphEdge> &edges = navgraph->edges();
131 for (const NavGraphEdge &edge : edges) {
132 fawkes_msgs::NavGraphEdge nge;
133 nge.from_node = edge.from();
134 nge.to_node = edge.to();
135 nge.directed = edge.is_directed();
136 const std::map<std::string, std::string> &props = edge.properties();
137 for (const auto p : props) {
138 fawkes_msgs::NavGraphProperty ngp;
139 ngp.key = p.first;
140 ngp.value = p.second;
141 nge.properties.push_back(ngp);
142 }
143 ngm.edges.push_back(nge);
144 }
145
146 pub_.publish(ngm);
147}
148
149bool
150NavGraphROSPubThread::svs_search_path_cb(fawkes_msgs::NavGraphSearchPath::Request & req,
151 fawkes_msgs::NavGraphSearchPath::Response &res)
152{
153 NavGraphNode from, to;
154
155 if (req.from_node.empty()) {
157 if (!tf_listener->transform_origin(cfg_base_frame_, cfg_global_frame_, pose)) {
158 logger->log_warn(name(), "Failed to compute pose, cannot generate plan");
159
160 res.ok = false;
161 res.errmsg = "Failed to compute pose, cannot generate plan";
162 return true;
163 }
164
165 from = navgraph->closest_node(pose.getOrigin().x(), pose.getOrigin().y());
166 if (!from.is_valid()) {
167 res.ok = false;
168 res.errmsg = "Failed to get closest node to pose";
169 return true;
170 }
171
172 fawkes_msgs::NavGraphNode free_start;
173 free_start.name = "free-start";
174 free_start.pose.x = pose.getOrigin().x();
175 free_start.pose.y = pose.getOrigin().y();
176 free_start.has_orientation = true;
177 free_start.pose.theta = tf::get_yaw(pose.getRotation());
178 res.path.push_back(free_start);
179 } else {
180 from = navgraph->node(req.from_node);
181 if (!from.is_valid()) {
182 res.ok = false;
183 res.errmsg = "Failed to find start node " + req.from_node;
184 return true;
185 }
186 }
187
188 NavGraphPath path;
189
190 if (!req.to_node.empty()) {
191 path = navgraph->search_path(from.name(), req.to_node);
192 } else {
193 NavGraphNode close_to_goal = navgraph->closest_node(req.to_pose.x, req.to_pose.y);
194 path = navgraph->search_path(from, close_to_goal);
195 if (!path.empty()) {
196 NavGraphNode free_target("free-target", req.to_pose.x, req.to_pose.y);
197 if (std::isfinite(req.to_pose.theta)) {
198 free_target.set_property("orientation", (float)req.to_pose.theta);
199 }
200 path.add_node(free_target, navgraph->cost(path.nodes().back(), free_target));
201 }
202 }
203
204 // translate path into result
205 convert_nodes(path.nodes(), res.path);
206 res.cost = path.cost();
207
208 res.ok = true;
209 return true;
210}
211
212bool
213NavGraphROSPubThread::svs_get_pwcosts_cb(fawkes_msgs::NavGraphGetPairwiseCosts::Request & req,
214 fawkes_msgs::NavGraphGetPairwiseCosts::Response &res)
215{
216 for (unsigned int i = 0; i < req.nodes.size(); ++i) {
217 for (unsigned int j = 0; j < req.nodes.size(); ++j) {
218 if (i == j)
219 continue;
220
221 fawkes::NavGraphNode from_node, to_node;
222 try {
223 from_node = navgraph->node(req.nodes[i]);
224 to_node = navgraph->node(req.nodes[j]);
225 } catch (fawkes::Exception &e) {
226 res.ok = false;
227 res.errmsg = "Failed to get path from '" + req.nodes[i] + "' to '" + req.nodes[j]
228 + "': " + e.what_no_backtrace();
229 res.path_costs.clear();
230 return true;
231 }
232
233 fawkes::NavGraphNode start_node, goal_node;
234
235 if (from_node.unconnected()) {
236 start_node = navgraph->closest_node_to(from_node.name());
237 //logger->log_warn(name(), "[F-NavGraph] From node %s is UNCONNECTED, starting instead from %s",
238 // from_node.name().c_str(), start_node.name().c_str());
239 } else {
240 start_node = from_node;
241 }
242 if (to_node.unconnected()) {
243 goal_node = navgraph->closest_node_to(to_node.name());
244 //logger->log_warn(name(), "[F-NavGraph] To node %s is UNCONNECTED, ending instead at %s",
245 // to_node.name().c_str(), goal_node.name().c_str());
246 } else {
247 goal_node = to_node;
248 }
249 fawkes::NavGraphPath p = navgraph->search_path(start_node, goal_node);
250 if (p.empty()) {
251 res.ok = false;
252 res.errmsg =
253 "Failed to get path from '" + start_node.name() + "' to '" + goal_node.name() + "'";
254 res.path_costs.clear();
255 return true;
256 }
257 fawkes_msgs::NavGraphPathCost pc;
258 pc.from_node = req.nodes[i];
259 pc.to_node = req.nodes[j];
260 pc.cost = p.cost();
261 if (from_node.unconnected()) {
262 pc.cost += navgraph->cost(from_node, start_node);
263 }
264 if (to_node.unconnected()) {
265 pc.cost += navgraph->cost(goal_node, to_node);
266 }
267 res.path_costs.push_back(pc);
268 }
269 }
270
271 res.ok = true;
272 return true;
273}
NavGraphROSPubThread()
Constructor.
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.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual const char * what() const noexcept
Get primary string.
Definition: exception.cpp:639
virtual const char * what_no_backtrace() const noexcept
Get primary string (does not implicitly print the back trace).
Definition: exception.cpp:663
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
Mutex locking helper.
Definition: mutex_locker.h:34
fawkes::LockPtr< NavGraph > navgraph
NavGraph instance shared in framework.
Definition: navgraph.h:44
Topological graph edge.
Definition: navgraph_edge.h:38
Topological graph node.
Definition: navgraph_node.h:36
bool unconnected() const
Check if this node shall be unconnected.
Definition: navgraph_node.h:74
bool is_valid() const
Check if node is valid, i.e.
const std::string & name() const
Get name of node.
Definition: navgraph_node.h:50
Class representing a path for a NavGraph.
Definition: navgraph_path.h:37
bool empty() const
Check if path is empty.
const std::vector< NavGraphNode > & nodes() const
Get nodes along the path.
float cost() const
Get cost of path from start to to end.
void add_node(const NavGraphNode &node, float cost_from_end=0)
Add a node to the path.
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
Definition: ros.h:47
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
tf::Transformer * tf_listener
This is the transform listener which saves transforms published by other threads in the system.
Definition: tf.h:67
bool transform_origin(const std::string &source_frame, const std::string &target_frame, Stamped< Pose > &stamped_out, const fawkes::Time time=fawkes::Time(0, 0)) const
Transform ident pose from one frame to another.
Fawkes library namespace.