Fawkes API  Fawkes Development Version
navgraph_thread.cpp
1 /***************************************************************************
2  * navgraph_thread.cpp - Graph-based global path planning
3  *
4  * Created: Tue Sep 18 16:00:34 2012
5  * Copyright 2012-2014 Tim Niemueller [www.niemueller.de]
6  * 2014 Tobias Neumann
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL file in the doc directory.
20  */
21 
22 #include "navgraph_thread.h"
23 
24 #include <navgraph/yaml_navgraph.h>
25 #include <navgraph/constraints/constraint_repo.h>
26 #include <utils/math/angle.h>
27 #include <tf/utils.h>
28 #include <core/utils/lockptr.h>
29 
30 #include <fstream>
31 #include <cmath>
32 
33 using namespace fawkes;
34 
35 /** @class NavGraphThread "navgraph_thread.h"
36  * Thread to perform graph-based path planning.
37  * @author Tim Niemueller
38  */
39 
40 /** Constructor. */
42  : Thread("NavGraphThread", Thread::OPMODE_WAITFORWAKEUP),
43  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_ACT),
44  AspectProviderAspect(&navgraph_aspect_inifin_)
45 {
46 #ifdef HAVE_VISUALIZATION
47  vt_ = NULL;
48 #endif
49 }
50 
51 #ifdef HAVE_VISUALIZATION
52 /** Constructor. */
54  : Thread("NavGraphThread", Thread::OPMODE_WAITFORWAKEUP),
55  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_ACT),
56  AspectProviderAspect(&navgraph_aspect_inifin_)
57 {
58  vt_ = vt;
59 }
60 #endif
61 
62 /** Destructor. */
64 {
65 }
66 
67 void
69 {
70  try {
71  cfg_graph_file_ = config->get_string("/navgraph/graph_file");
72  } catch (Exception &e) {
73  logger->log_warn(name(), "No graph file given, will create empty one");
74  }
75  cfg_base_frame_ = config->get_string("/frames/base");
76  cfg_global_frame_ = config->get_string("/frames/fixed");
77  cfg_nav_if_id_ = config->get_string("/navgraph/navigator_interface_id");
78  cfg_resend_interval_ = config->get_float("/navgraph/resend_interval");
79  cfg_replan_interval_ = config->get_float("/navgraph/replan_interval");
80  cfg_replan_factor_ = config->get_float("/navgraph/replan_cost_factor");
81  cfg_target_time_ = config->get_float("/navgraph/target_time");
82  cfg_target_ori_time_ = config->get_float("/navgraph/target_ori_time");
83  cfg_log_graph_ = config->get_bool("/navgraph/log_graph");
84  cfg_abort_on_error_ = config->get_bool("/navgraph/abort_on_error");
85 #ifdef HAVE_VISUALIZATION
86  cfg_visual_interval_ = config->get_float("/navgraph/visualization_interval");
87 #endif
88  cfg_monitor_file_ = false;
89  try {
90  cfg_monitor_file_ = config->get_bool("/navgraph/monitor_file");
91  } catch (Exception &e) {} // ignored
92 
93  cfg_enable_path_execution_ = true;
94  try {
95  cfg_enable_path_execution_ = config->get_bool("/navgraph/path_execution");
96  } catch (Exception &e) {} // ignored
97 
98  if (config->exists("/navgraph/travel_tolerance") ||
99  config->exists("/navgraph/target_tolerance") ||
100  config->exists("/navgraph/orientation_tolerance") ||
101  config->exists("/navgraph/shortcut_tolerance"))
102  {
103  logger->log_error(name(), "Tolerances may no longe rbe set in the config.");
104  logger->log_error(name(), "The must be set as default properties in the graph.");
105  logger->log_error(name(), "Remove the following config values (move to navgraph):");
106  logger->log_error(name(), " /navgraph/travel_tolerance");
107  logger->log_error(name(), " /navgraph/target_tolerance");
108  logger->log_error(name(), " /navgraph/orientation_tolerance");
109  logger->log_error(name(), " /navgraph/shortcut_tolerance");
110  throw Exception("Navgraph tolerances may no longer be set in the config");
111  }
112 
113  if (cfg_enable_path_execution_) {
114  pp_nav_if_ = blackboard->open_for_writing<NavigatorInterface>("Pathplan");
115  nav_if_ = blackboard->open_for_reading<NavigatorInterface>(cfg_nav_if_id_.c_str());
116  path_if_ = blackboard->open_for_writing<NavPathInterface>("NavPath");
117  }
118 
119 
120  if (! cfg_graph_file_.empty()) {
121  if (cfg_graph_file_[0] != '/') {
122  cfg_graph_file_ = std::string(CONFDIR) + "/" + cfg_graph_file_;
123  }
124  graph_ = load_graph(cfg_graph_file_);
125  } else {
126  graph_ = LockPtr<NavGraph>(new NavGraph("generated"), /* recursive mutex */ true);
127  }
128 
129  if (! graph_->has_default_property("travel_tolerance")) {
130  throw Exception("Graph must specify travel tolerance");
131  }
132  if (! graph_->has_default_property("target_tolerance")) {
133  throw Exception("Graph must specify target tolerance");
134  }
135  if (! graph_->has_default_property("orientation_tolerance")) {
136  throw Exception("Graph must specify orientation tolerance");
137  }
138  if (! graph_->has_default_property("shortcut_tolerance")) {
139  throw Exception("Graph must specify shortcut tolerance");
140  }
141  if (graph_->has_default_property("target_time")) {
142  cfg_target_time_ = graph_->default_property_as_float("target_time");
143  logger->log_info(name(), "Using target time %f from graph file", cfg_target_time_);
144  }
145  if (graph_->has_default_property("target_ori_time")) {
146  cfg_target_time_ = graph_->default_property_as_float("target_ori_time");
147  logger->log_info(name(), "Using target orientation time %f from graph file", cfg_target_ori_time_);
148  }
149 
150  navgraph_aspect_inifin_.set_navgraph(graph_);
151  if (cfg_log_graph_) {
152  log_graph();
153  }
154 
155  if (cfg_monitor_file_) {
156  logger->log_info(name(), "Enabling graph file monitoring");
157  try {
158  fam_ = new FileAlterationMonitor();
159  fam_->watch_file(cfg_graph_file_.c_str());
160  fam_->add_listener(this);
161  } catch (Exception &e) {
162  logger->log_warn(name(), "Could not enable graph file monitoring");
163  logger->log_warn(name(), e);
164  }
165 
166  }
167 
168  exec_active_ = false;
169  target_reached_ = false;
170  target_ori_reached_= false;
171  target_rotating_ = false;
172  last_node_ = "";
173  error_reason_ = "";
174  constrained_plan_ = false;
175  cmd_sent_at_ = new Time(clock);
176  path_planned_at_ = new Time(clock);
177  target_reached_at_ = new Time(clock);
178  error_at_ = new Time(clock);
179 #ifdef HAVE_VISUALIZATION
180  visualized_at_ = new Time(clock);
181  if (vt_) {
182  graph_->add_change_listener(vt_);
183  }
184 #endif
185 
186  constraint_repo_ = graph_->constraint_repo();
187 }
188 
189 void
191 {
192  delete cmd_sent_at_;
193  delete path_planned_at_;
194  delete target_reached_at_;
195  delete error_at_;
196 #ifdef HAVE_VISUALIZATION
197  delete visualized_at_;
198  if (vt_) {
199  graph_->remove_change_listener(vt_);
200  }
201 #endif
202  graph_.clear();
203  if (cfg_enable_path_execution_) {
204  blackboard->close(pp_nav_if_);
205  blackboard->close(nav_if_);
206  blackboard->close(path_if_);
207  }
208 }
209 
210 void
212 {
213 #ifdef HAVE_VISUALIZATION
214  if (vt_) {
215  vt_->set_constraint_repo(constraint_repo_);
216  vt_->set_graph(graph_);
217  }
218 #endif
219 }
220 
221 void
223 {
224  // process messages
225  bool needs_write = false;
226  while (cfg_enable_path_execution_ && ! pp_nav_if_->msgq_empty()) {
227  needs_write = true;
228 
229  if (pp_nav_if_->msgq_first_is<NavigatorInterface::StopMessage>()) {
230  NavigatorInterface::StopMessage *msg = pp_nav_if_->msgq_first(msg);
231  pp_nav_if_->set_msgid(msg->id());
232 
233  stop_motion();
234  exec_active_ = false;
235 
236  } else if (pp_nav_if_->msgq_first_is<NavigatorInterface::CartesianGotoMessage>()) {
238  logger->log_info(name(), "cartesian goto (x,y,ori) = (%f,%f,%f)",
239  msg->x(), msg->y(), msg->orientation());
240 
241  pp_nav_if_->set_msgid(msg->id());
242  if (generate_plan(msg->x(), msg->y(), msg->orientation())) {
243  optimize_plan();
244  start_plan();
245  } else {
246  stop_motion();
247  }
248 
249  } else if (pp_nav_if_->msgq_first_is<NavigatorInterface::PlaceGotoMessage>()) {
250  NavigatorInterface::PlaceGotoMessage *msg = pp_nav_if_->msgq_first(msg);
251  logger->log_info(name(), "goto '%s'", msg->place());
252 
253  pp_nav_if_->set_msgid(msg->id());
254  if (generate_plan(msg->place())) {
255  optimize_plan();
256  start_plan();
257  } else {
258  stop_motion();
259  }
260 
263  logger->log_info(name(), "goto '%s' with ori %f", msg->place(), msg->orientation());
264 
265  pp_nav_if_->set_msgid(msg->id());
266  if (generate_plan(msg->place(), msg->orientation())) {
267  optimize_plan();
268  start_plan();
269  } else {
270  stop_motion();
271  }
272  }
273 
274  pp_nav_if_->msgq_pop();
275  }
276 
277  if (cfg_monitor_file_) {
278  fam_->process_events();
279  }
280 
281  if (cfg_enable_path_execution_ && exec_active_) {
282  // check if current was target reached
283  size_t shortcut_to;
284 
285  if (! tf_listener->transform_origin(cfg_base_frame_, cfg_global_frame_, pose_)) {
286  logger->log_warn(name(), "Cannot get pose info, skipping loop");
287 
288  } else if (target_reached_) {
289  // reached the target, check if colli/navi/local planner is final
290  nav_if_->read();
291  fawkes::Time now(clock);
292  if (nav_if_->is_final()) {
293  pp_nav_if_->set_final(true);
294  exec_active_ = false;
295  needs_write = true;
296 
297  } else if (target_ori_reached_) {
298  if ((now - target_reached_at_) >= target_time_) {
299  stop_motion();
300  needs_write = true;
301  }
302 
303  } else if (!target_rotating_ && (now - target_reached_at_) >= target_time_) {
304  if (traversal_.current().has_property("orientation")) {
305  // send one last command, which will only rotate
306  send_next_goal();
307  target_rotating_ = true;
308  } else {
309  stop_motion();
310  needs_write = true;
311  }
312 
313  } else if (target_rotating_ && node_ori_reached()) {
314  //logger->log_debug(name(), "loop(), target_rotating_, ori reached, but colli not final");
315  // reset timer with new timeout value
316  target_time_ = 0;
317  if (traversal_.current().has_property("target_ori_time")) {
318  target_time_ = traversal_.current().property_as_float("target_ori_time");
319  }
320  if (target_time_ == 0) target_time_ = cfg_target_ori_time_;
321 
322  target_ori_reached_ = true;
323  target_reached_at_->stamp();
324  }
325 
326  } else if (node_reached()) {
327  logger->log_info(name(), "Node '%s' has been reached",
328  traversal_.current().name().c_str());
329  last_node_ = traversal_.current().name();
330  if (traversal_.last()) {
331  target_time_ = 0;
332  if (traversal_.current().has_property("target-time")) {
333  target_time_ = traversal_.current().property_as_float("target-time");
334  }
335  if (target_time_ == 0) target_time_ = cfg_target_time_;
336 
337  target_reached_ = true;
338  target_reached_at_->stamp();
339 
340  } else if (traversal_.next()) {
341  publish_path();
342 
343  try {
344  logger->log_info(name(), "Sending next goal %s after node reached",
345  traversal_.current().name().c_str());
346  send_next_goal();
347  } catch (Exception &e) {
348  logger->log_warn(name(), "Failed to send next goal (node reached)");
349  logger->log_warn(name(), e);
350  }
351  }
352 
353  } else if ((shortcut_to = shortcut_possible()) > 0) {
354  logger->log_info(name(), "Shortcut posible, jumping from '%s' to '%s'",
355  traversal_.current().name().c_str(),
356  traversal_.path().nodes()[shortcut_to].name().c_str());
357 
358  traversal_.set_current(shortcut_to);
359 
360  if (traversal_.remaining() > 0) {
361  try {
362  logger->log_info(name(), "Sending next goal after taking a shortcut");
363  send_next_goal();
364  } catch (Exception &e) {
365  logger->log_warn(name(), "Failed to send next goal (shortcut)");
366  logger->log_warn(name(), e);
367  }
368  }
369 
370  } else {
371  fawkes::Time now(clock);
372  bool new_plan = false;
373 
374  if (traversal_.remaining() > 2 && (now - path_planned_at_) > cfg_replan_interval_)
375  {
376  *path_planned_at_ = now;
377  constraint_repo_.lock();
378  if (constraint_repo_->compute() || constraint_repo_->modified(/* reset */ true)) {
379  if (replan(traversal_.current(), traversal_.path().goal())) {
380  // do not optimize here, we know that we do want to travel
381  // to the first node, we are already on the way...
382  //optimize_plan();
383  start_plan();
384  new_plan = true;
385  }
386  }
387  constraint_repo_.unlock();
388  }
389 
390  if (! new_plan && (now - cmd_sent_at_) > cfg_resend_interval_) {
391  try {
392  //logger->log_info(name(), "Re-sending goal");
393  send_next_goal();
394  } catch (Exception &e) {
395  logger->log_warn(name(), "Failed to send next goal (resending)");
396  logger->log_warn(name(), e);
397  }
398  }
399  }
400  }
401 
402 #ifdef HAVE_VISUALIZATION
403  if (vt_) {
404  fawkes::Time now(clock);
405  if (now - visualized_at_ >= cfg_visual_interval_) {
406  *visualized_at_ = now;
407  constraint_repo_.lock();
408  if (constraint_repo_->compute() || constraint_repo_->modified(/* reset */ false)) {
409  vt_->wakeup();
410  }
411  constraint_repo_.unlock();
412  }
413  }
414 #endif
415 
416  if (cfg_enable_path_execution_ && needs_write) {
417  pp_nav_if_->write();
418  }
419 }
420 
422 NavGraphThread::load_graph(std::string filename)
423 {
424  std::ifstream inf(filename);
425  std::string firstword;
426  inf >> firstword;
427  inf.close();
428 
429  if (firstword == "%YAML") {
430  logger->log_info(name(), "Loading YAML graph from %s", filename.c_str());
431  return fawkes::LockPtr<NavGraph>(load_yaml_navgraph(filename), /* recursive mutex */ true);
432  } else {
433  throw Exception("Unknown graph format");
434  }
435 }
436 
437 bool
438 NavGraphThread::generate_plan(std::string goal_name)
439 {
440  if (! tf_listener->transform_origin(cfg_base_frame_, cfg_global_frame_, pose_)) {
441  logger->log_warn(name(),
442  "Failed to compute pose, cannot generate plan");
443  if (cfg_enable_path_execution_) {
444  pp_nav_if_->set_final(true);
445  pp_nav_if_->set_error_code(NavigatorInterface::ERROR_PATH_GEN_FAIL);
446  }
447  return false;
448  }
449 
451  graph_->closest_node(pose_.getOrigin().x(), pose_.getOrigin().y());
452  NavGraphNode goal = graph_->node(goal_name);
453 
454  if (! goal.is_valid()) {
455  logger->log_error(name(), "Failed to generate path from (%.2f,%.2f) to %s: goal is unknown",
456  init.x(), init.y(), goal_name.c_str());
457  if (cfg_enable_path_execution_) {
458  pp_nav_if_->set_final(true);
459  pp_nav_if_->set_error_code(NavigatorInterface::ERROR_UNKNOWN_PLACE);
460  }
461  return false;
462  }
463 
464  logger->log_debug(name(), "Starting at (%f,%f), closest node is '%s'",
465  pose_.getOrigin().x(), pose_.getOrigin().y(), init.name().c_str());
466 
467  try {
468  path_ = graph_->search_path(init, goal, /* use constraints */ true);
469  } catch (Exception &e) {
470  logger->log_error(name(), "Failed to generate path from (%.2f,%.2f) to %s: %s",
471  init.x(), init.y(), goal_name.c_str(), e.what_no_backtrace());
472  if (cfg_enable_path_execution_) {
473  pp_nav_if_->set_final(true);
474  pp_nav_if_->set_error_code(NavigatorInterface::ERROR_PATH_GEN_FAIL);
475  }
476  return false;
477  }
478 
479  if (! path_.empty()) {
480  constrained_plan_ = true;
481  } else {
482  constrained_plan_ = false;
483  logger->log_warn(name(), "Failed to generate plan, will try without constraints");
484  try {
485  path_ = graph_->search_path(init, goal, /* use constraints */ false);
486  } catch (Exception &e) {
487  if (cfg_enable_path_execution_) {
488  pp_nav_if_->set_final(true);
489  pp_nav_if_->set_error_code(NavigatorInterface::ERROR_PATH_GEN_FAIL);
490  }
491  return false;
492  }
493  }
494 
495  if (path_.empty()) {
496  logger->log_error(name(), "Failed to generate plan to travel to '%s'",
497  goal_name.c_str());
498  }
499 
500  traversal_ = path_.traversal();
501  return true;
502 }
503 
504 bool
505 NavGraphThread::generate_plan(std::string goal_name, float ori)
506 {
507  if (generate_plan(goal_name)) {
508 
509  if (! path_.empty() && std::isfinite(ori)) {
510  path_.nodes_mutable().back().set_property("orientation", ori);
511  }
512 
513  traversal_ = path_.traversal();
514  return true;
515  } else {
516  if (cfg_enable_path_execution_) {
517  pp_nav_if_->set_final(true);
518  pp_nav_if_->set_error_code(NavigatorInterface::ERROR_PATH_GEN_FAIL);
519  }
520  return false;
521  }
522 }
523 
524 bool
525 NavGraphThread::generate_plan(float x, float y, float ori)
526 {
527  NavGraphNode close_to_goal = graph_->closest_node(x, y);
528  if (generate_plan(close_to_goal.name())) {
529 
530  NavGraphNode n("free-target", x, y);
531  if (std::isfinite(ori)) {
532  n.set_property("orientation", ori);
533  }
534  graph_->apply_default_properties(n);
535  path_.add_node(n);
536  traversal_ = path_.traversal();
537  return true;
538 
539  } else {
540  if (cfg_enable_path_execution_) {
541  pp_nav_if_->set_final(true);
542  pp_nav_if_->set_error_code(NavigatorInterface::ERROR_PATH_GEN_FAIL);
543  }
544  return false;
545  }
546 }
547 
548 
549 bool
550 NavGraphThread::replan(const NavGraphNode &start, const NavGraphNode &goal)
551 {
552  logger->log_debug(name(), "Starting at node '%s'", start.name().c_str());
553 
554  NavGraphNode act_goal = goal;
555 
556  NavGraphNode close_to_goal;
557  if (goal.name() == "free-target") {
558  close_to_goal = graph_->closest_node(goal.x(), goal.y());
559  act_goal = close_to_goal;
560  }
561 
562  NavGraphPath new_path =
563  graph_->search_path(start, act_goal,
564  /* use constraints */ true, /* compute constraints */ false);
565 
566  if (! new_path.empty()) {
567  // get cost of current plan
568  NavGraphNode pose("current-pose", pose_.getOrigin().x(), pose_.getOrigin().y());
569  float old_cost = graph_->cost(pose, traversal_.current()) + traversal_.remaining_cost();
570  float new_cost = new_path.cost();
571 
572  if (new_cost <= old_cost * cfg_replan_factor_) {
573  constrained_plan_ = true;
574  path_ = new_path;
575  if (goal.name() == "free-target") {
576  // add free target node again
577  path_.add_node(goal);
578  }
579  traversal_ = path_.traversal();
580  logger->log_info(name(), "Executing after re-planning from '%s' to '%s', "
581  "old cost: %f new cost: %f (%f * %f)",
582  start.name().c_str(), goal.name().c_str(),
583  old_cost, new_cost * cfg_replan_factor_, new_cost, cfg_replan_factor_);
584  return true;
585  } else {
586  logger->log_warn(name(), "Re-planning from '%s' to '%s' resulted in "
587  "more expensive plan: %f > %f (%f * %f), keeping old",
588  start.name().c_str(), goal.name().c_str(),
589  new_cost, old_cost * cfg_replan_factor_, old_cost, cfg_replan_factor_);
590  return false;
591  }
592  } else {
593  logger->log_error(name(), "Failed to re-plan from '%s' to '%s'",
594  start.name().c_str(), goal.name().c_str());
595  return false;
596  }
597 }
598 
599 
600 /** Optimize the current plan.
601  * Note that after generating a plan, the robot first needs to
602  * travel to the first actual node from a free position within
603  * the environment. It can happen, that this closest node lies
604  * in the opposite direction of the second node, hence the robot
605  * needs to "go back" first, and only then starts following
606  * the path. We can optimize this by removing the first node,
607  * so that the robot directly travels to the second node which
608  * "lies on the way".
609  */
610 void
611 NavGraphThread::optimize_plan()
612 {
613  if (traversal_.remaining() > 1) {
614  // get current position of robot in map frame
615  const NavGraphPath &path = traversal_.path();
616  double sqr_dist_a = ( pow(pose_.getOrigin().x() - path.nodes()[0].x(), 2) +
617  pow(pose_.getOrigin().y() - path.nodes()[0].y(), 2) );
618  double sqr_dist_b = ( pow(path.nodes()[0].x() - path.nodes()[1].x(), 2) +
619  pow(path.nodes()[0].y() - path.nodes()[1].y(), 2) );
620  double sqr_dist_c = ( pow(pose_.getOrigin().x() - path.nodes()[1].x(), 2) +
621  pow(pose_.getOrigin().y() - path.nodes()[1].y(), 2) );
622 
623  if (sqr_dist_a + sqr_dist_b >= sqr_dist_c){
624  traversal_.next();
625  }
626  }
627 }
628 
629 
630 void
631 NavGraphThread::start_plan()
632 {
633  if (! cfg_enable_path_execution_) return;
634 
635  path_planned_at_->stamp();
636 
637  target_reached_ = false;
638  target_ori_reached_ = false;
639  target_rotating_ = false;
640  if (traversal_.remaining() == 0) {
641  exec_active_ = false;
642  pp_nav_if_->set_final(true);
643  pp_nav_if_->set_error_code(NavigatorInterface::ERROR_UNKNOWN_PLACE);
644  logger->log_warn(name(), "Cannot start empty plan.");
645 
646 #ifdef HAVE_VISUALIZATION
647  if (vt_) {
648  vt_->reset_plan();
649  visualized_at_->stamp();
650  }
651 #endif
652 
653  } else {
654  traversal_.next();
655 
656  std::string m = path_.nodes()[0].name();
657  for (unsigned int i = 1; i < path_.size(); ++i) {
658  m += " - " + path_.nodes()[i].name();
659  }
660  logger->log_info(name(), "Starting route: %s", m.c_str());
661 #ifdef HAVE_VISUALIZATION
662  if (vt_) {
663  vt_->set_traversal(traversal_);
664  visualized_at_->stamp();
665  }
666 #endif
667 
668  exec_active_ = true;
669 
670  NavGraphNode final_target = path_.goal();
671 
672  pp_nav_if_->set_error_code(NavigatorInterface::ERROR_NONE);
673  pp_nav_if_->set_final(false);
674  pp_nav_if_->set_dest_x(final_target.x());
675  pp_nav_if_->set_dest_y(final_target.y());
676 
677  try {
678  logger->log_info(name(), "Sending next goal on plan start");
679  send_next_goal();
680  } catch (Exception &e) {
681  logger->log_warn(name(), "Failed to send next goal (start plan)");
682  logger->log_warn(name(), e);
683  }
684  }
685 
686  publish_path();
687 }
688 
689 
690 void
691 NavGraphThread::stop_motion()
692 {
693  if (! cfg_enable_path_execution_) return;
694 
696  try {
697  nav_if_->msgq_enqueue(stop);
698  } catch (Exception &e) {
699  logger->log_warn(name(), "Failed to stop motion, exception follows");
700  logger->log_warn(name(), e);
701  }
702  last_node_ = "";
703  exec_active_ = false;
704  target_ori_reached_ = false;
705  target_rotating_ = false;
706  pp_nav_if_->set_final(true);
707  traversal_.invalidate();
708 
709 #ifdef HAVE_VISUALIZATION
710  if (vt_) {
711  vt_->reset_plan();
712  visualized_at_->stamp();
713  }
714 #endif
715 
716 }
717 
718 
719 void
720 NavGraphThread::send_next_goal()
721 {
722  if (! cfg_enable_path_execution_) return;
723 
724  bool stop_at_target = false;
725  bool orient_at_target = false;
726 
727  if (! traversal_.running()) {
728  throw Exception("Cannot send next goal if plan is empty");
729  }
730 
731  const NavGraphNode &next_target = traversal_.current();
732 
733  float ori = NAN;
734  if ( traversal_.last() ) {
735  stop_at_target = true;
736 
737  if ( next_target.has_property("orientation") ) {
738  orient_at_target = true;
739 
740  // take the given orientation for the final node
741  ori = next_target.property_as_float("orientation");
742  }
743 
744  } else {
745  // set direction facing from next_target (what is the current point
746  // to drive to) to next point to drive to. So orientation is the
747  // direction from next_target to the target after that
748  const NavGraphNode &next_next_target = traversal_.peek_next();
749 
750  ori = atan2f( next_next_target.y() - next_target.y(),
751  next_next_target.x() - next_target.x());
752 
753  }
754 
755  // get target position in base frame
756  tf::Stamped<tf::Pose> tpose;
758  tposeglob(tf::Transform(tf::create_quaternion_from_yaw(ori),
759  tf::Vector3(next_target.x(), next_target.y(), 0)),
760  Time(0,0), cfg_global_frame_);
761  try {
762  tf_listener->transform_pose(cfg_base_frame_, tposeglob, tpose);
763  } catch (Exception &e) {
764  logger->log_warn(name(),
765  "Failed to compute pose, cannot generate plan", e.what());
766  throw;
767  }
768 
769  if( target_reached_ ) {
770  // no need for traveling anymore, just rotating
771  tpose.setOrigin(tf::Vector3(0.f, 0.f, 0.f));
772  }
773 
775  new NavigatorInterface::CartesianGotoMessage(tpose.getOrigin().x(),
776  tpose.getOrigin().y(),
777  tf::get_yaw(tpose.getRotation()));
778 
781  if ( orient_at_target ) {
782  orient_mode_msg = new NavigatorInterface::SetOrientationModeMessage(
783  fawkes::NavigatorInterface::OrientationMode::OrientAtTarget );
784  } else {
785  orient_mode_msg = new NavigatorInterface::SetOrientationModeMessage(
786  fawkes::NavigatorInterface::OrientationMode::OrientDuringTravel );
787  }
788 
789  try {
790 #ifdef HAVE_VISUALIZATION
791  if (vt_) vt_->set_current_edge(last_node_, next_target.name());
792 #endif
793 
794  if (! nav_if_->has_writer()) {
795  throw Exception("No writer for navigator interface");
796  }
797 
798  nav_if_->msgq_enqueue(stop_at_target_msg);
799  nav_if_->msgq_enqueue(orient_mode_msg);
800 
801  logger->log_debug(name(), "Sending goto(x=%f,y=%f,ori=%f) for node '%s'",
802  tpose.getOrigin().x(), tpose.getOrigin().y(),
803  tf::get_yaw(tpose.getRotation()), next_target.name().c_str());
804 
805  nav_if_->msgq_enqueue(gotomsg);
806  cmd_sent_at_->stamp();
807 
808  error_at_->stamp();
809  error_reason_ = "";
810 
811  } catch (Exception &e) {
812  if (cfg_abort_on_error_) {
813  logger->log_warn(name(), "Failed to send cartesian goto for "
814  "next goal, exception follows");
815  logger->log_warn(name(), e);
816  exec_active_ = false;
817  pp_nav_if_->set_final(true);
818  pp_nav_if_->set_error_code(NavigatorInterface::ERROR_OBSTRUCTION);
819  pp_nav_if_->write();
820 #ifdef HAVE_VISUALIZATION
821  if (vt_) vt_->reset_plan();
822 #endif
823  } else {
824  fawkes::Time now(clock);
825  if (error_reason_ != e.what_no_backtrace() || (now - error_at_) > 4.0) {
826  error_reason_ = e.what_no_backtrace();
827  *error_at_ = now;
828  logger->log_warn(name(), "Failed to send cartesian goto for "
829  "next goal, exception follows");
830  logger->log_warn(name(), e);
831  logger->log_warn(name(), "*** NOT aborting goal (as per config)");
832  }
833  }
834  }
835 }
836 
837 /** Check if current node has been reached.
838  * Compares the distance to the node to defined tolerances.
839  */
840 bool
841 NavGraphThread::node_reached()
842 {
843  if (! traversal_) {
844  logger->log_error(name(), "Cannot check node reached if no traversal given");
845  return true;
846  }
847 
848  if (! traversal_.running()) {
849  logger->log_error(name(), "Cannot check node reached if no traversal running");
850  return true;
851  }
852 
853  const NavGraphNode &cur_target = traversal_.current();
854 
855  // get distance to current target in map frame
856  float dist = sqrt(pow(pose_.getOrigin().x() - cur_target.x(), 2) +
857  pow( pose_.getOrigin().y() - cur_target.y(), 2));
858 
859  float tolerance = cur_target.property_as_float("travel_tolerance");
860  // use a different tolerance for the final node
861  if (traversal_.last()) {
862  tolerance = cur_target.property_as_float("target_tolerance");
863  //return (dist <= tolerance) && node_ori_reached(cur_target);
864  }
865 
866  // can be no or invalid tolerance, be very generous
867  if (tolerance == 0.) {
868  logger->log_warn(name(), "Invalid tolerance for node %s, using 1.0",
869  cur_target.name().c_str());
870  tolerance = 1.0;
871  }
872 
873  return (dist <= tolerance);
874 }
875 
876 
877 /** Check if orientation of current node has been reached.
878  * Compares the angular distance to the targeted orientation
879  * to the defined angular tolerances.
880  */
881 bool
882 NavGraphThread::node_ori_reached()
883 {
884  if (! traversal_) {
885  logger->log_error(name(), "Cannot check node reached if no traversal given");
886  return true;
887  }
888 
889  if (! traversal_.running()) {
890  logger->log_error(name(), "Cannot check node reached if no traversal running");
891  return true;
892  }
893 
894  const NavGraphNode &cur_target = traversal_.current();
895  return node_ori_reached(cur_target);
896 }
897 
898 
899 /** Check if orientation of a given node has been reached.
900  * Compares the angular distance to the targeted orientation
901  * to the defined angular tolerances.
902  */
903 bool
904 NavGraphThread::node_ori_reached(const NavGraphNode &node)
905 {
906  if (node.has_property("orientation")) {
907  float ori_tolerance = node.property_as_float("orientation_tolerance");
908  float ori_diff =
909  angle_distance( normalize_rad(tf::get_yaw(pose_.getRotation())),
910  normalize_rad(node.property_as_float("orientation")));
911 
912  //logger->log_info(name(), "Ori=%f Rot=%f Diff=%f Tol=%f Dist=%f Tol=%f", cur_target.property_as_float("orientation"), tf::get_yaw(pose_.getRotation() ), ori_diff, ori_tolerance, dist, tolerance);
913  return (ori_diff <= ori_tolerance);
914 
915  } else {
916  return true;
917  }
918 }
919 
920 
921 size_t
922 NavGraphThread::shortcut_possible()
923 {
924  if (!traversal_ || traversal_.remaining() < 1) {
925  logger->log_debug(name(), "Cannot shortcut if no path nodes remaining");
926  return 0;
927  }
928 
929  for (size_t i = traversal_.path().size() - 1; i > traversal_.current_index(); --i) {
930  const NavGraphNode &node = traversal_.path().nodes()[i];
931 
932  float dist = sqrt(pow(pose_.getOrigin().x() - node.x(), 2) +
933  pow(pose_.getOrigin().y() - node.y(), 2));
934 
935  float tolerance = node.property_as_float("shortcut_tolerance");
936 
937  if (tolerance == 0.0) return 0;
938  if (dist <= tolerance) return i;
939  }
940 
941  return 0;
942 }
943 
944 
945 void
946 NavGraphThread::fam_event(const char *filename, unsigned int mask)
947 {
948  // The file will be ignored from now onwards, re-register
949  if (mask & FAM_IGNORED) {
950  fam_->watch_file(cfg_graph_file_.c_str());
951  }
952 
953  if (mask & (FAM_MODIFY | FAM_IGNORED)) {
954  logger->log_info(name(), "Graph changed on disk, reloading");
955 
956  try {
957  LockPtr<NavGraph> new_graph = load_graph(cfg_graph_file_);
958  **graph_ = **new_graph;
959  } catch (Exception &e) {
960  logger->log_warn(name(), "Loading new graph failed, exception follows");
961  logger->log_warn(name(), e);
962  return;
963  }
964 
965 #ifdef HAVE_VISUALIZATION
966  if (vt_) {
967  vt_->set_graph(graph_);
968  visualized_at_->stamp();
969  }
970 #endif
971 
972  if (exec_active_) {
973  // store the goal and restart it after the graph has been reloaded
974 
975  stop_motion();
976  NavGraphNode goal = path_.goal();
977 
978  bool gen_ok = false;
979  if (goal.name() == "free-target") {
980  gen_ok = generate_plan(goal.x(), goal.y(), goal.property_as_float("orientation"));
981  } else {
982  gen_ok = generate_plan(goal.name());
983  }
984 
985  if (gen_ok) {
986  optimize_plan();
987  start_plan();
988  } else {
989  stop_motion();
990  }
991  }
992  }
993 }
994 
995 
996 void
997 NavGraphThread::log_graph()
998 {
999  const std::vector<NavGraphNode> & nodes = graph_->nodes();
1000  std::vector<NavGraphNode>::const_iterator n;
1001  for (n = nodes.begin(); n != nodes.end(); ++n) {
1002  logger->log_info(name(), "Node %s @ (%f,%f)%s",
1003  n->name().c_str(), n->x(), n->y(),
1004  n->unconnected() ? " UNCONNECTED" : "");
1005 
1006  const std::map<std::string, std::string> &props = n->properties();
1007  std::map<std::string, std::string>::const_iterator p;
1008  for (p = props.begin(); p != props.end(); ++p) {
1009  logger->log_info(name(), " - %s: %s", p->first.c_str(), p->second.c_str());
1010  }
1011  }
1012 
1013  std::vector<NavGraphEdge> edges = graph_->edges();
1014  std::vector<NavGraphEdge>::iterator e;
1015  for (e = edges.begin(); e != edges.end(); ++e) {
1016  logger->log_info(name(), "Edge %10s --%s %s",
1017  e->from().c_str(), e->is_directed() ? ">" : "-", e->to().c_str());
1018 
1019  const std::map<std::string, std::string> &props = e->properties();
1020  std::map<std::string, std::string>::const_iterator p;
1021  for (p = props.begin(); p != props.end(); ++p) {
1022  logger->log_info(name(), " - %s: %s", p->first.c_str(), p->second.c_str());
1023  }
1024  }
1025 }
1026 
1027 void
1028 NavGraphThread::publish_path()
1029 {
1030  if (! cfg_enable_path_execution_) return;
1031 
1032  std::vector<std::string> vpath(40, "");
1033 
1034  if (traversal_) {
1035  size_t ind = 0;
1036  size_t r = traversal_.running() ? traversal_.current_index() : traversal_.remaining();
1037  for (; r < traversal_.path().size(); ++r) {
1038  vpath[ind++] = traversal_.path().nodes()[r].name();
1039  }
1040  }
1041 
1042  path_if_->set_path_node_1(vpath[0].c_str());
1043  path_if_->set_path_node_2(vpath[1].c_str());
1044  path_if_->set_path_node_3(vpath[2].c_str());
1045  path_if_->set_path_node_4(vpath[3].c_str());
1046  path_if_->set_path_node_5(vpath[4].c_str());
1047  path_if_->set_path_node_6(vpath[5].c_str());
1048  path_if_->set_path_node_7(vpath[6].c_str());
1049  path_if_->set_path_node_8(vpath[7].c_str());
1050  path_if_->set_path_node_9(vpath[8].c_str());
1051  path_if_->set_path_node_10(vpath[9].c_str());
1052  path_if_->set_path_node_11(vpath[10].c_str());
1053  path_if_->set_path_node_12(vpath[11].c_str());
1054  path_if_->set_path_node_13(vpath[12].c_str());
1055  path_if_->set_path_node_14(vpath[13].c_str());
1056  path_if_->set_path_node_15(vpath[14].c_str());
1057  path_if_->set_path_node_16(vpath[15].c_str());
1058  path_if_->set_path_node_17(vpath[16].c_str());
1059  path_if_->set_path_node_18(vpath[17].c_str());
1060  path_if_->set_path_node_19(vpath[18].c_str());
1061  path_if_->set_path_node_20(vpath[19].c_str());
1062  path_if_->set_path_node_21(vpath[20].c_str());
1063  path_if_->set_path_node_22(vpath[21].c_str());
1064  path_if_->set_path_node_23(vpath[22].c_str());
1065  path_if_->set_path_node_24(vpath[23].c_str());
1066  path_if_->set_path_node_25(vpath[24].c_str());
1067  path_if_->set_path_node_26(vpath[25].c_str());
1068  path_if_->set_path_node_27(vpath[26].c_str());
1069  path_if_->set_path_node_28(vpath[27].c_str());
1070  path_if_->set_path_node_29(vpath[28].c_str());
1071  path_if_->set_path_node_30(vpath[29].c_str());
1072  path_if_->set_path_node_31(vpath[30].c_str());
1073  path_if_->set_path_node_32(vpath[31].c_str());
1074  path_if_->set_path_node_33(vpath[32].c_str());
1075  path_if_->set_path_node_34(vpath[33].c_str());
1076  path_if_->set_path_node_35(vpath[34].c_str());
1077  path_if_->set_path_node_36(vpath[35].c_str());
1078  path_if_->set_path_node_37(vpath[36].c_str());
1079  path_if_->set_path_node_38(vpath[37].c_str());
1080  path_if_->set_path_node_39(vpath[38].c_str());
1081  path_if_->set_path_node_40(vpath[39].c_str());
1082  path_if_->set_path_length(traversal_.remaining());
1083  path_if_->write();
1084 }
Thread(const char *name)
Constructor.
Definition: thread.cpp:205
void set_navgraph(LockPtr< NavGraph > &navgraph)
Set navgraph.
void set_path_node_37(const char *new_path_node_37)
Set path_node_37 value.
fawkes::LockPtr< NavGraphConstraintRepo > constraint_repo() const
Get locked pointer to constraint repository.
Definition: navgraph.cpp:145
size_t size() const
Get size of path.
float default_property_as_float(const std::string &prop) const
Get property converted to float.
Definition: navgraph.cpp:778
void set_path_node_29(const char *new_path_node_29)
Set path_node_29 value.
unsigned int id() const
Get message ID.
Definition: message.cpp:197
virtual ~NavGraphThread()
Destructor.
void set_path_node_26(const char *new_path_node_26)
Set path_node_26 value.
void set_path_node_14(const char *new_path_node_14)
Set path_node_14 value.
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1048
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
NavGraphThread()
Constructor.
bool compute()
Call compute method on all registered constraints.
void set_path_node_16(const char *new_path_node_16)
Set path_node_16 value.
void set_path_node_1(const char *new_path_node_1)
Set path_node_1 value.
void set_path_node_15(const char *new_path_node_15)
Set path_node_15 value.
std::vector< NavGraphNode > & nodes_mutable()
Get nodes along the path as mutable vector.
void set_path_node_27(const char *new_path_node_27)
Set path_node_27 value.
float normalize_rad(float angle_rad)
Normalize angle in radian between 0 (inclusive) and 2*PI (exclusive).
Definition: angle.h:93
void set_path_node_19(const char *new_path_node_19)
Set path_node_19 value.
const NavGraphNode & current() const
Get current node in path.
Fawkes library namespace.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
void set_path_node_6(const char *new_path_node_6)
Set path_node_6 value.
void set_final(const bool new_final)
Set final value.
Topological map graph.
Definition: navgraph.h:57
void set_path_node_18(const char *new_path_node_18)
Set path_node_18 value.
void set_path_node_30(const char *new_path_node_30)
Set path_node_30 value.
void unlock() const
Unlock object mutex.
Definition: lockptr.h:255
AspectProviderAspect(AspectIniFin *inifin)
Constructor.
void set_path_node_25(const char *new_path_node_25)
Set path_node_25 value.
A class for handling time.
Definition: time.h:91
bool has_property(const std::string &property) const
Check if node has specified property.
Definition: navgraph_node.h:95
Class representing a path for a NavGraph.
Definition: navgraph_path.h:39
Send Marker messages to rviz to show navgraph info.
fawkes::NavGraphPath search_path(const std::string &from, const std::string &to, bool use_constraints=true, bool compute_constraints=true)
Search for a path between two nodes with default distance costs.
Definition: navgraph.cpp:987
virtual const char * what() const
Get primary string.
Definition: exception.cpp:661
bool is_valid() const
Check if node is valid, i.e.
Thread class encapsulation of pthreads.
Definition: thread.h:42
void set_current(size_t new_current)
Set the current node.
void set_path_node_36(const char *new_path_node_36)
Set path_node_36 value.
SetOrientationModeMessage Fawkes BlackBoard Interface Message.
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:500
void transform_pose(const std::string &target_frame, const Stamped< Pose > &stamped_in, Stamped< Pose > &stamped_out) const
Transform a stamped pose into the target frame.
void set_path_node_11(const char *new_path_node_11)
Set path_node_11 value.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
const std::vector< NavGraphNode > & nodes() const
Get nodes of the graph.
Definition: navgraph.cpp:124
void add_change_listener(ChangeListener *listener)
Add a change listener.
Definition: navgraph.cpp:1452
float orientation() const
Get orientation value.
void set_path_node_24(const char *new_path_node_24)
Set path_node_24 value.
float cost() const
Get cost of path from start to to end.
void set_path_node_28(const char *new_path_node_28)
Set path_node_28 value.
virtual void init()
Initialize the thread.
LockPtr<> is a reference-counting shared lockable smartpointer.
Definition: lockptr.h:57
bool last() const
Check if the current node is the last node in the path.
void set_path_node_22(const char *new_path_node_22)
Set path_node_22 value.
SetStopAtTargetMessage Fawkes BlackBoard Interface Message.
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:45
Thread aspect to use blocked timing.
void set_path_node_35(const char *new_path_node_35)
Set path_node_35 value.
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1193
void set_path_node_32(const char *new_path_node_32)
Set path_node_32 value.
PlaceGotoMessage Fawkes BlackBoard Interface Message.
void set_path_node_31(const char *new_path_node_31)
Set path_node_31 value.
void set_path_node_13(const char *new_path_node_13)
Set path_node_13 value.
bool is_final() const
Get final value.
void set_path_node_17(const char *new_path_node_17)
Set path_node_17 value.
static const unsigned int FAM_IGNORED
File was ignored.
Definition: fam.h:57
Base class for exceptions in Fawkes.
Definition: exception.h:36
Message * msgq_first()
Get the first message from the message queue.
Definition: interface.cpp:1180
void set_path_node_40(const char *new_path_node_40)
Set path_node_40 value.
const NavGraphNode & peek_next() const
Peek on the next node.
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:477
CartesianGotoMessage Fawkes BlackBoard Interface Message.
void set_path_node_38(const char *new_path_node_38)
Set path_node_38 value.
virtual void once()
Execute an action exactly once.
size_t remaining() const
Get the number of remaining nodes in path traversal.
void set_path_node_12(const char *new_path_node_12)
Set path_node_12 value.
void set_property(const std::string &property, const std::string &value)
Set property.
void remove_change_listener(ChangeListener *listener)
Remove a change listener.
Definition: navgraph.cpp:1461
NavGraphNode node(const std::string &name) const
Get a specified node.
Definition: navgraph.cpp:156
void set_path_node_2(const char *new_path_node_2)
Set path_node_2 value.
void set_path_length(const uint32_t new_path_length)
Set path_length value.
NavGraph * load_yaml_navgraph(std::string filename)
Load topological map graph stored in RCSoft format.
const std::string & name() const
Get name of node.
Definition: navgraph_node.h:49
bool has_writer() const
Check if there is a writer for the interface.
Definition: interface.cpp:834
const std::vector< NavGraphNode > & nodes() const
Get nodes along the path.
Definition: navgraph_path.h:94
Traversal traversal() const
Get a new path traversal handle.
const char * name() const
Get name of thread.
Definition: thread.h:95
void apply_default_properties(NavGraphNode &node)
Set default properties on node for which no local value exists.
Definition: navgraph.cpp:861
Monitors files for changes.
Definition: fam.h:71
const std::vector< NavGraphEdge > & edges() const
Get edges of the graph.
Definition: navgraph.cpp:134
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
Definition: exception.cpp:686
virtual void loop()
Code to execute in the thread.
bool msgq_first_is()
Check if first message has desired type.
Definition: interface.h:314
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
bool running() const
Check if traversal is currently runnung.
const NavGraphPath & path() const
Get parent path the traversal belongs to.
Definition: navgraph_path.h:63
float y() const
Get Y coordinate in global frame.
Definition: navgraph_node.h:59
void set_path_node_10(const char *new_path_node_10)
Set path_node_10 value.
bool empty() const
Check if path is empty.
void set_error_code(const uint32_t new_error_code)
Set error_code value.
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.
size_t current_index() const
Get index of current node in path.
virtual void finalize()
Finalize the thread.
Thread aspect provide a new aspect.
void set_path_node_4(const char *new_path_node_4)
Set path_node_4 value.
NavGraphNode closest_node(float pos_x, float pos_y, const std::string &property="") const
Get node closest to a specified point with a certain property.
Definition: navgraph.cpp:181
Wrapper class to add time stamp and frame ID to base types.
Definition: types.h:133
unsigned int msgq_enqueue(Message *message)
Enqueue message at end of queue.
Definition: interface.cpp:903
void set_path_node_9(const char *new_path_node_9)
Set path_node_9 value.
void clear()
Set underlying instance to 0, decrementing reference count of existing instance appropriately.
Definition: lockptr.h:492
Topological graph node.
Definition: navgraph_node.h:38
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
float remaining_cost() const
Get the remaining cost to the goal.
void add_listener(FamListener *listener)
Add a listener.
Definition: fam.cpp:267
void set_dest_x(const float new_dest_x)
Set dest_x value.
void set_path_node_34(const char *new_path_node_34)
Set path_node_34 value.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
void set_path_node_33(const char *new_path_node_33)
Set path_node_33 value.
void set_path_node_20(const char *new_path_node_20)
Set path_node_20 value.
bool has_default_property(const std::string &property) const
Check if graph has specified default property.
Definition: navgraph.cpp:753
float x() const
Get X coordinate in global frame.
Definition: navgraph_node.h:54
PlaceWithOriGotoMessage Fawkes BlackBoard Interface Message.
void invalidate()
Invalidate this traversal.
bool modified(bool reset_modified=false)
Check if the constraint repo has been modified.
void set_path_node_21(const char *new_path_node_21)
Set path_node_21 value.
virtual void fam_event(const char *filename, unsigned int mask)
Event has been raised.
void set_path_node_8(const char *new_path_node_8)
Set path_node_8 value.
void set_path_node_7(const char *new_path_node_7)
Set path_node_7 value.
virtual bool exists(const char *path)=0
Check if a given value exists.
BlockedTimingAspect(WakeupHook wakeup_hook)
Constructor.
Time & stamp()
Set this time to the current time.
Definition: time.cpp:783
static const unsigned int FAM_MODIFY
File was modified.
Definition: fam.h:41
float property_as_float(const std::string &prop) const
Get property converted to float.
void add_node(const NavGraphNode &node, float cost_from_end=0)
Add a node to the path.
void process_events(int timeout=0)
Process events.
Definition: fam.cpp:289
void set_path_node_23(const char *new_path_node_23)
Set path_node_23 value.
tf::Transformer * tf_listener
This is the transform listener which saves transforms published by other threads in the system...
Definition: tf.h:70
bool next()
Move on traversal to next node.
void set_dest_y(const float new_dest_y)
Set dest_y value.
NavPathInterface Fawkes BlackBoard Interface.
void set_path_node_5(const char *new_path_node_5)
Set path_node_5 value.
void set_msgid(const uint32_t new_msgid)
Set msgid value.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:44
void set_path_node_39(const char *new_path_node_39)
Set path_node_39 value.
void set_path_node_3(const char *new_path_node_3)
Set path_node_3 value.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
void watch_file(const char *filepath)
Watch a file.
Definition: fam.cpp:207
void lock() const
Lock access to the encapsulated object.
Definition: lockptr.h:247
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
float cost(const NavGraphNode &from, const NavGraphNode &to) const
Calculate cost between two adjacent nodes.
Definition: navgraph.cpp:1107
void start(bool wait=true)
Call this method to start the thread.
Definition: thread.cpp:511
float angle_distance(float angle_rad1, float angle_rad2)
Determines the distance between two angle provided as radians.
Definition: angle.h:128
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
StopMessage Fawkes BlackBoard Interface Message.
NavigatorInterface Fawkes BlackBoard Interface.
const NavGraphNode & goal() const
Get goal of path.
virtual void close(Interface *interface)=0
Close interface.