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
33using 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),
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),
56 AspectProviderAspect(&navgraph_aspect_inifin_)
57{
58 vt_ = vt;
59}
60#endif
61
62/** Destructor. */
64{
65}
66
67void
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");
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
196void
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
217void
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
228void
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
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 {
246 "Received stop message for non-active command "
247 "(got %u, running %u)",
248 msg->msgid(),
249 pp_nav_if_->msgid());
250 }
251
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 {
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) {
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
440NavGraphThread::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
456bool
457NavGraphThread::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
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) {
499 "Failed to generate path from (%.2f,%.2f) to %s: %s",
500 init.x(),
501 init.y(),
502 goal_name.c_str(),
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
538bool
539NavGraphThread::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
557bool
558NavGraphThread::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()) {
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
589bool
590NavGraphThread::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();
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 {
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 {
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 */
662void
663NavGraphThread::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
681void
682NavGraphThread::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
741void
742NavGraphThread::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
770void
771NavGraphThread::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
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
831 if (orient_at_target) {
833 fawkes::NavigatorInterface::OrientationMode::OrientAtTarget);
834 } else {
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
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_) {
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;
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 */
900bool
901NavGraphThread::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 */
939bool
940NavGraphThread::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 */
960bool
961NavGraphThread::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
976size_t
977NavGraphThread::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
1001void
1002NavGraphThread::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
1051void
1052NavGraphThread::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
1088void
1089NavGraphThread::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.
const NavGraphPath & path() const
Get parent path the traversal belongs to.
Definition: navgraph_path.h:64
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.
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.
size_t size() const
Get size of path.
const std::vector< NavGraphNode > & nodes() const
Get nodes along the 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.
std::vector< NavGraphNode > & nodes_mutable()
Get nodes along the path as mutable vector.
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