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  if (config->exists("/navgraph/travel_tolerance") ||
94  config->exists("/navgraph/target_tolerance") ||
95  config->exists("/navgraph/orientation_tolerance") ||
96  config->exists("/navgraph/shortcut_tolerance"))
97  {
98  logger->log_error(name(), "Tolerances may no longe rbe set in the config.");
99  logger->log_error(name(), "The must be set as default properties in the graph.");
100  logger->log_error(name(), "Remove the following config values (move to navgraph):");
101  logger->log_error(name(), " /navgraph/travel_tolerance");
102  logger->log_error(name(), " /navgraph/target_tolerance");
103  logger->log_error(name(), " /navgraph/orientation_tolerance");
104  logger->log_error(name(), " /navgraph/shortcut_tolerance");
105  throw Exception("Navgraph tolerances may no longer be set in the config");
106  }
107 
108  pp_nav_if_ = blackboard->open_for_writing<NavigatorInterface>("Pathplan");
109  nav_if_ = blackboard->open_for_reading<NavigatorInterface>(cfg_nav_if_id_.c_str());
110  path_if_ = blackboard->open_for_writing<NavPathInterface>("NavPath");
111 
112 
113  if (! cfg_graph_file_.empty()) {
114  if (cfg_graph_file_[0] != '/') {
115  cfg_graph_file_ = std::string(CONFDIR) + "/" + cfg_graph_file_;
116  }
117  graph_ = load_graph(cfg_graph_file_);
118  } else {
119  graph_ = LockPtr<NavGraph>(new NavGraph("generated"), /* recursive mutex */ true);
120  }
121 
122  if (! graph_->has_default_property("travel_tolerance")) {
123  throw Exception("Graph must specify travel tolerance");
124  }
125  if (! graph_->has_default_property("target_tolerance")) {
126  throw Exception("Graph must specify target tolerance");
127  }
128  if (! graph_->has_default_property("orientation_tolerance")) {
129  throw Exception("Graph must specify orientation tolerance");
130  }
131  if (! graph_->has_default_property("shortcut_tolerance")) {
132  throw Exception("Graph must specify shortcut tolerance");
133  }
134  if (graph_->has_default_property("target_time")) {
135  cfg_target_time_ = graph_->default_property_as_float("target_time");
136  logger->log_info(name(), "Using target time %f from graph file", cfg_target_time_);
137  }
138  if (graph_->has_default_property("target_ori_time")) {
139  cfg_target_time_ = graph_->default_property_as_float("target_ori_time");
140  logger->log_info(name(), "Using target orientation time %f from graph file", cfg_target_ori_time_);
141  }
142 
143  navgraph_aspect_inifin_.set_navgraph(graph_);
144  if (cfg_log_graph_) {
145  log_graph();
146  }
147 
148  if (cfg_monitor_file_) {
149  logger->log_info(name(), "Enabling graph file monitoring");
150  try {
151  fam_ = new FileAlterationMonitor();
152  fam_->watch_file(cfg_graph_file_.c_str());
153  fam_->add_listener(this);
154  } catch (Exception &e) {
155  logger->log_warn(name(), "Could not enable graph file monitoring");
156  logger->log_warn(name(), e);
157  }
158 
159  }
160 
161  exec_active_ = false;
162  target_reached_ = false;
163  target_ori_reached_= false;
164  target_rotating_ = false;
165  last_node_ = "";
166  error_reason_ = "";
167  constrained_plan_ = false;
168  cmd_sent_at_ = new Time(clock);
169  path_planned_at_ = new Time(clock);
170  target_reached_at_ = new Time(clock);
171  error_at_ = new Time(clock);
172 #ifdef HAVE_VISUALIZATION
173  visualized_at_ = new Time(clock);
174  if (vt_) {
175  graph_->add_change_listener(vt_);
176  }
177 #endif
178 
179  constraint_repo_ = graph_->constraint_repo();
180 }
181 
182 void
184 {
185  delete cmd_sent_at_;
186  delete path_planned_at_;
187  delete target_reached_at_;
188  delete error_at_;
189 #ifdef HAVE_VISUALIZATION
190  delete visualized_at_;
191  if (vt_) {
192  graph_->remove_change_listener(vt_);
193  }
194 #endif
195  graph_.clear();
196  blackboard->close(pp_nav_if_);
197  blackboard->close(nav_if_);
198  blackboard->close(path_if_);
199 }
200 
201 void
203 {
204 #ifdef HAVE_VISUALIZATION
205  if (vt_) {
206  vt_->set_constraint_repo(constraint_repo_);
207  vt_->set_graph(graph_);
208  }
209 #endif
210 }
211 
212 void
214 {
215  // process messages
216  bool needs_write = false;
217  while (! pp_nav_if_->msgq_empty()) {
218  needs_write = true;
219 
220  if (pp_nav_if_->msgq_first_is<NavigatorInterface::StopMessage>()) {
221  NavigatorInterface::StopMessage *msg = pp_nav_if_->msgq_first(msg);
222  pp_nav_if_->set_msgid(msg->id());
223 
224  stop_motion();
225  exec_active_ = false;
226 
227  } else if (pp_nav_if_->msgq_first_is<NavigatorInterface::CartesianGotoMessage>()) {
229  logger->log_info(name(), "cartesian goto (x,y,ori) = (%f,%f,%f)",
230  msg->x(), msg->y(), msg->orientation());
231 
232  pp_nav_if_->set_msgid(msg->id());
233  if (generate_plan(msg->x(), msg->y(), msg->orientation())) {
234  optimize_plan();
235  start_plan();
236  } else {
237  stop_motion();
238  }
239 
240  } else if (pp_nav_if_->msgq_first_is<NavigatorInterface::PlaceGotoMessage>()) {
241  NavigatorInterface::PlaceGotoMessage *msg = pp_nav_if_->msgq_first(msg);
242  logger->log_info(name(), "goto '%s'", msg->place());
243 
244  pp_nav_if_->set_msgid(msg->id());
245  if (generate_plan(msg->place())) {
246  optimize_plan();
247  start_plan();
248  } else {
249  stop_motion();
250  }
251 
254  logger->log_info(name(), "goto '%s' with ori %f", msg->place(), msg->orientation());
255 
256  pp_nav_if_->set_msgid(msg->id());
257  if (generate_plan(msg->place(), msg->orientation())) {
258  optimize_plan();
259  start_plan();
260  } else {
261  stop_motion();
262  }
263  }
264 
265  pp_nav_if_->msgq_pop();
266  }
267 
268  if (cfg_monitor_file_) {
269  fam_->process_events();
270  }
271 
272  if (exec_active_) {
273  // check if current was target reached
274  size_t shortcut_to;
275 
276  if (! tf_listener->transform_origin(cfg_base_frame_, cfg_global_frame_, pose_)) {
277  logger->log_warn(name(), "Cannot get pose info, skipping loop");
278 
279  } else if (target_reached_) {
280  // reached the target, check if colli/navi/local planner is final
281  nav_if_->read();
282  fawkes::Time now(clock);
283  if (nav_if_->is_final()) {
284  pp_nav_if_->set_final(true);
285  exec_active_ = false;
286  needs_write = true;
287 
288  } else if (target_ori_reached_) {
289  if ((now - target_reached_at_) >= target_time_) {
290  stop_motion();
291  needs_write = true;
292  }
293 
294  } else if (!target_rotating_ && (now - target_reached_at_) >= target_time_) {
295  if (traversal_.current().has_property("orientation")) {
296  // send one last command, which will only rotate
297  send_next_goal();
298  target_rotating_ = true;
299  } else {
300  stop_motion();
301  needs_write = true;
302  }
303 
304  } else if (target_rotating_ && node_ori_reached()) {
305  //logger->log_debug(name(), "loop(), target_rotating_, ori reached, but colli not final");
306  // reset timer with new timeout value
307  target_time_ = 0;
308  if (traversal_.current().has_property("target_ori_time")) {
309  target_time_ = traversal_.current().property_as_float("target_ori_time");
310  }
311  if (target_time_ == 0) target_time_ = cfg_target_ori_time_;
312 
313  target_ori_reached_ = true;
314  target_reached_at_->stamp();
315  }
316 
317  } else if (node_reached()) {
318  logger->log_info(name(), "Node '%s' has been reached",
319  traversal_.current().name().c_str());
320  last_node_ = traversal_.current().name();
321  if (traversal_.last()) {
322  target_time_ = 0;
323  if (traversal_.current().has_property("target-time")) {
324  target_time_ = traversal_.current().property_as_float("target-time");
325  }
326  if (target_time_ == 0) target_time_ = cfg_target_time_;
327 
328  target_reached_ = true;
329  target_reached_at_->stamp();
330 
331  } else if (traversal_.next()) {
332  publish_path();
333 
334  try {
335  logger->log_info(name(), "Sending next goal %s after node reached",
336  traversal_.current().name().c_str());
337  send_next_goal();
338  } catch (Exception &e) {
339  logger->log_warn(name(), "Failed to send next goal (node reached)");
340  logger->log_warn(name(), e);
341  }
342  }
343 
344  } else if ((shortcut_to = shortcut_possible()) > 0) {
345  logger->log_info(name(), "Shortcut posible, jumping from '%s' to '%s'",
346  traversal_.current().name().c_str(),
347  traversal_.path().nodes()[shortcut_to].name().c_str());
348 
349  traversal_.set_current(shortcut_to);
350 
351  if (traversal_.remaining() > 0) {
352  try {
353  logger->log_info(name(), "Sending next goal after taking a shortcut");
354  send_next_goal();
355  } catch (Exception &e) {
356  logger->log_warn(name(), "Failed to send next goal (shortcut)");
357  logger->log_warn(name(), e);
358  }
359  }
360 
361  } else {
362  fawkes::Time now(clock);
363  bool new_plan = false;
364 
365  if (traversal_.remaining() > 2 && (now - path_planned_at_) > cfg_replan_interval_)
366  {
367  *path_planned_at_ = now;
368  constraint_repo_.lock();
369  if (constraint_repo_->compute() || constraint_repo_->modified(/* reset */ true)) {
370  if (replan(traversal_.current(), traversal_.path().goal())) {
371  // do not optimize here, we know that we do want to travel
372  // to the first node, we are already on the way...
373  //optimize_plan();
374  start_plan();
375  new_plan = true;
376  }
377  }
378  constraint_repo_.unlock();
379  }
380 
381  if (! new_plan && (now - cmd_sent_at_) > cfg_resend_interval_) {
382  try {
383  //logger->log_info(name(), "Re-sending goal");
384  send_next_goal();
385  } catch (Exception &e) {
386  logger->log_warn(name(), "Failed to send next goal (resending)");
387  logger->log_warn(name(), e);
388  }
389  }
390  }
391  }
392 
393 #ifdef HAVE_VISUALIZATION
394  if (vt_) {
395  fawkes::Time now(clock);
396  if (now - visualized_at_ >= cfg_visual_interval_) {
397  *visualized_at_ = now;
398  constraint_repo_.lock();
399  if (constraint_repo_->compute() || constraint_repo_->modified(/* reset */ false)) {
400  vt_->wakeup();
401  }
402  constraint_repo_.unlock();
403  }
404  }
405 #endif
406 
407  if (needs_write) {
408  pp_nav_if_->write();
409  }
410 }
411 
413 NavGraphThread::load_graph(std::string filename)
414 {
415  std::ifstream inf(filename);
416  std::string firstword;
417  inf >> firstword;
418  inf.close();
419 
420  if (firstword == "%YAML") {
421  logger->log_info(name(), "Loading YAML graph from %s", filename.c_str());
422  return fawkes::LockPtr<NavGraph>(load_yaml_navgraph(filename), /* recursive mutex */ true);
423  } else {
424  throw Exception("Unknown graph format");
425  }
426 }
427 
428 bool
429 NavGraphThread::generate_plan(std::string goal_name)
430 {
431  if (! tf_listener->transform_origin(cfg_base_frame_, cfg_global_frame_, pose_)) {
432  logger->log_warn(name(),
433  "Failed to compute pose, cannot generate plan");
434  pp_nav_if_->set_final(true);
435  pp_nav_if_->set_error_code(NavigatorInterface::ERROR_PATH_GEN_FAIL);
436  return false;
437  }
438 
440  graph_->closest_node(pose_.getOrigin().x(), pose_.getOrigin().y());
441  NavGraphNode goal = graph_->node(goal_name);
442 
443  if (! goal.is_valid()) {
444  logger->log_error(name(), "Failed to generate path from (%.2f,%.2f) to %s: goal is unknown",
445  init.x(), init.y(), goal_name.c_str());
446  pp_nav_if_->set_final(true);
447  pp_nav_if_->set_error_code(NavigatorInterface::ERROR_UNKNOWN_PLACE);
448  return false;
449  }
450 
451  logger->log_debug(name(), "Starting at (%f,%f), closest node is '%s'",
452  pose_.getOrigin().x(), pose_.getOrigin().y(), init.name().c_str());
453 
454  try {
455  path_ = graph_->search_path(init, goal, /* use constraints */ true);
456  } catch (Exception &e) {
457  logger->log_error(name(), "Failed to generate path from (%.2f,%.2f) to %s: %s",
458  init.x(), init.y(), goal_name.c_str(), e.what_no_backtrace());
459  pp_nav_if_->set_final(true);
460  pp_nav_if_->set_error_code(NavigatorInterface::ERROR_PATH_GEN_FAIL);
461  return false;
462  }
463 
464  if (! path_.empty()) {
465  constrained_plan_ = true;
466  } else {
467  constrained_plan_ = false;
468  logger->log_warn(name(), "Failed to generate plan, will try without constraints");
469  try {
470  path_ = graph_->search_path(init, goal, /* use constraints */ false);
471  } catch (Exception &e) {
472  pp_nav_if_->set_final(true);
473  pp_nav_if_->set_error_code(NavigatorInterface::ERROR_PATH_GEN_FAIL);
474  return false;
475  }
476  }
477 
478  if (path_.empty()) {
479  logger->log_error(name(), "Failed to generate plan to travel to '%s'",
480  goal_name.c_str());
481  }
482 
483  traversal_ = path_.traversal();
484  return true;
485 }
486 
487 bool
488 NavGraphThread::generate_plan(std::string goal_name, float ori)
489 {
490  if (generate_plan(goal_name)) {
491 
492  if (! path_.empty() && std::isfinite(ori)) {
493  path_.nodes_mutable().back().set_property("orientation", ori);
494  }
495 
496  traversal_ = path_.traversal();
497  return true;
498  } else {
499  pp_nav_if_->set_final(true);
500  pp_nav_if_->set_error_code(NavigatorInterface::ERROR_PATH_GEN_FAIL);
501  return false;
502  }
503 }
504 
505 bool
506 NavGraphThread::generate_plan(float x, float y, float ori)
507 {
508  NavGraphNode close_to_goal = graph_->closest_node(x, y);
509  if (generate_plan(close_to_goal.name())) {
510 
511  NavGraphNode n("free-target", x, y);
512  if (std::isfinite(ori)) {
513  n.set_property("orientation", ori);
514  }
515  graph_->apply_default_properties(n);
516  path_.add_node(n);
517  traversal_ = path_.traversal();
518  return true;
519 
520  } else {
521  pp_nav_if_->set_final(true);
522  pp_nav_if_->set_error_code(NavigatorInterface::ERROR_PATH_GEN_FAIL);
523  return false;
524  }
525 }
526 
527 
528 bool
529 NavGraphThread::replan(const NavGraphNode &start, const NavGraphNode &goal)
530 {
531  logger->log_debug(name(), "Starting at node '%s'", start.name().c_str());
532 
533  NavGraphNode act_goal = goal;
534 
535  NavGraphNode close_to_goal;
536  if (goal.name() == "free-target") {
537  close_to_goal = graph_->closest_node(goal.x(), goal.y());
538  act_goal = close_to_goal;
539  }
540 
541  NavGraphPath new_path =
542  graph_->search_path(start, act_goal,
543  /* use constraints */ true, /* compute constraints */ false);
544 
545  if (! new_path.empty()) {
546  // get cost of current plan
547  NavGraphNode pose("current-pose", pose_.getOrigin().x(), pose_.getOrigin().y());
548  float old_cost = graph_->cost(pose, traversal_.current()) + traversal_.remaining_cost();
549  float new_cost = new_path.cost();
550 
551  if (new_cost <= old_cost * cfg_replan_factor_) {
552  constrained_plan_ = true;
553  path_ = new_path;
554  if (goal.name() == "free-target") {
555  // add free target node again
556  path_.add_node(goal);
557  }
558  traversal_ = path_.traversal();
559  logger->log_info(name(), "Executing after re-planning from '%s' to '%s', "
560  "old cost: %f new cost: %f (%f * %f)",
561  start.name().c_str(), goal.name().c_str(),
562  old_cost, new_cost * cfg_replan_factor_, new_cost, cfg_replan_factor_);
563  return true;
564  } else {
565  logger->log_warn(name(), "Re-planning from '%s' to '%s' resulted in "
566  "more expensive plan: %f > %f (%f * %f), keeping old",
567  start.name().c_str(), goal.name().c_str(),
568  new_cost, old_cost * cfg_replan_factor_, old_cost, cfg_replan_factor_);
569  return false;
570  }
571  } else {
572  logger->log_error(name(), "Failed to re-plan from '%s' to '%s'",
573  start.name().c_str(), goal.name().c_str());
574  return false;
575  }
576 }
577 
578 
579 /** Optimize the current plan.
580  * Note that after generating a plan, the robot first needs to
581  * travel to the first actual node from a free position within
582  * the environment. It can happen, that this closest node lies
583  * in the opposite direction of the second node, hence the robot
584  * needs to "go back" first, and only then starts following
585  * the path. We can optimize this by removing the first node,
586  * so that the robot directly travels to the second node which
587  * "lies on the way".
588  */
589 void
590 NavGraphThread::optimize_plan()
591 {
592  if (traversal_.remaining() > 1) {
593  // get current position of robot in map frame
594  const NavGraphPath &path = traversal_.path();
595  double sqr_dist_a = ( pow(pose_.getOrigin().x() - path.nodes()[0].x(), 2) +
596  pow(pose_.getOrigin().y() - path.nodes()[0].y(), 2) );
597  double sqr_dist_b = ( pow(path.nodes()[0].x() - path.nodes()[1].x(), 2) +
598  pow(path.nodes()[0].y() - path.nodes()[1].y(), 2) );
599  double sqr_dist_c = ( pow(pose_.getOrigin().x() - path.nodes()[1].x(), 2) +
600  pow(pose_.getOrigin().y() - path.nodes()[1].y(), 2) );
601 
602  if (sqr_dist_a + sqr_dist_b >= sqr_dist_c){
603  traversal_.next();
604  }
605  }
606 }
607 
608 
609 void
610 NavGraphThread::start_plan()
611 {
612  path_planned_at_->stamp();
613 
614  target_reached_ = false;
615  target_ori_reached_ = false;
616  target_rotating_ = false;
617  if (traversal_.remaining() == 0) {
618  exec_active_ = false;
619  pp_nav_if_->set_final(true);
620  pp_nav_if_->set_error_code(NavigatorInterface::ERROR_UNKNOWN_PLACE);
621  logger->log_warn(name(), "Cannot start empty plan.");
622 
623 #ifdef HAVE_VISUALIZATION
624  if (vt_) {
625  vt_->reset_plan();
626  visualized_at_->stamp();
627  }
628 #endif
629 
630  } else {
631  traversal_.next();
632 
633  std::string m = path_.nodes()[0].name();
634  for (unsigned int i = 1; i < path_.size(); ++i) {
635  m += " - " + path_.nodes()[i].name();
636  }
637  logger->log_info(name(), "Starting route: %s", m.c_str());
638 #ifdef HAVE_VISUALIZATION
639  if (vt_) {
640  vt_->set_traversal(traversal_);
641  visualized_at_->stamp();
642  }
643 #endif
644 
645  exec_active_ = true;
646 
647  NavGraphNode final_target = path_.goal();
648 
649  pp_nav_if_->set_error_code(NavigatorInterface::ERROR_NONE);
650  pp_nav_if_->set_final(false);
651  pp_nav_if_->set_dest_x(final_target.x());
652  pp_nav_if_->set_dest_y(final_target.y());
653 
654  try {
655  logger->log_info(name(), "Sending next goal on plan start");
656  send_next_goal();
657  } catch (Exception &e) {
658  logger->log_warn(name(), "Failed to send next goal (start plan)");
659  logger->log_warn(name(), e);
660  }
661  }
662 
663  publish_path();
664 }
665 
666 
667 void
668 NavGraphThread::stop_motion()
669 {
671  try {
672  nav_if_->msgq_enqueue(stop);
673  } catch (Exception &e) {
674  logger->log_warn(name(), "Failed to stop motion, exception follows");
675  logger->log_warn(name(), e);
676  }
677  last_node_ = "";
678  exec_active_ = false;
679  target_ori_reached_ = false;
680  target_rotating_ = false;
681  pp_nav_if_->set_final(true);
682  traversal_.invalidate();
683 
684 #ifdef HAVE_VISUALIZATION
685  if (vt_) {
686  vt_->reset_plan();
687  visualized_at_->stamp();
688  }
689 #endif
690 
691 }
692 
693 
694 void
695 NavGraphThread::send_next_goal()
696 {
697  bool stop_at_target = false;
698  bool orient_at_target = false;
699 
700  if (! traversal_.running()) {
701  throw Exception("Cannot send next goal if plan is empty");
702  }
703 
704  const NavGraphNode &next_target = traversal_.current();
705 
706  float ori = NAN;
707  if ( traversal_.last() ) {
708  stop_at_target = true;
709 
710  if ( next_target.has_property("orientation") ) {
711  orient_at_target = true;
712 
713  // take the given orientation for the final node
714  ori = next_target.property_as_float("orientation");
715  }
716 
717  } else {
718  // set direction facing from next_target (what is the current point
719  // to drive to) to next point to drive to. So orientation is the
720  // direction from next_target to the target after that
721  const NavGraphNode &next_next_target = traversal_.peek_next();
722 
723  ori = atan2f( next_next_target.y() - next_target.y(),
724  next_next_target.x() - next_target.x());
725 
726  }
727 
728  // get target position in base frame
729  tf::Stamped<tf::Pose> tpose;
731  tposeglob(tf::Transform(tf::create_quaternion_from_yaw(ori),
732  tf::Vector3(next_target.x(), next_target.y(), 0)),
733  Time(0,0), cfg_global_frame_);
734  try {
735  tf_listener->transform_pose(cfg_base_frame_, tposeglob, tpose);
736  } catch (Exception &e) {
737  logger->log_warn(name(),
738  "Failed to compute pose, cannot generate plan", e.what());
739  throw;
740  }
741 
742  if( target_reached_ ) {
743  // no need for traveling anymore, just rotating
744  tpose.setOrigin(tf::Vector3(0.f, 0.f, 0.f));
745  }
746 
748  new NavigatorInterface::CartesianGotoMessage(tpose.getOrigin().x(),
749  tpose.getOrigin().y(),
750  tf::get_yaw(tpose.getRotation()));
751 
754  if ( orient_at_target ) {
755  orient_mode_msg = new NavigatorInterface::SetOrientationModeMessage(
756  fawkes::NavigatorInterface::OrientationMode::OrientAtTarget );
757  } else {
758  orient_mode_msg = new NavigatorInterface::SetOrientationModeMessage(
759  fawkes::NavigatorInterface::OrientationMode::OrientDuringTravel );
760  }
761 
762  try {
763 #ifdef HAVE_VISUALIZATION
764  if (vt_) vt_->set_current_edge(last_node_, next_target.name());
765 #endif
766 
767  if (! nav_if_->has_writer()) {
768  throw Exception("No writer for navigator interface");
769  }
770 
771  nav_if_->msgq_enqueue(stop_at_target_msg);
772  nav_if_->msgq_enqueue(orient_mode_msg);
773 
774  logger->log_debug(name(), "Sending goto(x=%f,y=%f,ori=%f) for node '%s'",
775  tpose.getOrigin().x(), tpose.getOrigin().y(),
776  tf::get_yaw(tpose.getRotation()), next_target.name().c_str());
777 
778  nav_if_->msgq_enqueue(gotomsg);
779  cmd_sent_at_->stamp();
780 
781  error_at_->stamp();
782  error_reason_ = "";
783 
784  } catch (Exception &e) {
785  if (cfg_abort_on_error_) {
786  logger->log_warn(name(), "Failed to send cartesian goto for "
787  "next goal, exception follows");
788  logger->log_warn(name(), e);
789  exec_active_ = false;
790  pp_nav_if_->set_final(true);
791  pp_nav_if_->set_error_code(NavigatorInterface::ERROR_OBSTRUCTION);
792  pp_nav_if_->write();
793 #ifdef HAVE_VISUALIZATION
794  if (vt_) vt_->reset_plan();
795 #endif
796  } else {
797  fawkes::Time now(clock);
798  if (error_reason_ != e.what_no_backtrace() || (now - error_at_) > 4.0) {
799  error_reason_ = e.what_no_backtrace();
800  *error_at_ = now;
801  logger->log_warn(name(), "Failed to send cartesian goto for "
802  "next goal, exception follows");
803  logger->log_warn(name(), e);
804  logger->log_warn(name(), "*** NOT aborting goal (as per config)");
805  }
806  }
807  }
808 }
809 
810 /** Check if current node has been reached.
811  * Compares the distance to the node to defined tolerances.
812  */
813 bool
814 NavGraphThread::node_reached()
815 {
816  if (! traversal_) {
817  logger->log_error(name(), "Cannot check node reached if no traversal given");
818  return true;
819  }
820 
821  if (! traversal_.running()) {
822  logger->log_error(name(), "Cannot check node reached if no traversal running");
823  return true;
824  }
825 
826  const NavGraphNode &cur_target = traversal_.current();
827 
828  // get distance to current target in map frame
829  float dist = sqrt(pow(pose_.getOrigin().x() - cur_target.x(), 2) +
830  pow( pose_.getOrigin().y() - cur_target.y(), 2));
831 
832  float tolerance = cur_target.property_as_float("travel_tolerance");
833  // use a different tolerance for the final node
834  if (traversal_.last()) {
835  tolerance = cur_target.property_as_float("target_tolerance");
836  //return (dist <= tolerance) && node_ori_reached(cur_target);
837  }
838 
839  // can be no or invalid tolerance, be very generous
840  if (tolerance == 0.) {
841  logger->log_warn(name(), "Invalid tolerance for node %s, using 1.0",
842  cur_target.name().c_str());
843  tolerance = 1.0;
844  }
845 
846  return (dist <= tolerance);
847 }
848 
849 
850 /** Check if orientation of current node has been reached.
851  * Compares the angular distance to the targeted orientation
852  * to the defined angular tolerances.
853  */
854 bool
855 NavGraphThread::node_ori_reached()
856 {
857  if (! traversal_) {
858  logger->log_error(name(), "Cannot check node reached if no traversal given");
859  return true;
860  }
861 
862  if (! traversal_.running()) {
863  logger->log_error(name(), "Cannot check node reached if no traversal running");
864  return true;
865  }
866 
867  const NavGraphNode &cur_target = traversal_.current();
868  return node_ori_reached(cur_target);
869 }
870 
871 
872 /** Check if orientation of a given node has been reached.
873  * Compares the angular distance to the targeted orientation
874  * to the defined angular tolerances.
875  */
876 bool
877 NavGraphThread::node_ori_reached(const NavGraphNode &node)
878 {
879  if (node.has_property("orientation")) {
880  float ori_tolerance = node.property_as_float("orientation_tolerance");
881  float ori_diff =
882  angle_distance( normalize_rad(tf::get_yaw(pose_.getRotation())),
883  normalize_rad(node.property_as_float("orientation")));
884 
885  //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);
886  return (ori_diff <= ori_tolerance);
887 
888  } else {
889  return true;
890  }
891 }
892 
893 
894 size_t
895 NavGraphThread::shortcut_possible()
896 {
897  if (!traversal_ || traversal_.remaining() < 1) {
898  logger->log_debug(name(), "Cannot shortcut if no path nodes remaining");
899  return 0;
900  }
901 
902  for (size_t i = traversal_.path().size() - 1; i > traversal_.current_index(); --i) {
903  const NavGraphNode &node = traversal_.path().nodes()[i];
904 
905  float dist = sqrt(pow(pose_.getOrigin().x() - node.x(), 2) +
906  pow(pose_.getOrigin().y() - node.y(), 2));
907 
908  float tolerance = node.property_as_float("shortcut_tolerance");
909 
910  if (tolerance == 0.0) return 0;
911  if (dist <= tolerance) return i;
912  }
913 
914  return 0;
915 }
916 
917 
918 void
919 NavGraphThread::fam_event(const char *filename, unsigned int mask)
920 {
921  // The file will be ignored from now onwards, re-register
922  if (mask & FAM_IGNORED) {
923  fam_->watch_file(cfg_graph_file_.c_str());
924  }
925 
926  if (mask & (FAM_MODIFY | FAM_IGNORED)) {
927  logger->log_info(name(), "Graph changed on disk, reloading");
928 
929  try {
930  LockPtr<NavGraph> new_graph = load_graph(cfg_graph_file_);
931  **graph_ = **new_graph;
932  } catch (Exception &e) {
933  logger->log_warn(name(), "Loading new graph failed, exception follows");
934  logger->log_warn(name(), e);
935  return;
936  }
937 
938 #ifdef HAVE_VISUALIZATION
939  if (vt_) {
940  vt_->set_graph(graph_);
941  visualized_at_->stamp();
942  }
943 #endif
944 
945  if (exec_active_) {
946  // store the goal and restart it after the graph has been reloaded
947 
948  stop_motion();
949  NavGraphNode goal = path_.goal();
950 
951  bool gen_ok = false;
952  if (goal.name() == "free-target") {
953  gen_ok = generate_plan(goal.x(), goal.y(), goal.property_as_float("orientation"));
954  } else {
955  gen_ok = generate_plan(goal.name());
956  }
957 
958  if (gen_ok) {
959  optimize_plan();
960  start_plan();
961  } else {
962  stop_motion();
963  }
964  }
965  }
966 }
967 
968 
969 void
970 NavGraphThread::log_graph()
971 {
972  const std::vector<NavGraphNode> & nodes = graph_->nodes();
973  std::vector<NavGraphNode>::const_iterator n;
974  for (n = nodes.begin(); n != nodes.end(); ++n) {
975  logger->log_info(name(), "Node %s @ (%f,%f)%s",
976  n->name().c_str(), n->x(), n->y(),
977  n->unconnected() ? " UNCONNECTED" : "");
978 
979  const std::map<std::string, std::string> &props = n->properties();
980  std::map<std::string, std::string>::const_iterator p;
981  for (p = props.begin(); p != props.end(); ++p) {
982  logger->log_info(name(), " - %s: %s", p->first.c_str(), p->second.c_str());
983  }
984  }
985 
986  std::vector<NavGraphEdge> edges = graph_->edges();
987  std::vector<NavGraphEdge>::iterator e;
988  for (e = edges.begin(); e != edges.end(); ++e) {
989  logger->log_info(name(), "Edge %10s --%s %s",
990  e->from().c_str(), e->is_directed() ? ">" : "-", e->to().c_str());
991 
992  const std::map<std::string, std::string> &props = e->properties();
993  std::map<std::string, std::string>::const_iterator p;
994  for (p = props.begin(); p != props.end(); ++p) {
995  logger->log_info(name(), " - %s: %s", p->first.c_str(), p->second.c_str());
996  }
997  }
998 }
999 
1000 void
1001 NavGraphThread::publish_path()
1002 {
1003  std::vector<std::string> vpath(40, "");
1004 
1005  if (traversal_) {
1006  size_t ind = 0;
1007  size_t r = traversal_.running() ? traversal_.current_index() : traversal_.remaining();
1008  for (; r < traversal_.path().size(); ++r) {
1009  vpath[ind++] = traversal_.path().nodes()[r].name();
1010  }
1011  }
1012 
1013  path_if_->set_path_node_1(vpath[0].c_str());
1014  path_if_->set_path_node_2(vpath[1].c_str());
1015  path_if_->set_path_node_3(vpath[2].c_str());
1016  path_if_->set_path_node_4(vpath[3].c_str());
1017  path_if_->set_path_node_5(vpath[4].c_str());
1018  path_if_->set_path_node_6(vpath[5].c_str());
1019  path_if_->set_path_node_7(vpath[6].c_str());
1020  path_if_->set_path_node_8(vpath[7].c_str());
1021  path_if_->set_path_node_9(vpath[8].c_str());
1022  path_if_->set_path_node_10(vpath[9].c_str());
1023  path_if_->set_path_node_11(vpath[10].c_str());
1024  path_if_->set_path_node_12(vpath[11].c_str());
1025  path_if_->set_path_node_13(vpath[12].c_str());
1026  path_if_->set_path_node_14(vpath[13].c_str());
1027  path_if_->set_path_node_15(vpath[14].c_str());
1028  path_if_->set_path_node_16(vpath[15].c_str());
1029  path_if_->set_path_node_17(vpath[16].c_str());
1030  path_if_->set_path_node_18(vpath[17].c_str());
1031  path_if_->set_path_node_19(vpath[18].c_str());
1032  path_if_->set_path_node_20(vpath[19].c_str());
1033  path_if_->set_path_node_21(vpath[20].c_str());
1034  path_if_->set_path_node_22(vpath[21].c_str());
1035  path_if_->set_path_node_23(vpath[22].c_str());
1036  path_if_->set_path_node_24(vpath[23].c_str());
1037  path_if_->set_path_node_25(vpath[24].c_str());
1038  path_if_->set_path_node_26(vpath[25].c_str());
1039  path_if_->set_path_node_27(vpath[26].c_str());
1040  path_if_->set_path_node_28(vpath[27].c_str());
1041  path_if_->set_path_node_29(vpath[28].c_str());
1042  path_if_->set_path_node_30(vpath[29].c_str());
1043  path_if_->set_path_node_31(vpath[30].c_str());
1044  path_if_->set_path_node_32(vpath[31].c_str());
1045  path_if_->set_path_node_33(vpath[32].c_str());
1046  path_if_->set_path_node_34(vpath[33].c_str());
1047  path_if_->set_path_node_35(vpath[34].c_str());
1048  path_if_->set_path_node_36(vpath[35].c_str());
1049  path_if_->set_path_node_37(vpath[36].c_str());
1050  path_if_->set_path_node_38(vpath[37].c_str());
1051  path_if_->set_path_node_39(vpath[38].c_str());
1052  path_if_->set_path_node_40(vpath[39].c_str());
1053  path_if_->set_path_length(traversal_.remaining());
1054  path_if_->write();
1055 }
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.