Fawkes API  Fawkes Development Version
navgraph_interactive_thread.cpp
1 /***************************************************************************
2  * navgraph_interactive_thread.cpp - Interactive navgraph editing
3  *
4  * Created: Thu Jan 15 16:26:40 2015
5  * Copyright 2015 Tim Niemueller [www.niemueller.de]
6  ****************************************************************************/
7 
8 /* This program is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2 of the License, or
11  * (at your option) any later version.
12  *
13  * This program is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU Library General Public License for more details.
17  *
18  * Read the full text in the LICENSE.GPL file in the doc directory.
19  */
20 
21 #include "navgraph_interactive_thread.h"
22 
23 #include <interactive_markers/interactive_marker_server.h>
24 #include <navgraph/yaml_navgraph.h>
25 #include <tf/types.h>
26 
27 using namespace fawkes;
28 using namespace interactive_markers;
29 
30 /** @class NavGraphInteractiveThread "navgraph_interactive_thread.h"
31  * Thread to perform graph-based path planning.
32  * @author Tim Niemueller
33  */
34 
35 /** Constructor. */
37 : Thread("NavGraphInteractiveThread", Thread::OPMODE_WAITFORWAKEUP)
38 {
39 }
40 
41 /** Destructor. */
43 {
44 }
45 
46 void
47 NavGraphInteractiveThread::process_node_ori_feedback(
48  const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback,
49  const NodeMenu & menu,
50  visualization_msgs::InteractiveMarker & int_marker)
51 {
52  const std::shared_ptr<MenuHandler> &handler = menu.handler;
53  MenuHandler::EntryHandle entry_handle = (MenuHandler::EntryHandle)feedback->menu_entry_id;
54  MenuHandler::CheckState check_state;
55  if (handler->getCheckState(entry_handle, check_state)) {
56  if (check_state == MenuHandler::UNCHECKED) {
57  visualization_msgs::InteractiveMarkerControl ori_control;
58  ori_control.name = "rot_z";
59  ori_control.orientation.w = 1;
60  ori_control.orientation.x = 0;
61  ori_control.orientation.y = 1;
62  ori_control.orientation.z = 0;
63  ori_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
64  int_marker.controls.push_back(ori_control);
65  handler->setCheckState(entry_handle, MenuHandler::CHECKED);
66 
67  auto control =
68  std::find_if(int_marker.controls.begin(),
69  int_marker.controls.end(),
70  [](const visualization_msgs::InteractiveMarkerControl &c) -> bool {
71  return c.name == "menu";
72  });
73  if (control != int_marker.controls.end() && !control->markers.empty()) {
74  visualization_msgs::Marker &box_marker = control->markers[0];
75  box_marker.type = visualization_msgs::Marker::ARROW;
76  box_marker.points.clear();
77  geometry_msgs::Point p1, p2;
78  p1.x = p1.y = p1.z = 0.;
79  p2.x = 0.2;
80  p2.y = p2.z = 0.;
81  box_marker.points.push_back(p1);
82  box_marker.points.push_back(p2);
83  box_marker.scale.x = 0.35;
84  box_marker.scale.y = 0.35;
85  box_marker.scale.z = 0.2;
86  }
87  } else {
88  int_marker.controls.erase(
89  std::remove_if(int_marker.controls.begin(),
90  int_marker.controls.end(),
91  [](const visualization_msgs::InteractiveMarkerControl &c) -> bool {
92  return c.name == "rot_z";
93  }),
94  int_marker.controls.end());
95  handler->setCheckState(entry_handle, MenuHandler::UNCHECKED);
96 
97  auto control =
98  std::find_if(int_marker.controls.begin(),
99  int_marker.controls.end(),
100  [](const visualization_msgs::InteractiveMarkerControl &c) -> bool {
101  return c.name == "menu";
102  });
103  if (control != int_marker.controls.end() && !control->markers.empty()) {
104  visualization_msgs::Marker &box_marker = control->markers[0];
105  box_marker.points.clear();
106  box_marker.type = visualization_msgs::Marker::SPHERE;
107  box_marker.scale.x = 0.25;
108  box_marker.scale.y = 0.25;
109  box_marker.scale.z = 0.25;
110  }
111  }
112  server_->insert(int_marker,
113  boost::bind(&NavGraphInteractiveThread::process_node_feedback, this, _1));
114  server_->applyChanges();
115  handler->reApply(*server_);
116  } else {
117  logger->log_warn(name(),
118  "Got menu feedback for %s/%s, but check state cannot be retrieved",
119  feedback->marker_name.c_str(),
120  feedback->control_name.c_str());
121  }
122 }
123 
124 void
125 NavGraphInteractiveThread::process_node_feedback(
126  const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
127 {
128  switch (feedback->event_type) {
129  case visualization_msgs::InteractiveMarkerFeedback::BUTTON_CLICK: break;
130 
131  case visualization_msgs::InteractiveMarkerFeedback::MENU_SELECT:
132  logger->log_debug(name(),
133  "%s/%s: menu item %u clicked",
134  feedback->marker_name.c_str(),
135  feedback->control_name.c_str(),
136  feedback->menu_entry_id);
137  {
138  visualization_msgs::InteractiveMarker int_marker;
139  if (server_->get(feedback->marker_name, int_marker)) {
140  if (node_menus_.find(int_marker.name) != node_menus_.end()) {
141  NodeMenu &menu = node_menus_[int_marker.name];
142  if (feedback->menu_entry_id == menu.ori_handle) {
143  process_node_ori_feedback(feedback, menu, int_marker);
144  } else if (feedback->menu_entry_id == menu.goto_handle) {
145  if (nav_if_->has_writer()) {
147  new NavigatorInterface::PlaceGotoMessage(int_marker.name.c_str());
148  nav_if_->msgq_enqueue(gotomsg);
149  } else {
150  logger->log_error(name(),
151  "Cannot goto %s, no writer for interface",
152  int_marker.name.c_str());
153  }
154  } else if (feedback->menu_entry_id == menu.remove_handle) {
155  navgraph.lock();
156  navgraph->remove_node(feedback->marker_name);
157  navgraph->calc_reachability(true);
158  navgraph.unlock();
159  server_->erase(feedback->marker_name);
160  server_->applyChanges();
161  } else if (menu.undir_connect_nodes.find(feedback->menu_entry_id)
162  != menu.undir_connect_nodes.end()) {
163  std::string to_node = menu.undir_connect_nodes[feedback->menu_entry_id];
164  NavGraphEdge edge(int_marker.name, to_node);
165  navgraph.lock();
166  try {
167  navgraph->add_edge(edge);
168  navgraph->calc_reachability(true);
169  } catch (Exception &e) {
170  navgraph.unlock();
171  logger->log_error(name(),
172  "Failed to insert edge %s--%s as requested, exception follows",
173  int_marker.name.c_str(),
174  to_node.c_str());
175  logger->log_error(name(), e);
176  }
177  server_->erase(int_marker.name);
178  add_node(navgraph->node(int_marker.name), *navgraph);
179  server_->erase(to_node);
180  add_node(navgraph->node(to_node), *navgraph);
181  navgraph.unlock();
182  server_->applyChanges();
183 
184  } else if (menu.dir_connect_nodes.find(feedback->menu_entry_id)
185  != menu.dir_connect_nodes.end()) {
186  NavGraphEdge edge(int_marker.name, menu.dir_connect_nodes[feedback->menu_entry_id]);
187  edge.set_directed(true);
188  navgraph.lock();
189  try {
190  navgraph->add_edge(edge);
191  navgraph->calc_reachability(true);
192  } catch (Exception &e) {
193  navgraph.unlock();
194  logger->log_error(name(),
195  "Failed to insert edge %s->%s as requested, "
196  "exception follows",
197  int_marker.name.c_str(),
198  menu.dir_connect_nodes[feedback->menu_entry_id].c_str());
199  logger->log_error(name(), e);
200  }
201  server_->erase(int_marker.name);
202  add_node(navgraph->node(int_marker.name), *navgraph);
203  navgraph.unlock();
204  server_->applyChanges();
205 
206  } else if (menu.disconnect_nodes.find(feedback->menu_entry_id)
207  != menu.disconnect_nodes.end()) {
208  navgraph.lock();
209  std::string to_node = menu.disconnect_nodes[feedback->menu_entry_id];
210  NavGraphEdge edge = navgraph->edge(feedback->marker_name, to_node);
211  if (!edge) {
212  logger->log_error(name(),
213  "Failed to find edge %s--%s",
214  feedback->marker_name.c_str(),
215  to_node.c_str());
216  }
217  navgraph->remove_edge(edge);
218  navgraph->calc_reachability(true);
219  server_->erase(feedback->marker_name);
220  add_node(navgraph->node(feedback->marker_name), *navgraph);
221  if (!edge.is_directed()) {
222  server_->erase(to_node);
223  add_node(navgraph->node(to_node), *navgraph);
224  }
225  navgraph.unlock();
226  server_->applyChanges();
227  } else {
228  logger->log_warn(name(),
229  "Got menu feedback for %s/%s, but marker not known",
230  feedback->marker_name.c_str(),
231  feedback->control_name.c_str());
232  }
233 
234  } else {
235  logger->log_warn(name(),
236  "Got feedback for %s/%s, but marker not known",
237  feedback->marker_name.c_str(),
238  feedback->control_name.c_str());
239  }
240  }
241  }
242  break;
243 
244  case visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE:
245  if (feedback->header.frame_id == cfg_global_frame_) {
246  navgraph.lock();
247  NavGraphNode node = navgraph->node(feedback->marker_name);
248  if (node) {
249  if (feedback->control_name == "rot_z") {
250  // orientation update
251  tf::Quaternion q(feedback->pose.orientation.x,
252  feedback->pose.orientation.y,
253  feedback->pose.orientation.z,
254  feedback->pose.orientation.w);
255  node.set_property(navgraph::PROP_ORIENTATION, (float)tf::get_yaw(q));
256  } else {
257  // position update
258  node.set_x(feedback->pose.position.x);
259  node.set_y(feedback->pose.position.y);
260  }
261  navgraph->update_node(node);
262  } else {
263  logger->log_warn(name(),
264  "Position update for %s, but not unknown",
265  feedback->marker_name.c_str());
266  }
267  navgraph.unlock();
268  navgraph->notify_of_change();
269  } else {
270  logger->log_warn(name(),
271  "Interactive marker feedback in non-global frame %s, ignoring",
272  feedback->header.frame_id.c_str());
273  }
274  break;
275 
276  case visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN:
277  case visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP: break;
278  }
279 
280  server_->applyChanges();
281 }
282 
283 void
284 NavGraphInteractiveThread::process_graph_feedback(
285  const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
286 {
287  if (feedback->event_type == visualization_msgs::InteractiveMarkerFeedback::MENU_SELECT) {
288  if (feedback->menu_entry_id == graph_menu_.stop_handle) {
289  if (nav_if_->has_writer()) {
291  nav_if_->msgq_enqueue(stop);
292  } else {
293  logger->log_error(name(), "Cannot stop, no writer for interface");
294  }
295  } else if (feedback->menu_entry_id == graph_menu_.add_handle) {
296  navgraph.lock();
297  for (unsigned int i = 0; i < 1000; ++i) {
298  std::string name = NavGraph::format_name("N%u", i);
299  if (!navgraph->node_exists(name)) {
300  // valid new node name, create node
301  NavGraphNode node(name, 0., 0.);
302  navgraph->add_node(node);
303  add_node(node, *navgraph);
304  navgraph->calc_reachability(true);
305  server_->applyChanges();
306  break;
307  }
308  }
309  navgraph.unlock();
310  } else if (feedback->menu_entry_id == graph_menu_.save_handle) {
311  navgraph.lock();
312  save_yaml_navgraph(cfg_save_filename_, *navgraph);
313  navgraph.unlock();
314  }
315  }
316 }
317 
318 void
320 {
321  cfg_global_frame_ = config->get_string("/navgraph/global_frame");
322  cfg_save_filename_ = config->get_string("/navgraph/interactive/out-file");
323 
324  // create an interactive marker server on the topic namespace simple_marker
325  server_ = new interactive_markers::InteractiveMarkerServer("navgraph_interactive");
326 
327  navgraph.lock();
328  add_graph(*navgraph);
329 
330  const std::vector<NavGraphNode> &nodes = navgraph->nodes();
331  for (const NavGraphNode &node : nodes) {
332  add_node(node, *navgraph);
333  }
334  navgraph.unlock();
335 
336  // 'commit' changes and send to all clients
337  server_->applyChanges();
338 
339  nav_if_ = blackboard->open_for_reading<NavigatorInterface>("Pathplan");
340 }
341 
342 void
344 {
345  node_menus_.clear();
346  graph_menu_handler_.reset();
347  delete server_;
348  blackboard->close(nav_if_);
349 }
350 
351 void
353 {
354 }
355 
356 void
357 NavGraphInteractiveThread::add_node(const NavGraphNode &node, NavGraph *navgraph)
358 {
359  const bool has_ori = node.has_property(navgraph::PROP_ORIENTATION);
360  const tf::Quaternion ori_q =
361  has_ori ? tf::create_quaternion_from_yaw(node.property_as_float(navgraph::PROP_ORIENTATION))
362  : tf::Quaternion(0, 0, 0, 1);
363 
364  // create an interactive marker for our server
365  visualization_msgs::InteractiveMarker int_marker;
366  int_marker.header.frame_id = cfg_global_frame_;
367  int_marker.name = node.name();
368  int_marker.description = ""; //node.name();
369  int_marker.scale = 0.5;
370 
371  int_marker.pose.position.x = node.x();
372  int_marker.pose.position.y = node.y();
373  int_marker.pose.position.z = 0.;
374  if (has_ori) {
375  int_marker.pose.orientation.x = ori_q[0];
376  int_marker.pose.orientation.y = ori_q[1];
377  int_marker.pose.orientation.z = ori_q[2];
378  int_marker.pose.orientation.w = ori_q[3];
379  } else {
380  int_marker.pose.orientation.x = int_marker.pose.orientation.y = int_marker.pose.orientation.z =
381  0.;
382  int_marker.pose.orientation.w = 1.;
383  }
384 
385  // create a grey box marker
386  visualization_msgs::Marker box_marker;
387  if (has_ori) {
388  box_marker.type = visualization_msgs::Marker::ARROW;
389  geometry_msgs::Point p1, p2;
390  p1.x = p1.y = p1.z = 0.;
391  p2.x = 0.2;
392  p2.y = p2.z = 0.;
393  box_marker.points.push_back(p1);
394  box_marker.points.push_back(p2);
395  box_marker.scale.x = 0.35;
396  box_marker.scale.y = 0.35;
397  box_marker.scale.z = 0.2;
398 
399  } else {
400  box_marker.type = visualization_msgs::Marker::SPHERE;
401  box_marker.scale.x = 0.25;
402  box_marker.scale.y = 0.25;
403  box_marker.scale.z = 0.25;
404  }
405  box_marker.color.r = 0.5;
406  box_marker.color.g = 0.5;
407  box_marker.color.b = 0.5;
408  box_marker.color.a = 1.0;
409 
410  // create a non-interactive control which contains the box
411  visualization_msgs::InteractiveMarkerControl box_control;
412  box_control.always_visible = true;
413  box_control.markers.push_back(box_marker);
414  box_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MENU;
415  box_control.description = "Options";
416  box_control.name = "menu";
417  int_marker.controls.push_back(box_control);
418 
419  NodeMenu menu;
420  menu.handler = std::shared_ptr<MenuHandler>(new MenuHandler());
421  menu.ori_handle =
422  menu.handler->insert("Orientation",
423  boost::bind(&NavGraphInteractiveThread::process_node_feedback, this, _1));
424  menu.goto_handle =
425  menu.handler->insert("Go to",
426  boost::bind(&NavGraphInteractiveThread::process_node_feedback, this, _1));
427  menu.handler->setCheckState(menu.ori_handle, MenuHandler::UNCHECKED);
428  const std::vector<NavGraphNode> &nodes = navgraph->nodes();
429  const std::vector<NavGraphEdge> &edges = navgraph->edges();
430  MenuHandler::EntryHandle connect_undir_menu_handle = menu.handler->insert("Connect with");
431  MenuHandler::EntryHandle connect_dir_menu_handle = menu.handler->insert("Connect directed");
432  MenuHandler::EntryHandle disconnect_menu_handle = menu.handler->insert("Disconnect from");
433  std::for_each(nodes.begin(), nodes.end(), [&, this](const NavGraphNode &n) -> void {
434  if (n.name() != node.name()) {
435  auto edge =
436  std::find_if(edges.begin(), edges.end(), [&n, &node](const NavGraphEdge &e) -> bool {
437  return (e.from() == node.name() && e.to() == n.name())
438  || (!e.is_directed() && e.from() == n.name() && e.to() == node.name());
439  });
440  if (edge == edges.end()) {
441  MenuHandler::EntryHandle undir_handle = menu.handler->insert(
442  connect_undir_menu_handle,
443  n.name(),
444  boost::bind(&NavGraphInteractiveThread::process_node_feedback, this, _1));
445  menu.undir_connect_nodes[undir_handle] = n.name();
446 
447  MenuHandler::EntryHandle dir_handle = menu.handler->insert(
448  connect_dir_menu_handle,
449  n.name(),
450  boost::bind(&NavGraphInteractiveThread::process_node_feedback, this, _1));
451  menu.dir_connect_nodes[dir_handle] = n.name();
452  } else {
453  MenuHandler::EntryHandle handle = menu.handler->insert(
454  disconnect_menu_handle,
455  n.name(),
456  boost::bind(&NavGraphInteractiveThread::process_node_feedback, this, _1));
457  menu.disconnect_nodes[handle] = n.name();
458  }
459  }
460  });
461 
462  MenuHandler::EntryHandle properties_menu_handle = menu.handler->insert("Properties");
463  for (const auto &p : node.properties()) {
464  std::string p_s = p.first + ": " + p.second;
465  menu.handler->insert(properties_menu_handle, p_s);
466  }
467 
468  menu.remove_handle =
469  menu.handler->insert("Remove Node",
470  boost::bind(&NavGraphInteractiveThread::process_node_feedback, this, _1));
471 
472  // create a control which will move the box
473  // this control does not contain any markers,
474  // which will cause RViz to insert two arrows
475  visualization_msgs::InteractiveMarkerControl pos_control;
476  pos_control.orientation_mode = visualization_msgs::InteractiveMarkerControl::FIXED;
477  pos_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
478 
479  pos_control.name = "move_x";
480  pos_control.orientation.x = 0.;
481  pos_control.orientation.y = 0.;
482  pos_control.orientation.z = 0.;
483  pos_control.orientation.w = 1.;
484  int_marker.controls.push_back(pos_control);
485 
486  pos_control.name = "move_y";
487  pos_control.orientation.x = 0.;
488  pos_control.orientation.y = 0.;
489  pos_control.orientation.z = 1.;
490  pos_control.orientation.w = 1.;
491  int_marker.controls.push_back(pos_control);
492 
493  if (has_ori) {
494  visualization_msgs::InteractiveMarkerControl ori_control;
495  ori_control.name = "rot_z";
496  ori_control.orientation.w = 1;
497  ori_control.orientation.x = 0;
498  ori_control.orientation.y = 1;
499  ori_control.orientation.z = 0;
500  ori_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
501  int_marker.controls.push_back(ori_control);
502  menu.handler->setCheckState(menu.ori_handle, MenuHandler::CHECKED);
503  }
504 
505  server_->insert(int_marker,
506  boost::bind(&NavGraphInteractiveThread::process_node_feedback, this, _1));
507 
508  menu.handler->apply(*server_, int_marker.name);
509  node_menus_[int_marker.name] = menu;
510 }
511 
512 void
513 NavGraphInteractiveThread::add_graph(NavGraph *navgraph)
514 {
515  // create an interactive marker for our server
516  visualization_msgs::InteractiveMarker int_marker;
517  int_marker.header.frame_id = cfg_global_frame_;
518  int_marker.name = navgraph->name();
519  int_marker.description = "";
520  int_marker.scale = 0.5;
521 
522  int_marker.pose.position.x = 0.;
523  int_marker.pose.position.y = 0.;
524  int_marker.pose.position.z = 1.;
525  int_marker.pose.orientation.x = int_marker.pose.orientation.y = int_marker.pose.orientation.z =
526  0.;
527  int_marker.pose.orientation.w = 1.;
528 
529  // create a grey box marker
530  visualization_msgs::Marker box_marker;
531  box_marker.type = visualization_msgs::Marker::CUBE;
532  box_marker.scale.x = 0.25;
533  box_marker.scale.y = 0.25;
534  box_marker.scale.z = 0.25;
535  box_marker.color.r = 0.5;
536  box_marker.color.g = 0.5;
537  box_marker.color.b = 0.5;
538  box_marker.color.a = 1.0;
539 
540  // create a non-interactive control which contains the box
541  visualization_msgs::InteractiveMarkerControl box_control;
542  box_control.always_visible = true;
543  box_control.markers.push_back(box_marker);
544  box_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MENU;
545  box_control.description = "Graph Ops";
546  box_control.name = "menu";
547  int_marker.controls.push_back(box_control);
548 
549  std::shared_ptr<MenuHandler> menu_handler(new MenuHandler());
550  graph_menu_.add_handle =
551  menu_handler->insert("Add Node",
552  boost::bind(&NavGraphInteractiveThread::process_graph_feedback, this, _1));
553  graph_menu_.save_handle =
554  menu_handler->insert("Save Graph",
555  boost::bind(&NavGraphInteractiveThread::process_graph_feedback, this, _1));
556 
557  graph_menu_.stop_handle =
558  menu_handler->insert("STOP",
559  boost::bind(&NavGraphInteractiveThread::process_graph_feedback, this, _1));
560 
561  graph_menu_handler_ = menu_handler;
562 
563  server_->insert(int_marker,
564  boost::bind(&NavGraphInteractiveThread::process_graph_feedback, this, _1));
565 
566  menu_handler->apply(*server_, int_marker.name);
567 }
fawkes::NavGraphNode
Topological graph node.
Definition: navgraph_node.h:36
fawkes::NavGraphNode::has_property
bool has_property(const std::string &property) const
Check if node has specified property.
Definition: navgraph_node.h:117
NavGraphInteractiveThread::NavGraphInteractiveThread
NavGraphInteractiveThread()
Constructor.
Definition: navgraph_interactive_thread.cpp:36
fawkes::NavigatorInterface::StopMessage
StopMessage Fawkes BlackBoard Interface Message.
Definition: NavigatorInterface.h:111
fawkes::NavGraphEdge
Topological graph edge.
Definition: navgraph_edge.h:38
fawkes::NavGraphNode::set_property
void set_property(const std::string &property, const std::string &value)
Set property.
Definition: navgraph_node.cpp:138
fawkes::NavGraphNode::set_y
void set_y(float y)
Set Y position.
Definition: navgraph_node.cpp:80
fawkes::Thread::name
const char * name() const
Get name of thread.
Definition: thread.h:100
fawkes::NavGraphNode::name
const std::string & name() const
Get name of node.
Definition: navgraph_node.h:50
fawkes::NavGraphNode::properties
const std::map< std::string, std::string > & properties() const
Get all properties.
Definition: navgraph_node.h:107
fawkes::NavGraph
Topological map graph.
Definition: navgraph.h:50
NavGraphInteractiveThread::finalize
virtual void finalize()
Finalize the thread.
Definition: navgraph_interactive_thread.cpp:343
NavGraphInteractiveThread::~NavGraphInteractiveThread
virtual ~NavGraphInteractiveThread()
Destructor.
Definition: navgraph_interactive_thread.cpp:42
fawkes::LoggingAspect::logger
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
fawkes::BlackBoard::close
virtual void close(Interface *interface)=0
Close interface.
fawkes::save_yaml_navgraph
void save_yaml_navgraph(std::string filename, NavGraph *graph)
Save navgraph to YAML file.
Definition: yaml_navgraph.cpp:386
fawkes::Logger::log_error
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
fawkes
Fawkes library namespace.
fawkes::NavGraphNode::x
float x() const
Get X coordinate in global frame.
Definition: navgraph_node.h:58
fawkes::NavGraphEdge::is_directed
bool is_directed() const
Check if edge is directed.
Definition: navgraph_edge.h:162
fawkes::NavGraphNode::property_as_float
float property_as_float(const std::string &prop) const
Get property converted to float.
Definition: navgraph_node.h:144
fawkes::Logger::log_warn
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
fawkes::Interface::has_writer
bool has_writer() const
Check if there is a writer for the interface.
Definition: interface.cpp:817
fawkes::NavGraphNode::y
float y() const
Get Y coordinate in global frame.
Definition: navgraph_node.h:66
fawkes::ConfigurableAspect::config
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
fawkes::NavGraphNode::set_x
void set_x(float x)
Set X position.
Definition: navgraph_node.cpp:71
fawkes::NavigatorInterface::PlaceGotoMessage
PlaceGotoMessage Fawkes BlackBoard Interface Message.
Definition: NavigatorInterface.h:377
fawkes::Thread
Thread class encapsulation of pthreads.
Definition: thread.h:46
fawkes::BlackBoardAspect::blackboard
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
fawkes::NavGraphAspect::navgraph
fawkes::LockPtr< NavGraph > navgraph
NavGraph instance shared in framework.
Definition: navgraph.h:44
NavGraphInteractiveThread::loop
virtual void loop()
Code to execute in the thread.
Definition: navgraph_interactive_thread.cpp:352
fawkes::Configuration::get_string
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
fawkes::BlackBoard::open_for_reading
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
NavGraphInteractiveThread::init
virtual void init()
Initialize the thread.
Definition: navgraph_interactive_thread.cpp:319
fawkes::Interface::msgq_enqueue
unsigned int msgq_enqueue(Message *message)
Enqueue message at end of queue.
Definition: interface.cpp:882
fawkes::Logger::log_debug
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
fawkes::NavigatorInterface
NavigatorInterface Fawkes BlackBoard Interface.
Definition: NavigatorInterface.h:34
fawkes::Exception
Base class for exceptions in Fawkes.
Definition: exception.h:36