21 #include "navgraph_interactive_thread.h"
23 #include <interactive_markers/interactive_marker_server.h>
24 #include <navgraph/yaml_navgraph.h>
28 using namespace interactive_markers;
37 :
Thread(
"NavGraphInteractiveThread",
Thread::OPMODE_WAITFORWAKEUP)
47 NavGraphInteractiveThread::process_node_ori_feedback(
48 const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback,
49 const NodeMenu & menu,
50 visualization_msgs::InteractiveMarker & int_marker)
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);
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";
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.;
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;
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";
94 int_marker.controls.end());
95 handler->setCheckState(entry_handle, MenuHandler::UNCHECKED);
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";
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;
112 server_->insert(int_marker,
113 boost::bind(&NavGraphInteractiveThread::process_node_feedback,
this, _1));
114 server_->applyChanges();
115 handler->reApply(*server_);
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());
125 NavGraphInteractiveThread::process_node_feedback(
126 const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
128 switch (feedback->event_type) {
129 case visualization_msgs::InteractiveMarkerFeedback::BUTTON_CLICK:
break;
131 case visualization_msgs::InteractiveMarkerFeedback::MENU_SELECT:
133 "%s/%s: menu item %u clicked",
134 feedback->marker_name.c_str(),
135 feedback->control_name.c_str(),
136 feedback->menu_entry_id);
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) {
151 "Cannot goto %s, no writer for interface",
152 int_marker.name.c_str());
154 }
else if (feedback->menu_entry_id == menu.remove_handle) {
156 navgraph->remove_node(feedback->marker_name);
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];
172 "Failed to insert edge %s--%s as requested, exception follows",
173 int_marker.name.c_str(),
177 server_->erase(int_marker.name);
179 server_->erase(to_node);
182 server_->applyChanges();
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);
195 "Failed to insert edge %s->%s as requested, "
197 int_marker.name.c_str(),
198 menu.dir_connect_nodes[feedback->menu_entry_id].c_str());
201 server_->erase(int_marker.name);
204 server_->applyChanges();
206 }
else if (menu.disconnect_nodes.find(feedback->menu_entry_id)
207 != menu.disconnect_nodes.end()) {
209 std::string to_node = menu.disconnect_nodes[feedback->menu_entry_id];
213 "Failed to find edge %s--%s",
214 feedback->marker_name.c_str(),
219 server_->erase(feedback->marker_name);
222 server_->erase(to_node);
226 server_->applyChanges();
229 "Got menu feedback for %s/%s, but marker not known",
230 feedback->marker_name.c_str(),
231 feedback->control_name.c_str());
236 "Got feedback for %s/%s, but marker not known",
237 feedback->marker_name.c_str(),
238 feedback->control_name.c_str());
244 case visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE:
245 if (feedback->header.frame_id == cfg_global_frame_) {
249 if (feedback->control_name ==
"rot_z") {
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));
258 node.
set_x(feedback->pose.position.x);
259 node.
set_y(feedback->pose.position.y);
264 "Position update for %s, but not unknown",
265 feedback->marker_name.c_str());
271 "Interactive marker feedback in non-global frame %s, ignoring",
272 feedback->header.frame_id.c_str());
276 case visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN:
277 case visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP:
break;
280 server_->applyChanges();
284 NavGraphInteractiveThread::process_graph_feedback(
285 const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
287 if (feedback->event_type == visualization_msgs::InteractiveMarkerFeedback::MENU_SELECT) {
288 if (feedback->menu_entry_id == graph_menu_.stop_handle) {
295 }
else if (feedback->menu_entry_id == graph_menu_.add_handle) {
297 for (
unsigned int i = 0; i < 1000; ++i) {
298 std::string
name = NavGraph::format_name(
"N%u", i);
305 server_->applyChanges();
310 }
else if (feedback->menu_entry_id == graph_menu_.save_handle) {
325 server_ =
new interactive_markers::InteractiveMarkerServer(
"navgraph_interactive");
330 const std::vector<NavGraphNode> &nodes =
navgraph->nodes();
337 server_->applyChanges();
346 graph_menu_handler_.reset();
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);
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 =
"";
369 int_marker.scale = 0.5;
371 int_marker.pose.position.x = node.
x();
372 int_marker.pose.position.y = node.
y();
373 int_marker.pose.position.z = 0.;
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];
380 int_marker.pose.orientation.x = int_marker.pose.orientation.y = int_marker.pose.orientation.z =
382 int_marker.pose.orientation.w = 1.;
386 visualization_msgs::Marker box_marker;
388 box_marker.type = visualization_msgs::Marker::ARROW;
389 geometry_msgs::Point p1, p2;
390 p1.x = p1.y = p1.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;
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;
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;
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);
420 menu.handler = std::shared_ptr<MenuHandler>(
new MenuHandler());
422 menu.handler->insert(
"Orientation",
423 boost::bind(&NavGraphInteractiveThread::process_node_feedback,
this, _1));
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()) {
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());
440 if (edge == edges.end()) {
441 MenuHandler::EntryHandle undir_handle = menu.handler->insert(
442 connect_undir_menu_handle,
444 boost::bind(&NavGraphInteractiveThread::process_node_feedback, this, _1));
445 menu.undir_connect_nodes[undir_handle] = n.name();
447 MenuHandler::EntryHandle dir_handle = menu.handler->insert(
448 connect_dir_menu_handle,
450 boost::bind(&NavGraphInteractiveThread::process_node_feedback, this, _1));
451 menu.dir_connect_nodes[dir_handle] = n.name();
453 MenuHandler::EntryHandle handle = menu.handler->insert(
454 disconnect_menu_handle,
456 boost::bind(&NavGraphInteractiveThread::process_node_feedback, this, _1));
457 menu.disconnect_nodes[handle] = n.name();
462 MenuHandler::EntryHandle properties_menu_handle = menu.handler->insert(
"Properties");
464 std::string p_s = p.first +
": " + p.second;
465 menu.handler->insert(properties_menu_handle, p_s);
469 menu.handler->insert(
"Remove Node",
470 boost::bind(&NavGraphInteractiveThread::process_node_feedback,
this, _1));
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;
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);
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);
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);
505 server_->insert(int_marker,
506 boost::bind(&NavGraphInteractiveThread::process_node_feedback,
this, _1));
508 menu.handler->apply(*server_, int_marker.name);
509 node_menus_[int_marker.name] = menu;
513 NavGraphInteractiveThread::add_graph(
NavGraph *navgraph)
516 visualization_msgs::InteractiveMarker int_marker;
517 int_marker.header.frame_id = cfg_global_frame_;
519 int_marker.description =
"";
520 int_marker.scale = 0.5;
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 =
527 int_marker.pose.orientation.w = 1.;
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;
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);
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));
557 graph_menu_.stop_handle =
558 menu_handler->insert(
"STOP",
559 boost::bind(&NavGraphInteractiveThread::process_graph_feedback,
this, _1));
561 graph_menu_handler_ = menu_handler;
563 server_->insert(int_marker,
564 boost::bind(&NavGraphInteractiveThread::process_graph_feedback,
this, _1));
566 menu_handler->apply(*server_, int_marker.name);
NavGraphInteractiveThread()
Constructor.
virtual ~NavGraphInteractiveThread()
Destructor.
virtual void loop()
Code to execute in the thread.
virtual void finalize()
Finalize the thread.
virtual void init()
Initialize the thread.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
virtual void close(Interface *interface)=0
Close interface.
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.
Base class for exceptions in Fawkes.
bool has_writer() const
Check if there is a writer for the interface.
unsigned int msgq_enqueue(Message *message)
Enqueue message at end of queue.
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.
Logger * logger
This is the Logger member used to access the logger.
fawkes::LockPtr< NavGraph > navgraph
NavGraph instance shared in framework.
bool is_directed() const
Check if edge is directed.
float x() const
Get X coordinate in global frame.
void set_x(float x)
Set X position.
void set_property(const std::string &property, const std::string &value)
Set property.
const std::string & name() const
Get name of node.
const std::map< std::string, std::string > & properties() const
Get all properties.
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.
void set_y(float y)
Set Y position.
PlaceGotoMessage Fawkes BlackBoard Interface Message.
StopMessage Fawkes BlackBoard Interface Message.
NavigatorInterface Fawkes BlackBoard Interface.
Thread class encapsulation of pthreads.
const char * name() const
Get name of thread.
Fawkes library namespace.
void save_yaml_navgraph(std::string filename, NavGraph *graph)
Save navgraph to YAML file.