void
NavGraphInteractiveThread::process_node_feedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
{
switch ( feedback->event_type )
{
case visualization_msgs::InteractiveMarkerFeedback::BUTTON_CLICK:
break;
case visualization_msgs::InteractiveMarkerFeedback::MENU_SELECT:
logger->log_debug(name(), "%s/%s: menu item %u clicked",
feedback->marker_name.c_str(), feedback->control_name.c_str(),
feedback->menu_entry_id);
{
visualization_msgs::InteractiveMarker int_marker;
if (server_->get(feedback->marker_name, int_marker)) {
if (node_menus_.find(int_marker.name) != node_menus_.end()) {
NodeMenu &menu = node_menus_[int_marker.name];
if (feedback->menu_entry_id == menu.ori_handle) {
process_node_ori_feedback(feedback, menu, int_marker);
} else if (feedback->menu_entry_id == menu.goto_handle) {
if (nav_if_->has_writer()) {
NavigatorInterface::PlaceGotoMessage *gotomsg =
new NavigatorInterface::PlaceGotoMessage(int_marker.name.c_str());
nav_if_->msgq_enqueue(gotomsg);
} else {
logger->log_error(name(), "Cannot goto %s, no writer for interface",
int_marker.name.c_str());
}
} else if (feedback->menu_entry_id == menu.remove_handle) {
navgraph.lock();
navgraph->remove_node(feedback->marker_name);
navgraph->calc_reachability(true);
navgraph.unlock();
server_->erase(feedback->marker_name);
server_->applyChanges();
} else if (menu.undir_connect_nodes.find(feedback->menu_entry_id) != menu.undir_connect_nodes.end()) {
std::string to_node = menu.undir_connect_nodes[feedback->menu_entry_id];
NavGraphEdge edge(int_marker.name, to_node);
navgraph.lock();
try {
navgraph->add_edge(edge);
navgraph->calc_reachability(true);
} catch (Exception &e) {
navgraph.unlock();
logger->log_error(name(), "Failed to insert edge %s--%s as requested, exception follows",
int_marker.name.c_str(), to_node.c_str());
logger->log_error(name(), e);
}
server_->erase(int_marker.name);
add_node(navgraph->node(int_marker.name), *navgraph);
server_->erase(to_node);
add_node(navgraph->node(to_node), *navgraph);
navgraph.unlock();
server_->applyChanges();
} else if (menu.dir_connect_nodes.find(feedback->menu_entry_id) != menu.dir_connect_nodes.end()) {
NavGraphEdge edge(int_marker.name,
menu.dir_connect_nodes[feedback->menu_entry_id]);
edge.set_directed(true);
navgraph.lock();
try {
navgraph->add_edge(edge);
navgraph->calc_reachability(true);
} catch (Exception &e) {
navgraph.unlock();
logger->log_error(name(), "Failed to insert edge %s->%s as requested, "
"exception follows", int_marker.name.c_str(),
menu.dir_connect_nodes[feedback->menu_entry_id].c_str());
logger->log_error(name(), e);
}
server_->erase(int_marker.name);
add_node(navgraph->node(int_marker.name), *navgraph);
navgraph.unlock();
server_->applyChanges();
} else if (menu.disconnect_nodes.find(feedback->menu_entry_id) != menu.disconnect_nodes.end()) {
navgraph.lock();
std::string to_node = menu.disconnect_nodes[feedback->menu_entry_id];
NavGraphEdge edge = navgraph->edge(feedback->marker_name, to_node);
if (! edge) {
logger->log_error(name(), "Failed to find edge %s--%s",
feedback->marker_name.c_str(), to_node.c_str());
}
navgraph->remove_edge(edge);
navgraph->calc_reachability(true);
server_->erase(feedback->marker_name);
add_node(navgraph->node(feedback->marker_name), *navgraph);
if (! edge.is_directed()) {
server_->erase(to_node);
add_node(navgraph->node(to_node), *navgraph);
}
navgraph.unlock();
server_->applyChanges();
} else {
logger->log_warn(name(), "Got menu feedback for %s/%s, but marker not known",
feedback->marker_name.c_str(), feedback->control_name.c_str());
}
} else {
logger->log_warn(name(), "Got feedback for %s/%s, but marker not known",
feedback->marker_name.c_str(), feedback->control_name.c_str());
}
}
}
break;
case visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE:
if (feedback->header.frame_id == cfg_global_frame_) {
navgraph.lock();
NavGraphNode node = navgraph->node(feedback->marker_name);
if (node) {
if (feedback->control_name == "rot_z") {
// orientation update
tf::Quaternion q(feedback->pose.orientation.x, feedback->pose.orientation.y,
feedback->pose.orientation.z, feedback->pose.orientation.w);
node.set_property(navgraph::PROP_ORIENTATION, (float)tf::get_yaw(q));
} else {
// position update
node.set_x(feedback->pose.position.x);
node.set_y(feedback->pose.position.y);
}
navgraph->update_node(node);
} else {
logger->log_warn(name(), "Position update for %s, but not unknown",
feedback->marker_name.c_str());
}
navgraph.unlock();
navgraph->notify_of_change();
} else {
logger->log_warn(name(), "Interactive marker feedback in non-global frame %s, ignoring",
feedback->header.frame_id.c_str());
}
break;
case visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN:
case visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP:
break;
}
server_->applyChanges();
}
