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 
31 using 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 
49 void
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 
68 void
70 {
71  navgraph->remove_change_listener(this);
72  pub_.shutdown();
73  svs_search_path_.shutdown();
74  svs_get_pwcosts_.shutdown();
75 }
76 
77 void
79 {
80 }
81 
82 void
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 
95 void
96 NavGraphROSPubThread::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 
120 void
121 NavGraphROSPubThread::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 
149 bool
150 NavGraphROSPubThread::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 
212 bool
213 NavGraphROSPubThread::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 }
fawkes::NavGraphNode
Definition: navgraph_node.h:40
NavGraphROSPubThread::graph_changed
virtual void graph_changed()
Definition: rospub_thread.cpp:83
fawkes::NavGraphEdge
Definition: navgraph_edge.h:42
fawkes::ROSAspect::rosnode
LockPtr< ros::NodeHandle > rosnode
Definition: ros.h:47
fawkes::MutexLocker
Definition: mutex_locker.h:39
NavGraphROSPubThread::loop
virtual void loop()
Code to execute in the thread.
Definition: rospub_thread.cpp:78
fawkes::NavGraphPath::add_node
void add_node(const NavGraphNode &node, float cost_from_end=0)
Add a node to the path.
Definition: navgraph_path.cpp:123
fawkes::Thread::name
const char * name() const
Definition: thread.h:100
fawkes::NavGraphNode::name
const std::string & name() const
Get name of node.
Definition: navgraph_node.h:60
fawkes::tf::Stamped< fawkes::tf::Pose >
fawkes::NavGraphNode::unconnected
bool unconnected() const
Check if this node shall be unconnected.
Definition: navgraph_node.h:84
fawkes::LoggingAspect::logger
Logger * logger
Definition: logging.h:53
NavGraphROSPubThread::NavGraphROSPubThread
NavGraphROSPubThread()
Constructor.
Definition: rospub_thread.cpp:39
fawkes::NavGraphPath::cost
float cost() const
Get cost of path from start to to end.
Definition: navgraph_path.h:138
fawkes
fawkes::NavGraphPath::nodes
const std::vector< NavGraphNode > & nodes() const
Get nodes along the path.
Definition: navgraph_path.h:105
fawkes::Logger::log_warn
virtual void log_warn(const char *component, const char *format,...)=0
fawkes::NavGraphNode::is_valid
bool is_valid() const
Check if node is valid, i.e.
Definition: navgraph_node.h:136
fawkes::ConfigurableAspect::config
Configuration * config
Definition: configurable.h:53
fawkes::Exception::what_no_backtrace
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
Definition: exception.cpp:663
fawkes::TransformAspect::tf_listener
tf::Transformer * tf_listener
Definition: tf.h:72
NavGraphROSPubThread::init
virtual void init()
Initialize the thread.
Definition: rospub_thread.cpp:50
fawkes::Thread
Definition: thread.h:45
fawkes::NavGraphAspect::navgraph
fawkes::LockPtr< NavGraph > navgraph
Definition: navgraph.h:50
fawkes::Configuration::get_string
virtual std::string get_string(const char *path)=0
fawkes::NavGraphPath
Definition: navgraph_path.h:41
fawkes::NavGraphPath::empty
bool empty() const
Check if path is empty.
Definition: navgraph_path.cpp:150
NavGraphROSPubThread::~NavGraphROSPubThread
virtual ~NavGraphROSPubThread()
Destructor.
Definition: rospub_thread.cpp:45
fawkes::tf::Transformer::transform_origin
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.
Definition: transformer.cpp:478
NavGraphROSPubThread::finalize
virtual void finalize()
Finalize the thread.
Definition: rospub_thread.cpp:69
fawkes::Exception
Definition: exception.h:41