void
NavGraphInteractiveThread::add_node(const NavGraphNode &node, NavGraph *navgraph)
{
const bool has_ori = node.has_property(navgraph::PROP_ORIENTATION);
const tf::Quaternion ori_q = has_ori
? tf::create_quaternion_from_yaw(node.property_as_float(navgraph::PROP_ORIENTATION))
: tf::Quaternion(0,0,0,1);
// create an interactive marker for our server
visualization_msgs::InteractiveMarker int_marker;
int_marker.header.frame_id = cfg_global_frame_;
int_marker.name = node.name();
int_marker.description = ""; //node.name();
int_marker.scale = 0.5;
int_marker.pose.position.x = node.x();
int_marker.pose.position.y = node.y();
int_marker.pose.position.z = 0.;
if (has_ori) {
int_marker.pose.orientation.x = ori_q[0];
int_marker.pose.orientation.y = ori_q[1];
int_marker.pose.orientation.z = ori_q[2];
int_marker.pose.orientation.w = ori_q[3];
} else {
int_marker.pose.orientation.x = int_marker.pose.orientation.y = int_marker.pose.orientation.z = 0.;
int_marker.pose.orientation.w = 1.;
}
// create a grey box marker
visualization_msgs::Marker box_marker;
if (has_ori) {
box_marker.type = visualization_msgs::Marker::ARROW;
geometry_msgs::Point p1, p2;
p1.x = p1.y = p1.z = 0.;
p2.x = 0.2;
p2.y = p2.z = 0.;
box_marker.points.push_back(p1);
box_marker.points.push_back(p2);
box_marker.scale.x = 0.35;
box_marker.scale.y = 0.35;
box_marker.scale.z = 0.2;
} else {
box_marker.type = visualization_msgs::Marker::SPHERE;
box_marker.scale.x = 0.25;
box_marker.scale.y = 0.25;
box_marker.scale.z = 0.25;
}
box_marker.color.r = 0.5;
box_marker.color.g = 0.5;
box_marker.color.b = 0.5;
box_marker.color.a = 1.0;
// create a non-interactive control which contains the box
visualization_msgs::InteractiveMarkerControl box_control;
box_control.always_visible = true;
box_control.markers.push_back(box_marker);
box_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MENU;
box_control.description="Options";
box_control.name="menu";
int_marker.controls.push_back(box_control);
NodeMenu menu;
menu.handler = std::shared_ptr<MenuHandler>(new MenuHandler());
menu.ori_handle =
menu.handler->insert("Orientation", boost::bind(&NavGraphInteractiveThread::process_node_feedback, this, _1));
menu.goto_handle =
menu.handler->insert("Go to", boost::bind(&NavGraphInteractiveThread::process_node_feedback, this, _1));
menu.handler->setCheckState(menu.ori_handle, MenuHandler::UNCHECKED);
const std::vector<NavGraphNode> &nodes = navgraph->nodes();
const std::vector<NavGraphEdge> &edges = navgraph->edges();
MenuHandler::EntryHandle connect_undir_menu_handle = menu.handler->insert("Connect with");
MenuHandler::EntryHandle connect_dir_menu_handle = menu.handler->insert("Connect directed");
MenuHandler::EntryHandle disconnect_menu_handle = menu.handler->insert("Disconnect from");
std::for_each(nodes.begin(), nodes.end(),
[&, this](const NavGraphNode &n)->void {
if (n.name() != node.name()) {
auto edge = std::find_if(edges.begin(), edges.end(),
[&n, &node](const NavGraphEdge &e)->bool {
return
(e.from() == node.name() &&
e.to() == n.name()) ||
(! e.is_directed() &&
e.from() == n.name() &&
e.to() == node.name());
});
if (edge == edges.end()) {
MenuHandler::EntryHandle undir_handle =
menu.handler->insert(connect_undir_menu_handle, n.name(),
boost::bind(&NavGraphInteractiveThread::process_node_feedback, this, _1));
menu.undir_connect_nodes[undir_handle] = n.name();
MenuHandler::EntryHandle dir_handle =
menu.handler->insert(connect_dir_menu_handle, n.name(),
boost::bind(&NavGraphInteractiveThread::process_node_feedback, this, _1));
menu.dir_connect_nodes[dir_handle] = n.name();
} else {
MenuHandler::EntryHandle handle =
menu.handler->insert(disconnect_menu_handle, n.name(),
boost::bind(&NavGraphInteractiveThread::process_node_feedback, this, _1));
menu.disconnect_nodes[handle] = n.name();
}
}
});
MenuHandler::EntryHandle properties_menu_handle = menu.handler->insert("Properties");
for (const auto &p : node.properties()) {
std::string p_s = p.first + ": " + p.second;
menu.handler->insert(properties_menu_handle, p_s);
}
menu.remove_handle =
menu.handler->insert("Remove Node",
boost::bind(&NavGraphInteractiveThread::process_node_feedback,
this, _1));
// create a control which will move the box
// this control does not contain any markers,
// which will cause RViz to insert two arrows
visualization_msgs::InteractiveMarkerControl pos_control;
pos_control.orientation_mode =
visualization_msgs::InteractiveMarkerControl::FIXED;
pos_control.interaction_mode =
visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
pos_control.name = "move_x";
pos_control.orientation.x = 0.;
pos_control.orientation.y = 0.;
pos_control.orientation.z = 0.;
pos_control.orientation.w = 1.;
int_marker.controls.push_back(pos_control);
pos_control.name = "move_y";
pos_control.orientation.x = 0.;
pos_control.orientation.y = 0.;
pos_control.orientation.z = 1.;
pos_control.orientation.w = 1.;
int_marker.controls.push_back(pos_control);
if (has_ori) {
visualization_msgs::InteractiveMarkerControl ori_control;
ori_control.name = "rot_z";
ori_control.orientation.w = 1;
ori_control.orientation.x = 0;
ori_control.orientation.y = 1;
ori_control.orientation.z = 0;
ori_control.interaction_mode =
visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
int_marker.controls.push_back(ori_control);
menu.handler->setCheckState(menu.ori_handle, MenuHandler::CHECKED);
}
server_->insert(int_marker, boost::bind(&NavGraphInteractiveThread::process_node_feedback, this, _1));
menu.handler->apply(*server_, int_marker.name);
node_menus_[int_marker.name] = menu;
}
