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 }
virtual void fam_event(const char *filename, unsigned int mask)
Event has been raised.
virtual void once()
Execute an action exactly once.
virtual void finalize()
Finalize the thread.
virtual void init()
Initialize the thread.
virtual ~NavGraphThread()
Destructor.
virtual void loop()
Code to execute in the thread.
NavGraphThread()
Constructor.
Send Marker messages to rviz to show navgraph info.
Thread aspect provide a new aspect.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual void close(Interface *interface)=0
Close interface.
Thread aspect to use blocked timing.
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:42
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual bool exists(const char *path)=0
Check if a given value exists.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual const char * what() const noexcept
Get primary string.
Definition: exception.cpp:639
virtual const char * what_no_backtrace() const noexcept
Get primary string (does not implicitly print the back trace).
Definition: exception.cpp:663
static const unsigned int FAM_MODIFY
File was modified.
Definition: fam.h:41
static const unsigned int FAM_IGNORED
File was ignored.
Definition: fam.h:57
Monitors files for changes.
Definition: fam.h:71
void watch_file(const char *filepath)
Watch a file.
Definition: fam.cpp:205
void process_events(int timeout=0)
Process events.
Definition: fam.cpp:283
void add_listener(FamListener *listener)
Add a listener.
Definition: fam.cpp:263
bool msgq_first_is()
Check if first message has desired type.
Definition: interface.h:351
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1215
Message * msgq_first()
Get the first message from the message queue.
Definition: interface.cpp:1200
unsigned int msgq_enqueue(Message *message, bool proxy=false)
Enqueue message at end of queue.
Definition: interface.cpp:915
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:501
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1062
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:479
bool has_writer() const
Check if there is a writer for the interface.
Definition: interface.cpp:848
LockPtr<> is a reference-counting shared lockable smartpointer.
Definition: lockptr.h:55
void lock() const
Lock access to the encapsulated object.
Definition: lockptr.h:257
void clear()
Set underlying instance to 0, decrementing reference count of existing instance appropriately.
Definition: lockptr.h:499
void unlock() const
Unlock object mutex.
Definition: lockptr.h:273
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
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.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
unsigned int id() const
Get message ID.
Definition: message.cpp:181
void set_navgraph(LockPtr< NavGraph > &navgraph)
Set navgraph.
bool modified(bool reset_modified=false)
Check if the constraint repo has been modified.
bool compute()
Call compute method on all registered constraints.
Topological graph node.
Definition: navgraph_node.h:36
float x() const
Get X coordinate in global frame.
Definition: navgraph_node.h:58
bool unconnected() const
Check if this node shall be unconnected.
Definition: navgraph_node.h:74
bool is_valid() const
Check if node is valid, i.e.
const std::string & name() const
Get name of node.
Definition: navgraph_node.h:50
float property_as_float(const std::string &prop) const
Get property converted to float.
bool has_property(const std::string &property) const
Check if node has specified property.
float y() const
Get Y coordinate in global frame.
Definition: navgraph_node.h:66
bool last() const
Check if the current node is the last node in the path.
float remaining_cost() const
Get the remaining cost to the goal.
size_t current_index() const
Get index of current node in path.
void invalidate()
Invalidate this traversal.
const NavGraphNode & current() const
Get current node in path.
bool next()
Move on traversal to next node.
const NavGraphPath & path() const
Get parent path the traversal belongs to.
Definition: navgraph_path.h:64
size_t remaining() const
Get the number of remaining nodes in path traversal.
bool running() const
Check if traversal is currently runnung.
void set_current(size_t new_current)
Set the current node.
const NavGraphNode & peek_next() const
Peek on the next node.
Class representing a path for a NavGraph.
Definition: navgraph_path.h:37
Traversal traversal() const
Get a new path traversal handle.
bool empty() const
Check if path is empty.
std::vector< NavGraphNode > & nodes_mutable()
Get nodes along the path as mutable vector.
const std::vector< NavGraphNode > & nodes() const
Get nodes along the path.
size_t size() const
Get size of path.
float cost() const
Get cost of path from start to to end.
const NavGraphNode & goal() const
Get goal of path.
void add_node(const NavGraphNode &node, float cost_from_end=0)
Add a node to the path.
Topological map graph.
Definition: navgraph.h:50
void remove_change_listener(ChangeListener *listener)
Remove a change listener.
Definition: navgraph.cpp:1484
bool has_default_property(const std::string &property) const
Check if graph has specified default property.
Definition: navgraph.cpp:769
float cost(const NavGraphNode &from, const NavGraphNode &to) const
Calculate cost between two adjacent nodes.
Definition: navgraph.cpp:1123
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
fawkes::LockPtr< NavGraphConstraintRepo > constraint_repo() const
Get locked pointer to constraint repository.
Definition: navgraph.cpp:152
const std::vector< NavGraphNode > & nodes() const
Get nodes of the graph.
Definition: navgraph.cpp:133
const std::vector< NavGraphEdge > & edges() const
Get edges of the graph.
Definition: navgraph.cpp:142
float default_property_as_float(const std::string &prop) const
Get property converted to float.
Definition: navgraph.cpp:794
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
NavGraphNode node(const std::string &name) const
Get a specified node.
Definition: navgraph.cpp:163
void apply_default_properties(NavGraphNode &node)
Set default properties on node for which no local value exists.
Definition: navgraph.cpp:875
void add_change_listener(ChangeListener *listener)
Add a change listener.
Definition: navgraph.cpp:1475
NavPathInterface Fawkes BlackBoard Interface.
void set_path_node_1(const char *new_path_node_1)
Set path_node_1 value.
void set_path_node_17(const char *new_path_node_17)
Set path_node_17 value.
void set_path_node_40(const char *new_path_node_40)
Set path_node_40 value.
void set_path_length(const uint32_t new_path_length)
Set path_length value.
void set_path_node_15(const char *new_path_node_15)
Set path_node_15 value.
void set_path_node_6(const char *new_path_node_6)
Set path_node_6 value.
void set_path_node_13(const char *new_path_node_13)
Set path_node_13 value.
void set_path_node_24(const char *new_path_node_24)
Set path_node_24 value.
void set_path_node_23(const char *new_path_node_23)
Set path_node_23 value.
void set_path_node_8(const char *new_path_node_8)
Set path_node_8 value.
void set_path_node_18(const char *new_path_node_18)
Set path_node_18 value.
void set_path_node_36(const char *new_path_node_36)
Set path_node_36 value.
void set_path_node_26(const char *new_path_node_26)
Set path_node_26 value.
void set_path_node_25(const char *new_path_node_25)
Set path_node_25 value.
void set_path_node_29(const char *new_path_node_29)
Set path_node_29 value.
void set_path_node_7(const char *new_path_node_7)
Set path_node_7 value.
void set_path_node_3(const char *new_path_node_3)
Set path_node_3 value.
void set_path_node_35(const char *new_path_node_35)
Set path_node_35 value.
void set_path_node_34(const char *new_path_node_34)
Set path_node_34 value.
void set_path_node_32(const char *new_path_node_32)
Set path_node_32 value.
void set_path_node_27(const char *new_path_node_27)
Set path_node_27 value.
void set_path_node_39(const char *new_path_node_39)
Set path_node_39 value.
void set_path_node_38(const char *new_path_node_38)
Set path_node_38 value.
void set_path_node_28(const char *new_path_node_28)
Set path_node_28 value.
void set_path_node_4(const char *new_path_node_4)
Set path_node_4 value.
void set_path_node_21(const char *new_path_node_21)
Set path_node_21 value.
void set_path_node_14(const char *new_path_node_14)
Set path_node_14 value.
void set_path_node_11(const char *new_path_node_11)
Set path_node_11 value.
void set_path_node_2(const char *new_path_node_2)
Set path_node_2 value.
void set_path_node_37(const char *new_path_node_37)
Set path_node_37 value.
void set_path_node_5(const char *new_path_node_5)
Set path_node_5 value.
void set_path_node_33(const char *new_path_node_33)
Set path_node_33 value.
void set_path_node_30(const char *new_path_node_30)
Set path_node_30 value.
void set_path_node_31(const char *new_path_node_31)
Set path_node_31 value.
void set_path_node_20(const char *new_path_node_20)
Set path_node_20 value.
void set_path_node_12(const char *new_path_node_12)
Set path_node_12 value.
void set_path_node_16(const char *new_path_node_16)
Set path_node_16 value.
void set_path_node_9(const char *new_path_node_9)
Set path_node_9 value.
void set_path_node_22(const char *new_path_node_22)
Set path_node_22 value.
void set_path_node_10(const char *new_path_node_10)
Set path_node_10 value.
void set_path_node_19(const char *new_path_node_19)
Set path_node_19 value.
CartesianGotoMessage Fawkes BlackBoard Interface Message.
float orientation() const
Get orientation value.
PlaceGotoMessage Fawkes BlackBoard Interface Message.
PlaceWithOriGotoMessage Fawkes BlackBoard Interface Message.
SetOrientationModeMessage Fawkes BlackBoard Interface Message.
SetStopAtTargetMessage Fawkes BlackBoard Interface Message.
StopMessage Fawkes BlackBoard Interface Message.
uint32_t msgid() const
Get msgid value.
NavigatorInterface Fawkes BlackBoard Interface.
bool is_final() const
Get final value.
void set_msgid(const uint32_t new_msgid)
Set msgid value.
void set_error_code(const uint32_t new_error_code)
Set error_code value.
uint32_t msgid() const
Get msgid value.
void set_dest_y(const float new_dest_y)
Set dest_y value.
void set_dest_x(const float new_dest_x)
Set dest_x value.
void set_final(const bool new_final)
Set final value.
void unref()
Decrement reference count and conditionally delete this instance.
Definition: refcount.cpp:95
void ref()
Increment reference count.
Definition: refcount.cpp:67
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
void start(bool wait=true)
Call this method to start the thread.
Definition: thread.cpp:499
A class for handling time.
Definition: time.h:93
Time & stamp()
Set this time to the current time.
Definition: time.cpp:704
tf::Transformer * tf_listener
This is the transform listener which saves transforms published by other threads in the system.
Definition: tf.h:67
Wrapper class to add time stamp and frame ID to base types.
Definition: types.h:130
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.
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.
Fawkes library namespace.
NavGraph * load_yaml_navgraph(std::string filename, bool allow_multi_graph)
Load topological map graph stored in RCSoft format.
float normalize_rad(float angle_rad)
Normalize angle in radian between 0 (inclusive) and 2*PI (exclusive).
Definition: angle.h:90
float angle_distance(float angle_rad1, float angle_rad2)
Determines the distance between two angle provided as radians.
Definition: angle.h:123