/** Add an edge. * @param edge edge to add * @param mode edge add mode * @param allow_existing if true allow an edge with the given parameters * to exist. In that case the method silently returns. Note that you might * loose edge properties in that case. If false, an exception is thrown if * a similar edge exists. * @throw Exception thrown if an edge for the same nodes already exist unless * @p allow_existing is set true, then simply returns. * Takes directed edges into account. So if a directed edge from p1 to p2 * exists, it is ok to add a (directed or undirected) edge from p2 to p1. */ void NavGraph::add_edge(const NavGraphEdge &edge, NavGraph::EdgeMode mode, bool allow_existing) { if (edge_exists(edge)) { if (allow_existing) return; else throw Exception("Edge from %s to %s already exists", edge.from().c_str(), edge.to().c_str()); } else { switch (mode) { case EDGE_NO_INTERSECTION: edge_add_no_intersection(edge); break; case EDGE_SPLIT_INTERSECTION: edge_add_split_intersection(edge); break; case EDGE_FORCE: edges_.push_back(edge); edges_.back().set_nodes(node(edge.from()), node(edge.to())); break; } reachability_calced_ = false; notify_of_change(); } }
/** Update a given edge. * Will search for an edge with the same originating and target node as the * given edge and will then call the assignment operator. This is intended * to update properties of an edge. * @param edge edge to update */ void NavGraph::update_edge(const NavGraphEdge &edge) { std::vector<NavGraphEdge>::iterator e = std::find(edges_.begin(), edges_.end(), edge); if (e != edges_.end()) { *e = edge; } else { throw Exception("No edge from %s to %s is known", edge.from().c_str(), edge.to().c_str()); } }
/** Remove an edge * @param edge edge to remove */ void NavGraph::remove_edge(const NavGraphEdge &edge) { edges_.erase( std::remove_if(edges_.begin(), edges_.end(), [&edge](const NavGraphEdge &e)->bool { return (edge.from() == e.from() && edge.to() == e.to()) || (! e.is_directed() && (edge.from() == e.to() && edge.to() == e.from())); }), edges_.end()); reachability_calced_ = false; notify_of_change(); }
/** Connect node to closest edge * @param n node to connect to closest node */ void NavGraph::connect_node_to_closest_edge(const NavGraphNode &n) { NavGraphEdge closest = closest_edge(n.x(), n.y()); cart_coord_2d_t p = closest.closest_point_on_edge(n.x(), n.y()); NavGraphNode closest_conn = closest_node(p.x, p.y); NavGraphNode cn; if (almost_equal(closest_conn.distance(p.x, p.y), 0.f, 2)) { cn = closest_conn; } else { cn = NavGraphNode(NavGraph::format_name("C-%s", n.name().c_str()), p.x, p.y); } if (closest.from() == cn.name() || closest.to() == cn.name()) { // we actually want to connect to one of the end nodes of the edge, // simply add the new edge and we are done NavGraphEdge new_edge(cn.name(), n.name()); new_edge.set_property("generated", true); new_edge.set_property("created-for", cn.name() + "--" + n.name()); add_edge(new_edge); } else { // we are inserting a new point into the edge remove_edge(closest); NavGraphEdge new_edge_1(closest.from(), cn.name()); NavGraphEdge new_edge_2(closest.to(), cn.name()); NavGraphEdge new_edge_3(cn.name(), n.name()); new_edge_1.set_properties(closest.properties()); new_edge_2.set_properties(closest.properties()); new_edge_3.set_property("created-for", cn.name() + "--" + n.name()); new_edge_3.set_property("generated", true); if (! node_exists(cn)) add_node(cn); add_edge(new_edge_1); add_edge(new_edge_2); add_edge(new_edge_3); } }
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(); }
void NavGraph::edge_add_split_intersection(const NavGraphEdge &edge) { std::list<std::pair<cart_coord_2d_t, NavGraphEdge>> intersections; const NavGraphNode &n1 = node(edge.from()); const NavGraphNode &n2 = node(edge.to()); try { for (const NavGraphEdge &e : edges_) { cart_coord_2d_t ip; if (e.intersection(n1.x(), n1.y(), n2.x(), n2.y(), ip)) { // we need to split the edge at the given intersection point, // and the new line segments as well intersections.push_back(std::make_pair(ip, e)); } } std::list<std::list<std::pair<cart_coord_2d_t, NavGraphEdge> >::iterator> deletions; for (auto i1 = intersections.begin(); i1 != intersections.end(); ++i1) { const std::pair<cart_coord_2d_t, NavGraphEdge> &p1 = *i1; const cart_coord_2d_t &c1 = p1.first; const NavGraphEdge &e1 = p1.second; const NavGraphNode &n1_from = node(e1.from()); const NavGraphNode &n1_to = node(e1.to()); for (auto i2 = std::next(i1); i2 != intersections.end(); ++i2) { const std::pair<cart_coord_2d_t, NavGraphEdge> &p2 = *i2; const cart_coord_2d_t &c2 = p2.first; const NavGraphEdge &e2 = p2.second; if (points_different(c1.x, c1.y, c2.x, c2.y)) continue; float d = 1.; if (e1.from() == e2.from() || e1.from() == e2.to()) { d = point_dist(n1_from.x(), n1_from.y(), c1.x, c1.y); } else if (e1.to() == e2.to() || e1.to() == e2.from()) { d = point_dist(n1_to.x(), n1_to.y(), c1.x, c1.y); } if (d < 1e-4) { // the intersection point is the same as a common end // point of the two edges, only keep it once deletions.push_back(i1); break; } } } for (auto d = deletions.rbegin(); d != deletions.rend(); ++d) { intersections.erase(*d); } if (intersections.empty()) { NavGraphEdge e(edge); e.set_property("created-for", edge.from() + "--" + edge.to()); add_edge(e, EDGE_FORCE); } else { Eigen::Vector2f e_origin(n1.x(), n1.y()); Eigen::Vector2f e_target(n2.x(), n2.y()); Eigen::Vector2f e_dir = (e_target - e_origin).normalized(); intersections.sort([&e_origin, &e_dir](const std::pair<cart_coord_2d_t, NavGraphEdge> &p1, const std::pair<cart_coord_2d_t, NavGraphEdge> &p2) { const Eigen::Vector2f p1p(p1.first.x, p1.first.y); const Eigen::Vector2f p2p(p2.first.x, p2.first.y); const float k1 = e_dir.dot(p1p - e_origin); const float k2 = e_dir.dot(p2p - e_origin); return k1 < k2; }); std::string en_from = edge.from(); cart_coord_2d_t ec_from(n1.x(), n1.y()); std::string prev_to; for (const auto &i : intersections) { const cart_coord_2d_t &c = i.first; const NavGraphEdge &e = i.second; // add intersection point (if necessary) NavGraphNode ip = closest_node(c.x, c.y); if (! ip || points_different(c.x, c.y, ip.x(), ip.y())) { ip = NavGraphNode(gen_unique_name(), c.x, c.y); add_node(ip); } // if neither edge end node is the intersection point, split the edge if (ip.name() != e.from() && ip.name() != e.to()) { NavGraphEdge e1(e.from(), ip.name(), e.is_directed()); NavGraphEdge e2(ip.name(), e.to(), e.is_directed()); remove_edge(e); e1.set_properties(e.properties()); e2.set_properties(e.properties()); add_edge(e1, EDGE_FORCE, /* allow existing */ true); add_edge(e2, EDGE_FORCE, /* allow existing */ true); // this is a special case: we might intersect an edge // which has the same end node and thus the new edge // from the intersection node to the end node would // be added twice prev_to = e.to(); } // add segment edge if (en_from != ip.name() && prev_to != ip.name()) { NavGraphEdge e3(en_from, ip.name(), edge.is_directed()); e3.set_property("created-for", en_from + "--" + ip.name()); add_edge(e3, EDGE_FORCE, /* allow existing */ true); } en_from = ip.name(); ec_from = c; } if (en_from != edge.to()) { NavGraphEdge e3(en_from, edge.to(), edge.is_directed()); e3.set_property("created-for", en_from + "--" + edge.to()); add_edge(e3, EDGE_FORCE, /* allow existing */ true); } } } catch (Exception &ex) { throw Exception("Failed to add edge %s-%s%s: %s", edge.from().c_str(), edge.is_directed() ? ">" : "-", edge.to().c_str(), ex.what_no_backtrace()); } }
void NavGraph::edge_add_no_intersection(const NavGraphEdge &edge) { try { const NavGraphNode &n1 = node(edge.from()); const NavGraphNode &n2 = node(edge.to()); for (const NavGraphEdge &ne : edges_) { if (edge.from() == ne.from() || edge.from() == ne.to() || edge.to() == ne.to() || edge.to() == ne.from()) continue; if (ne.intersects(n1.x(), n1.y(), n2.x(), n2.y())) { throw Exception("Edge %s-%s%s not added: intersects with %s-%s%s", edge.from().c_str(), edge.is_directed() ? ">" : "-", edge.to().c_str(), ne.from().c_str(), ne.is_directed() ? ">" : "-", ne.to().c_str()); } } add_edge(edge, EDGE_FORCE); } catch (Exception &ex) { throw Exception("Failed to add edge %s-%s%s: %s", edge.from().c_str(), edge.is_directed() ? ">" : "-", edge.to().c_str(), ex.what_no_backtrace()); } }