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