fawkes_navgraph_node.cpp
Go to the documentation of this file.
1 /***************************************************************************
2  * navgraph_node.cpp - navgraph access from ROS
3  *
4  * Created: Thu Mar 09 20:28:10 2017
5  * Copyright 2017 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 <ros/ros.h>
22 
23 // from Fawkes
24 #include <core/exception.h>
25 #include <core/utils/lockptr.h>
26 #include <core/threading/mutex_locker.h>
27 #include <utils/system/fam.h>
28 #include <navgraph/navgraph.h>
29 #include <navgraph/yaml_navgraph.h>
30 
31 #include <fawkes_msgs/NavGraph.h>
32 #include <fawkes_msgs/NavGraphGetPairwiseCosts.h>
33 #include <fawkes_msgs/NavGraphSearchPath.h>
34 
35 #include <boost/filesystem.hpp>
36 
37 #include <tf/transform_listener.h>
38 
39 #define GET_CONFIG(privn, n, path, var) \
40  if (! privn.getParam(path, var)) { \
41  n.getParam(path, var); \
42  }
43 
44 
45 class TransformListenerExtended : public tf::TransformListener
46 {
47  public:
48  TransformListenerExtended() : TransformListener() {}
49 
50  bool transform_origin(const std::string& source_frame,
51  const std::string& target_frame,
52  tf::Stamped<tf::Pose>& stamped_out)
53  {
54  tf::Stamped<tf::Pose> ident(tf::Transform(tf::Quaternion(0, 0, 0, 1),
55  tf::Vector3(0, 0, 0)),
56  ros::Time(0,0), source_frame);
57 
58  transformPose(target_frame, ident, stamped_out);
59  return true;
60  }
61 };
62 
63 class NavGraphRosNode : public fawkes::FamListener
64 {
65  public:
66  NavGraphRosNode(ros::NodeHandle &n)
67  : n(n)
68  {
69  ros::NodeHandle privn("~");
70  GET_CONFIG(privn, n, "navgraph_file", cfg_navgraph_file_);
71  GET_CONFIG(privn, n, "base_frame_id", cfg_base_frame_);
72  GET_CONFIG(privn, n, "global_frame_id", cfg_global_frame_);
73 
74  boost::filesystem::path p(cfg_navgraph_file_);
75  p = boost::filesystem::absolute(p);
76  cfg_navgraph_file_ = p.string();
77  if (boost::filesystem::exists(p)) {
78  navgraph = fawkes::LockPtr<fawkes::NavGraph>(fawkes::load_yaml_navgraph(cfg_navgraph_file_),
79  /* recursive mutex */ true);
80  ROS_INFO("[F-NavGraph] Loaded navgraph from '%s': %zu nodes, %zu edges",
81  cfg_navgraph_file_.c_str(), navgraph->nodes().size(), navgraph->edges().size());
82  } else {
83  navgraph = fawkes::LockPtr<fawkes::NavGraph>(new fawkes::NavGraph("empty"), /* recursive mutex */ true);
84  }
85 
86  boost::filesystem::create_directories(p.parent_path());
87  fam_ = new fawkes::FileAlterationMonitor();
88  fam_->add_filter((std::string("^") + p.filename().string() + "$").c_str());
89  printf("Watching dir %s\n", p.parent_path().string().c_str());
90  fam_->watch_dir(p.parent_path().string().c_str());
91  //printf("Watching file %s\n", cfg_navgraph_file_.c_str());
92  //fam_->watch_file(cfg_navgraph_file_.c_str());
93  fam_->add_listener(this);
94 
95  tf_listener = new TransformListenerExtended();
96 
97  pub_navgraph_ = n.advertise<fawkes_msgs::NavGraph>("navgraph", 10, /* latching */ true);
98  svs_search_path_ = n.advertiseService("navgraph/search_path",
99  &NavGraphRosNode::svs_search_path_cb, this);
100  svs_get_pwcosts_ = n.advertiseService("navgraph/get_pairwise_costs",
101  &NavGraphRosNode::svs_get_pwcosts_cb, this);
102 
103  fam_timer_ =
104  n.createWallTimer(ros::WallDuration(1.0), &NavGraphRosNode::cb_fam_timer, this);
105 
106  publish_graph();
107 }
108 
109  virtual ~NavGraphRosNode()
110  {
111  delete fam_;
112  delete tf_listener;
113  }
114 
115  virtual void
116  fam_event(const char *filename, unsigned int mask)
117  {
118  // The file will be ignored from now onwards, re-register
119  // if (mask & FAM_IGNORED) {
120  // boost::filesystem::path p(cfg_navgraph_file_);
121  // fam_->watch_dir(p.parent_path().string().c_str());
122  // }
123 
124  if (mask & FAM_DELETE) {
125  ROS_INFO("[F-NavGraph] Navgraph file deleted, clearing");
126  navgraph->clear();
127  publish_graph();
128  return;
129  }
130 
131  if (mask & (FAM_MODIFY | FAM_IGNORED)) {
132  ROS_INFO("[F-NavGraph] NavGraph changed on disk, reloading");
133 
134  try {
135  fawkes::LockPtr<fawkes::NavGraph> new_graph =
136  fawkes::LockPtr<fawkes::NavGraph>(fawkes::load_yaml_navgraph(cfg_navgraph_file_),
137  /* recursive mutex */ true);
138  ROS_INFO("[F-NavGraph] Re-loaded navgraph from '%s': %zu nodes, %zu edges",
139  cfg_navgraph_file_.c_str(), new_graph->nodes().size(), new_graph->edges().size());
140  **navgraph = **new_graph;
141  publish_graph();
142  } catch (fawkes::Exception &e) {
143  ROS_WARN("[F-NavGraph] Loading new graph failed: %s", e.what());
144  return;
145  } catch (std::runtime_error &e) {
146  ROS_WARN("[F-NavGraph] Loading new graph failed: %s", e.what());
147  return;
148  }
149  }
150  }
151 
152  void
153  cb_fam_timer(const ros::WallTimerEvent& event)
154  {
155  fam_->process_events();
156  }
157 
158  void
159  convert_nodes(const std::vector<fawkes::NavGraphNode> &nodes,
160  std::vector<fawkes_msgs::NavGraphNode> &out)
161  {
162  for (const fawkes::NavGraphNode &node : nodes) {
163  fawkes_msgs::NavGraphNode ngn;
164  ngn.name = node.name();
165  ngn.has_orientation = node.has_property(fawkes::navgraph::PROP_ORIENTATION);
166  ngn.pose.x = node.x();
167  ngn.pose.y = node.y();
168  if (ngn.has_orientation) {
169  ngn.pose.theta = node.property_as_float(fawkes::navgraph::PROP_ORIENTATION);
170  }
171  ngn.unconnected = node.unconnected();
172  const std::map<std::string, std::string> &props = node.properties();
173  for (const auto p : props) {
174  fawkes_msgs::NavGraphProperty ngp;
175  ngp.key = p.first;
176  ngp.value = p.second;
177  ngn.properties.push_back(ngp);
178  }
179  out.push_back(ngn);
180  }
181  }
182 
183  void
184  publish_graph()
185  {
186  fawkes::MutexLocker lock(navgraph.objmutex_ptr());
187 
188  fawkes_msgs::NavGraph ngm;
189 
190  const std::vector<fawkes::NavGraphNode> &nodes = navgraph->nodes();
191  convert_nodes(nodes, ngm.nodes);
192 
193  const std::vector<fawkes::NavGraphEdge> &edges = navgraph->edges();
194  for (const fawkes::NavGraphEdge &edge : edges) {
195  fawkes_msgs::NavGraphEdge nge;
196  nge.from_node = edge.from();
197  nge.to_node = edge.to();
198  nge.directed = edge.is_directed();
199  const std::map<std::string, std::string> &props = edge.properties();
200  for (const auto p : props) {
201  fawkes_msgs::NavGraphProperty ngp;
202  ngp.key = p.first;
203  ngp.value = p.second;
204  nge.properties.push_back(ngp);
205  }
206  ngm.edges.push_back(nge);
207  }
208 
209  pub_navgraph_.publish(ngm);
210  }
211 
212 
213  bool
214  svs_search_path_cb(fawkes_msgs::NavGraphSearchPath::Request &req,
215  fawkes_msgs::NavGraphSearchPath::Response &res)
216  {
217  fawkes::NavGraphNode from, to;
218 
219 
220  if (req.from_node.empty()) {
221  tf::Stamped<tf::Pose> pose;
222  if (! tf_listener->transform_origin(cfg_base_frame_, cfg_global_frame_, pose)) {
223  ROS_WARN("[F-NavGraph] Failed to compute pose, cannot generate plan");
224 
225  res.ok = false;
226  res.errmsg = "Failed to compute pose, cannot generate plan";
227  return true;
228  }
229 
230  from =
231  navgraph->closest_node(pose.getOrigin().x(), pose.getOrigin().y());
232  if (! from.is_valid()) {
233  res.ok = false;
234  res.errmsg = "Failed to get closest node to pose";
235  return true;
236  }
237 
238  fawkes_msgs::NavGraphNode free_start;
239  free_start.name = "free-start";
240  free_start.pose.x = pose.getOrigin().x();
241  free_start.pose.y = pose.getOrigin().y();
242  free_start.has_orientation = true;
243  free_start.pose.theta = tf::getYaw(pose.getRotation());
244  res.path.push_back(free_start);
245  } else {
246  from = navgraph->node(req.from_node);
247  if (! from.is_valid()) {
248  res.ok = false;
249  res.errmsg = "Failed to find start node " + req.from_node;
250  return true;
251  }
252  }
253 
254  fawkes::NavGraphPath path;
255 
256  if (! req.to_node.empty()) {
257  path = navgraph->search_path(from.name(), req.to_node);
258  } else {
259  fawkes::NavGraphNode close_to_goal = navgraph->closest_node(req.to_pose.x, req.to_pose.y);
260  path = navgraph->search_path(from, close_to_goal);
261  if (! path.empty()) {
262  fawkes::NavGraphNode free_target("free-target", req.to_pose.x, req.to_pose.y);
263  if (std::isfinite(req.to_pose.theta)) {
264  free_target.set_property("orientation", (float)req.to_pose.theta);
265  }
266  path.add_node(free_target, navgraph->cost(path.nodes().back(), free_target));
267  }
268  }
269 
270  // translate path into result
271  convert_nodes(path.nodes(), res.path);
272  res.cost = path.cost();
273 
274  res.ok = true;
275  return true;
276  }
277 
278  bool
279  svs_get_pwcosts_cb(fawkes_msgs::NavGraphGetPairwiseCosts::Request &req,
280  fawkes_msgs::NavGraphGetPairwiseCosts::Response &res)
281  {
282  for (unsigned int i = 0; i < req.nodes.size(); ++i) {
283  for (unsigned int j = 0; j < req.nodes.size(); ++j) {
284  if (i == j) continue;
285 
286  fawkes::NavGraphNode from_node, to_node;
287  try {
288  from_node = navgraph->node(req.nodes[i]);
289  to_node = navgraph->node(req.nodes[j]);
290  } catch (fawkes::Exception &e) {
291  res.ok = false;
292  res.errmsg = "Failed to get path from '" + req.nodes[i] + "' to '" +
293  req.nodes[j] + "': " + e.what_no_backtrace();
294  res.path_costs.clear();
295  return true;
296  }
297 
298  fawkes::NavGraphNode start_node, goal_node;
299 
300  if (from_node.unconnected()) {
301  start_node = navgraph->closest_node_to(from_node.name());
302  //ROS_WARN("[F-NavGraph] From node %s is UNCONNECTED, starting instead from %s",
303  // from_node.name().c_str(), start_node.name().c_str());
304  } else {
305  start_node = from_node;
306  }
307  if (to_node.unconnected()) {
308  goal_node = navgraph->closest_node_to(to_node.name());
309  //ROS_WARN("[F-NavGraph] To node %s is UNCONNECTED, ending instead at %s",
310  // to_node.name().c_str(), goal_node.name().c_str());
311  } else {
312  goal_node = to_node;
313  }
314  fawkes::NavGraphPath p = navgraph->search_path(start_node, goal_node);
315  if (p.empty()) {
316  res.ok = false;
317  res.errmsg = "Failed to get path from '" + start_node.name() + "' to '" + goal_node.name() + "'";
318  res.path_costs.clear();
319  return true;
320  }
321  fawkes_msgs::NavGraphPathCost pc;
322  pc.from_node = req.nodes[i];
323  pc.to_node = req.nodes[j];
324  pc.cost = p.cost();
325  if (from_node.unconnected()) {
326  pc.cost += navgraph->cost(from_node, start_node);
327  }
328  if (to_node.unconnected()) {
329  pc.cost += navgraph->cost(goal_node, to_node);
330  }
331  res.path_costs.push_back(pc);
332  }
333  }
334 
335  res.ok = true;
336  return true;
337  }
338 
339 
340  private:
341  ros::NodeHandle n;
342 
343  std::string cfg_navgraph_file_;
344  std::string cfg_base_frame_;
345  std::string cfg_global_frame_;
346 
347  fawkes::LockPtr<fawkes::NavGraph> navgraph;
348  fawkes::FileAlterationMonitor *fam_;
349 
350  ros::Publisher pub_navgraph_;
351  ros::ServiceServer svs_search_path_;
352  ros::ServiceServer svs_get_pwcosts_;
353  ros::WallTimer fam_timer_;
354 
355  TransformListenerExtended *tf_listener;
356 };
357 
358 int
359 main(int argc, char **argv)
360 {
361  ros::init(argc, argv, "navgraph_node");
362 
363  ros::NodeHandle n;
364  NavGraphRosNode navgraph_node(n);
365  ros::spin();
366 
367  return 0;
368 }
transform_listener.h
GET_CONFIG
#define GET_CONFIG(privn, n, path, var)
Definition: fawkes_navgraph_node.cpp:39
ros::NodeHandle
NavGraphRosNode::svs_search_path_
ros::ServiceServer svs_search_path_
Definition: fawkes_navgraph_node.cpp:351
tf::getYaw
static double getYaw(const Quaternion &bt_q)
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
NavGraphRosNode::fam_event
virtual void fam_event(const char *filename, unsigned int mask)
Definition: fawkes_navgraph_node.cpp:116
ros::NodeHandle::advertiseService
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
NavGraphRosNode::cb_fam_timer
void cb_fam_timer(const ros::WallTimerEvent &event)
Definition: fawkes_navgraph_node.cpp:153
ROS_WARN
#define ROS_WARN(...)
NavGraphRosNode::svs_get_pwcosts_cb
bool svs_get_pwcosts_cb(fawkes_msgs::NavGraphGetPairwiseCosts::Request &req, fawkes_msgs::NavGraphGetPairwiseCosts::Response &res)
Definition: fawkes_navgraph_node.cpp:279
NavGraphRosNode::cfg_navgraph_file_
std::string cfg_navgraph_file_
Definition: fawkes_navgraph_node.cpp:343
ros::spin
ROSCPP_DECL void spin(Spinner &spinner)
NavGraphRosNode::svs_get_pwcosts_
ros::ServiceServer svs_get_pwcosts_
Definition: fawkes_navgraph_node.cpp:352
tf::TransformListener::TransformListener
TransformListener(ros::Duration max_cache_time=ros::Duration(DEFAULT_CACHE_TIME), bool spin_thread=true)
ROS_INFO
#define ROS_INFO(...)
NavGraphRosNode::fam_timer_
ros::WallTimer fam_timer_
Definition: fawkes_navgraph_node.cpp:353
NavGraphRosNode::fam_
fawkes::FileAlterationMonitor * fam_
Definition: fawkes_navgraph_node.cpp:348
ros::NodeHandle::createWallTimer
WallTimer createWallTimer(WallDuration period, void(T::*callback)(const WallTimerEvent &), T *obj, bool oneshot=false, bool autostart=true) const
NavGraphRosNode::navgraph
fawkes::LockPtr< fawkes::NavGraph > navgraph
Definition: fawkes_navgraph_node.cpp:347
ros::NodeHandle::advertise
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
NavGraphRosNode::pub_navgraph_
ros::Publisher pub_navgraph_
Definition: fawkes_navgraph_node.cpp:350
NavGraphRosNode::tf_listener
TransformListenerExtended * tf_listener
Definition: fawkes_navgraph_node.cpp:355
NavGraphRosNode::NavGraphRosNode
NavGraphRosNode(ros::NodeHandle &n)
Definition: fawkes_navgraph_node.cpp:66
tf::TransformListener::transformPose
void transformPose(const std::string &target_frame, const geometry_msgs::PoseStamped &stamped_in, geometry_msgs::PoseStamped &stamped_out) const
TransformListenerExtended::transform_origin
bool transform_origin(const std::string &source_frame, const std::string &target_frame, tf::Stamped< tf::Pose > &stamped_out)
Definition: fawkes_navgraph_node.cpp:50
main
int main(int argc, char **argv)
Definition: fawkes_navgraph_node.cpp:359
NavGraphRosNode::convert_nodes
void convert_nodes(const std::vector< fawkes::NavGraphNode > &nodes, std::vector< fawkes_msgs::NavGraphNode > &out)
Definition: fawkes_navgraph_node.cpp:159
NavGraphRosNode::svs_search_path_cb
bool svs_search_path_cb(fawkes_msgs::NavGraphSearchPath::Request &req, fawkes_msgs::NavGraphSearchPath::Response &res)
Definition: fawkes_navgraph_node.cpp:214

rcll_fawkes_sim
Author(s): Tim Niemueller
autogenerated on Mon Jun 10 2019 14:31:10