21 #include "navgraph_interactive_thread.h" 23 #include <navgraph/yaml_navgraph.h> 26 #include <interactive_markers/interactive_marker_server.h> 38 :
Thread(
"NavGraphInteractiveThread",
Thread::OPMODE_WAITFORWAKEUP)
49 NavGraphInteractiveThread::process_node_ori_feedback
50 (
const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback,
51 const NodeMenu &menu, visualization_msgs::InteractiveMarker &int_marker)
53 const std::shared_ptr<MenuHandler> &handler = menu.handler;
54 MenuHandler::EntryHandle entry_handle = (MenuHandler::EntryHandle)feedback->menu_entry_id;
55 MenuHandler::CheckState check_state;
56 if (handler->getCheckState(entry_handle, check_state)) {
57 if (check_state == MenuHandler::UNCHECKED) {
58 visualization_msgs::InteractiveMarkerControl ori_control;
59 ori_control.name =
"rot_z";
60 ori_control.orientation.w = 1;
61 ori_control.orientation.x = 0;
62 ori_control.orientation.y = 1;
63 ori_control.orientation.z = 0;
64 ori_control.interaction_mode =
65 visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
66 int_marker.controls.push_back(ori_control);
67 handler->setCheckState(entry_handle, MenuHandler::CHECKED);
70 std::find_if(int_marker.controls.begin(), int_marker.controls.end(),
71 [](
const visualization_msgs::InteractiveMarkerControl &c)->
bool {
72 return c.name ==
"menu";
74 if (control != int_marker.controls.end() && ! control->markers.empty()) {
75 visualization_msgs::Marker &box_marker = control->markers[0];
76 box_marker.type = visualization_msgs::Marker::ARROW;
77 box_marker.points.clear();
78 geometry_msgs::Point p1, p2;
79 p1.x = p1.y = p1.z = 0.;
82 box_marker.points.push_back(p1);
83 box_marker.points.push_back(p2);
84 box_marker.scale.x = 0.35;
85 box_marker.scale.y = 0.35;
86 box_marker.scale.z = 0.2;
89 int_marker.controls.erase(
90 std::remove_if(int_marker.controls.begin(), int_marker.controls.end(),
91 [](
const visualization_msgs::InteractiveMarkerControl &c)->
bool {
92 return c.name ==
"rot_z";
94 int_marker.controls.end());
95 handler->setCheckState(entry_handle, MenuHandler::UNCHECKED);
98 std::find_if(int_marker.controls.begin(), int_marker.controls.end(),
99 [](
const visualization_msgs::InteractiveMarkerControl &c)->
bool {
100 return c.name ==
"menu";
102 if (control != int_marker.controls.end() && ! control->markers.empty()) {
103 visualization_msgs::Marker &box_marker = control->markers[0];
104 box_marker.points.clear();
105 box_marker.type = visualization_msgs::Marker::SPHERE;
106 box_marker.scale.x = 0.25;
107 box_marker.scale.y = 0.25;
108 box_marker.scale.z = 0.25;
111 server_->insert(int_marker, boost::bind(&NavGraphInteractiveThread::process_node_feedback,
this, _1));
112 server_->applyChanges();
113 handler->reApply(*server_);
115 logger->
log_warn(
name(),
"Got menu feedback for %s/%s, but check state cannot be retrieved",
116 feedback->marker_name.c_str(), feedback->control_name.c_str());
121 NavGraphInteractiveThread::process_node_feedback(
const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
123 switch ( feedback->event_type )
125 case visualization_msgs::InteractiveMarkerFeedback::BUTTON_CLICK:
128 case visualization_msgs::InteractiveMarkerFeedback::MENU_SELECT:
130 feedback->marker_name.c_str(), feedback->control_name.c_str(),
131 feedback->menu_entry_id);
133 visualization_msgs::InteractiveMarker int_marker;
134 if (server_->get(feedback->marker_name, int_marker)) {
135 if (node_menus_.find(int_marker.name) != node_menus_.end()) {
136 NodeMenu &menu = node_menus_[int_marker.name];
137 if (feedback->menu_entry_id == menu.ori_handle) {
138 process_node_ori_feedback(feedback, menu, int_marker);
139 }
else if (feedback->menu_entry_id == menu.goto_handle) {
146 int_marker.name.c_str());
148 }
else if (feedback->menu_entry_id == menu.remove_handle) {
150 navgraph->remove_node(feedback->marker_name);
153 server_->erase(feedback->marker_name);
154 server_->applyChanges();
155 }
else if (menu.undir_connect_nodes.find(feedback->menu_entry_id) != menu.undir_connect_nodes.end()) {
156 std::string to_node = menu.undir_connect_nodes[feedback->menu_entry_id];
165 int_marker.name.c_str(), to_node.c_str());
168 server_->erase(int_marker.name);
170 server_->erase(to_node);
173 server_->applyChanges();
175 }
else if (menu.dir_connect_nodes.find(feedback->menu_entry_id) != menu.dir_connect_nodes.end()) {
177 menu.dir_connect_nodes[feedback->menu_entry_id]);
186 "exception follows", int_marker.name.c_str(),
187 menu.dir_connect_nodes[feedback->menu_entry_id].c_str());
190 server_->erase(int_marker.name);
193 server_->applyChanges();
195 }
else if (menu.disconnect_nodes.find(feedback->menu_entry_id) != menu.disconnect_nodes.end()) {
197 std::string to_node = menu.disconnect_nodes[feedback->menu_entry_id];
201 feedback->marker_name.c_str(), to_node.c_str());
205 server_->erase(feedback->marker_name);
208 server_->erase(to_node);
212 server_->applyChanges();
215 feedback->marker_name.c_str(), feedback->control_name.c_str());
220 feedback->marker_name.c_str(), feedback->control_name.c_str());
226 case visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE:
227 if (feedback->header.frame_id == cfg_global_frame_) {
232 if (feedback->control_name ==
"rot_z") {
234 tf::Quaternion q(feedback->pose.orientation.x, feedback->pose.orientation.y,
235 feedback->pose.orientation.z, feedback->pose.orientation.w);
236 node.
set_property(navgraph::PROP_ORIENTATION, (
float)tf::get_yaw(q));
239 node.
set_x(feedback->pose.position.x);
240 node.
set_y(feedback->pose.position.y);
245 feedback->marker_name.c_str());
250 logger->
log_warn(
name(),
"Interactive marker feedback in non-global frame %s, ignoring",
251 feedback->header.frame_id.c_str());
255 case visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN:
256 case visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP:
260 server_->applyChanges();
265 NavGraphInteractiveThread::process_graph_feedback(
const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
267 if (feedback->event_type == visualization_msgs::InteractiveMarkerFeedback::MENU_SELECT) {
268 if (feedback->menu_entry_id == graph_menu_.stop_handle) {
275 }
else if (feedback->menu_entry_id == graph_menu_.add_handle) {
277 for (
unsigned int i = 0; i < 1000; ++i) {
278 std::string
name = NavGraph::format_name(
"N%u", i);
279 if (!
navgraph->node_exists(name)) {
285 server_->applyChanges();
290 }
else if (feedback->menu_entry_id == graph_menu_.save_handle) {
305 server_ =
new interactive_markers::InteractiveMarkerServer(
"navgraph_interactive");
310 const std::vector<NavGraphNode> &nodes =
navgraph->nodes();
317 server_->applyChanges();
326 graph_menu_handler_.reset();
340 const bool has_ori = node.
has_property(navgraph::PROP_ORIENTATION);
341 const tf::Quaternion ori_q = has_ori
342 ? tf::create_quaternion_from_yaw(node.
property_as_float(navgraph::PROP_ORIENTATION))
343 : tf::Quaternion(0,0,0,1);
346 visualization_msgs::InteractiveMarker int_marker;
347 int_marker.header.frame_id = cfg_global_frame_;
348 int_marker.name = node.
name();
349 int_marker.description =
"";
350 int_marker.scale = 0.5;
352 int_marker.pose.position.x = node.
x();
353 int_marker.pose.position.y = node.
y();
354 int_marker.pose.position.z = 0.;
356 int_marker.pose.orientation.x = ori_q[0];
357 int_marker.pose.orientation.y = ori_q[1];
358 int_marker.pose.orientation.z = ori_q[2];
359 int_marker.pose.orientation.w = ori_q[3];
361 int_marker.pose.orientation.x = int_marker.pose.orientation.y = int_marker.pose.orientation.z = 0.;
362 int_marker.pose.orientation.w = 1.;
366 visualization_msgs::Marker box_marker;
368 box_marker.type = visualization_msgs::Marker::ARROW;
369 geometry_msgs::Point p1, p2;
370 p1.x = p1.y = p1.z = 0.;
373 box_marker.points.push_back(p1);
374 box_marker.points.push_back(p2);
375 box_marker.scale.x = 0.35;
376 box_marker.scale.y = 0.35;
377 box_marker.scale.z = 0.2;
380 box_marker.type = visualization_msgs::Marker::SPHERE;
381 box_marker.scale.x = 0.25;
382 box_marker.scale.y = 0.25;
383 box_marker.scale.z = 0.25;
385 box_marker.color.r = 0.5;
386 box_marker.color.g = 0.5;
387 box_marker.color.b = 0.5;
388 box_marker.color.a = 1.0;
391 visualization_msgs::InteractiveMarkerControl box_control;
392 box_control.always_visible =
true;
393 box_control.markers.push_back(box_marker);
394 box_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MENU;
395 box_control.description=
"Options";
396 box_control.name=
"menu";
397 int_marker.controls.push_back(box_control);
400 menu.handler = std::shared_ptr<MenuHandler>(
new MenuHandler());
402 menu.handler->insert(
"Orientation", boost::bind(&NavGraphInteractiveThread::process_node_feedback,
this, _1));
404 menu.handler->insert(
"Go to", boost::bind(&NavGraphInteractiveThread::process_node_feedback,
this, _1));
405 menu.handler->setCheckState(menu.ori_handle, MenuHandler::UNCHECKED);
406 const std::vector<NavGraphNode> &nodes = navgraph->
nodes();
407 const std::vector<NavGraphEdge> &edges = navgraph->
edges();
408 MenuHandler::EntryHandle connect_undir_menu_handle = menu.handler->insert(
"Connect with");
409 MenuHandler::EntryHandle connect_dir_menu_handle = menu.handler->insert(
"Connect directed");
410 MenuHandler::EntryHandle disconnect_menu_handle = menu.handler->insert(
"Disconnect from");
411 std::for_each(nodes.begin(), nodes.end(),
413 if (n.name() != node.
name()) {
414 auto edge = std::find_if(edges.begin(), edges.end(),
417 (e.from() == node.
name() &&
418 e.to() == n.name()) ||
419 (! e.is_directed() &&
420 e.from() == n.name() &&
421 e.to() == node.
name());
423 if (edge == edges.end()) {
424 MenuHandler::EntryHandle undir_handle =
425 menu.handler->insert(connect_undir_menu_handle, n.name(),
426 boost::bind(&NavGraphInteractiveThread::process_node_feedback,
this, _1));
427 menu.undir_connect_nodes[undir_handle] = n.name();
429 MenuHandler::EntryHandle dir_handle =
430 menu.handler->insert(connect_dir_menu_handle, n.name(),
431 boost::bind(&NavGraphInteractiveThread::process_node_feedback,
this, _1));
432 menu.dir_connect_nodes[dir_handle] = n.name();
434 MenuHandler::EntryHandle handle =
435 menu.handler->insert(disconnect_menu_handle, n.name(),
436 boost::bind(&NavGraphInteractiveThread::process_node_feedback,
this, _1));
437 menu.disconnect_nodes[handle] = n.name();
442 MenuHandler::EntryHandle properties_menu_handle = menu.handler->insert(
"Properties");
444 std::string p_s = p.first +
": " + p.second;
445 menu.handler->insert(properties_menu_handle, p_s);
449 menu.handler->insert(
"Remove Node",
450 boost::bind(&NavGraphInteractiveThread::process_node_feedback,
456 visualization_msgs::InteractiveMarkerControl pos_control;
457 pos_control.orientation_mode =
458 visualization_msgs::InteractiveMarkerControl::FIXED;
459 pos_control.interaction_mode =
460 visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
462 pos_control.name =
"move_x";
463 pos_control.orientation.x = 0.;
464 pos_control.orientation.y = 0.;
465 pos_control.orientation.z = 0.;
466 pos_control.orientation.w = 1.;
467 int_marker.controls.push_back(pos_control);
469 pos_control.name =
"move_y";
470 pos_control.orientation.x = 0.;
471 pos_control.orientation.y = 0.;
472 pos_control.orientation.z = 1.;
473 pos_control.orientation.w = 1.;
474 int_marker.controls.push_back(pos_control);
477 visualization_msgs::InteractiveMarkerControl ori_control;
478 ori_control.name =
"rot_z";
479 ori_control.orientation.w = 1;
480 ori_control.orientation.x = 0;
481 ori_control.orientation.y = 1;
482 ori_control.orientation.z = 0;
483 ori_control.interaction_mode =
484 visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
485 int_marker.controls.push_back(ori_control);
486 menu.handler->setCheckState(menu.ori_handle, MenuHandler::CHECKED);
489 server_->insert(int_marker, boost::bind(&NavGraphInteractiveThread::process_node_feedback,
this, _1));
491 menu.handler->apply(*server_, int_marker.name);
492 node_menus_[int_marker.name] = menu;
497 NavGraphInteractiveThread::add_graph(
NavGraph *navgraph)
500 visualization_msgs::InteractiveMarker int_marker;
501 int_marker.header.frame_id = cfg_global_frame_;
502 int_marker.name = navgraph->
name();
503 int_marker.description =
"";
504 int_marker.scale = 0.5;
506 int_marker.pose.position.x = 0.;
507 int_marker.pose.position.y = 0.;
508 int_marker.pose.position.z = 1.;
509 int_marker.pose.orientation.x = int_marker.pose.orientation.y = int_marker.pose.orientation.z = 0.;
510 int_marker.pose.orientation.w = 1.;
513 visualization_msgs::Marker box_marker;
514 box_marker.type = visualization_msgs::Marker::CUBE;
515 box_marker.scale.x = 0.25;
516 box_marker.scale.y = 0.25;
517 box_marker.scale.z = 0.25;
518 box_marker.color.r = 0.5;
519 box_marker.color.g = 0.5;
520 box_marker.color.b = 0.5;
521 box_marker.color.a = 1.0;
524 visualization_msgs::InteractiveMarkerControl box_control;
525 box_control.always_visible =
true;
526 box_control.markers.push_back(box_marker);
527 box_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MENU;
528 box_control.description=
"Graph Ops";
529 box_control.name=
"menu";
530 int_marker.controls.push_back(box_control);
532 std::shared_ptr<MenuHandler> menu_handler(
new MenuHandler());
533 graph_menu_.add_handle =
534 menu_handler->insert(
"Add Node",
535 boost::bind(&NavGraphInteractiveThread::process_graph_feedback,
537 graph_menu_.save_handle =
538 menu_handler->insert(
"Save Graph",
539 boost::bind(&NavGraphInteractiveThread::process_graph_feedback,
542 graph_menu_.stop_handle =
543 menu_handler->insert(
"STOP",
544 boost::bind(&NavGraphInteractiveThread::process_graph_feedback,
547 graph_menu_handler_ = menu_handler;
549 server_->insert(int_marker,
550 boost::bind(&NavGraphInteractiveThread::process_graph_feedback,
this, _1));
552 menu_handler->apply(*server_, int_marker.name);
virtual ~NavGraphInteractiveThread()
Destructor.
NavGraphInteractiveThread()
Constructor.
fawkes::LockPtr< NavGraph > navgraph
NavGraph instance shared in framework.
virtual void init()
Initialize the thread.
Fawkes library namespace.
bool is_directed() const
Check if edge is directed.
void save_yaml_navgraph(std::string filename, NavGraph *graph)
Save navgraph to YAML file.
void set_y(float y)
Set Y position.
bool has_property(const std::string &property) const
Check if node has specified property.
Thread class encapsulation of pthreads.
Logger * logger
This is the Logger member used to access the logger.
const std::vector< NavGraphNode > & nodes() const
Get nodes of the graph.
const std::map< std::string, std::string > & properties() const
Get all properties.
std::string name() const
Get graph name.
PlaceGotoMessage Fawkes BlackBoard Interface Message.
virtual void loop()
Code to execute in the thread.
Base class for exceptions in Fawkes.
void set_property(const std::string &property, const std::string &value)
Set property.
const std::string & name() const
Get name of node.
bool has_writer() const
Check if there is a writer for the interface.
void set_directed(bool directed)
Set directed state.
const char * name() const
Get name of thread.
const std::vector< NavGraphEdge > & edges() const
Get edges of the graph.
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.
float y() const
Get Y coordinate in global frame.
virtual void finalize()
Finalize the thread.
unsigned int msgq_enqueue(Message *message)
Enqueue message at end of queue.
void set_x(float x)
Set X position.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
float x() const
Get X coordinate in global frame.
float property_as_float(const std::string &prop) const
Get property converted to float.
Configuration * config
This is the Configuration member used to access the configuration.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
StopMessage Fawkes BlackBoard Interface Message.
NavigatorInterface Fawkes BlackBoard Interface.
virtual void close(Interface *interface)=0
Close interface.