示例#1
0
/** Get node closest to another node with a certain property.
 * @param node_name the name of the node from which to start
 * @param consider_unconnected consider unconnected node for the search
 * of the closest node
 * @param property property the node must have to be considered,
 * empty string to not check for any property
 * @return node closest to the given point in the global frame, or an
 * invalid node if such a node cannot be found. The node will obviously
 * not be the node with the name @p node_name.
 */
NavGraphNode
NavGraph::closest_node_to(const std::string &node_name, bool consider_unconnected,
			  const std::string &property) const
{
  NavGraphNode n = node(node_name);
  std::vector<NavGraphNode> nodes = search_nodes(property);

  float min_dist = HUGE;

  std::vector<NavGraphNode>::iterator i;
  std::vector<NavGraphNode>::iterator elem = nodes.begin();
  for (i = nodes.begin(); i != nodes.end(); ++i) {
    if (! consider_unconnected && i->unconnected())  continue;

    float dx   = i->x() - n.x();
    float dy   = i->y() - n.y();
    float dist = sqrtf(dx * dx + dy * dy);
    if ((sqrtf(dx * dx + dy * dy) < min_dist) && (i->name() != node_name)) {
      min_dist = dist;
      elem = i;
    }
  }

  if (elem == nodes.end()) {
    return NavGraphNode();
  } else {
    return *elem;
  }
}
示例#2
0
/** Get nodes reachable from specified nodes.
 * @param node_name name of the node to get reachable nodes for
 * @return vector of names of nodes reachable from the specified node
 */
std::vector<std::string>
NavGraph::reachable_nodes(const std::string &node_name) const
{
  std::vector<std::string> rv;

  NavGraphNode n = node(node_name);
  if (! n.is_valid())  return rv;

  std::vector<NavGraphEdge>::const_iterator i;
  for (i = edges_.begin(); i != edges_.end(); ++i) {
    if (i->is_directed()) {
      if (i->from() == node_name) {
        rv.push_back(i->to());
      }
    } else {
      if (i->from() == node_name) {
        rv.push_back(i->to());
      } else if (i->to() == node_name) {
        rv.push_back(i->from());
      }
    }
  }

  std::sort(rv.begin(), rv.end());
  std::unique(rv.begin(), rv.end());
  return rv;
}
示例#3
0
/** Set default properties on node for which no local value exists.
 * This sets all default properties on the node, for which no
 * property of the same name has been set on the node.
 * @param node node to apply default properties to
 */
void
NavGraph::apply_default_properties(NavGraphNode &node)
{
  for (const auto &p : default_properties_) {
    if (! node.has_property(p.first)) {
      node.set_property(p.first, p.second);
    }
  }
}
示例#4
0
void
NavGraphGeneratorThread::filter_multi_graph()
{
  navgraph->calc_reachability(/* allow multi graph*/ true);

  std::list<std::set<std::string>> graphs;

  const std::vector<NavGraphNode> &nodes = navgraph->nodes();
  std::set<std::string> nodeset;
  std::for_each(nodes.begin(), nodes.end(),
		[&nodeset](const NavGraphNode &n){ nodeset.insert(n.name()); });

  while (! nodeset.empty()) {
    std::queue<std::string> q;
    q.push(* nodeset.begin());

    std::set<std::string> traversed;

    while (! q.empty()) {
      std::string &nname = q.front();
      traversed.insert(nname);

      NavGraphNode n = navgraph->node(nname);
      if (n) {
	const std::vector<std::string> & reachable = n.reachable_nodes();

	for (const std::string &r : reachable) {
	  if (traversed.find(r) == traversed.end()) q.push(r);
	}
      }
      q.pop();
    }

    std::set<std::string> nodediff;
    std::set_difference(nodeset.begin(), nodeset.end(),
			traversed.begin(), traversed.end(),
			std::inserter(nodediff, nodediff.begin()));
    graphs.push_back(traversed);
    nodeset = nodediff;
  }

  // reverse sort, largest set first
  graphs.sort([](const std::set<std::string> &a, const std::set<std::string> &b)->bool{
      return b.size() < a.size();   
    });

  std::for_each(std::next(graphs.begin()), graphs.end(),
		[&](const std::set<std::string> &g) {
		  logger->log_debug(name(), "  Removing disconnected sub-graph [%s]",
				    str_join(g.begin(), g.end(), ", ").c_str());
		  for (const std::string &n : g)  navgraph->remove_node(n);
		});
}
示例#5
0
/** Remove a node.
 * @param node node to remove
 */
void
NavGraph::remove_node(const NavGraphNode &node)
{
  nodes_.erase(std::remove(nodes_.begin(), nodes_.end(), node));
  edges_.erase(
    std::remove_if(edges_.begin(), edges_.end(),
		   [&node](const NavGraphEdge &edge)->bool {
		     return edge.from() == node.name() || edge.to() == node.name();
		   }), edges_.end());
  reachability_calced_ = false;
  notify_of_change();
}
示例#6
0
//------------------------------------------------------------------------
// 노드의 위치별로 map에 저장한다.
// map key = 노드의 위치 (x/cellSize + (map width/cellsize) * (y/cellSize))
// map data = node
// [2011/1/17 jjuiddong]
//------------------------------------------------------------------------
void CEvosMap::InitNodeMap()
{
	m_NodeMap.clear();

	const int nodeCount = m_Graph.NumNodes();
	for (int i=0; i < nodeCount; ++i)
	{
		NavGraphNode<> node = m_Graph.GetNode(i);
		const int key = GetNodeMapKey(node.Pos());
		m_NodeMap.insert(NodeMap::value_type(key, node.Index()));
	}
}
示例#7
0
/** Set nodes.
 * @param from_node originating node
 * @param to_node target node
 */
void
NavGraphEdge::set_nodes(const NavGraphNode &from_node,
			      const NavGraphNode &to_node)
{
  if (from_node.name() != from_) {
    throw Exception("Conflicting originating node names: %s vs. %s",
		    from_node.name().c_str(), from_.c_str());
  }
  if (to_node.name() != to_) {
    throw Exception("Conflicting target node names: %s vs. %s",
		    to_node.name().c_str(), to_.c_str());
  }

  from_node_ = from_node;
  to_node_   = to_node;
}
示例#8
0
/** Update a given node.
 * Will search for a node with the same name as the given node and will then
 * call the assignment operator. This is intended to update properties of a node.
 * @param node node to update
 */
void
NavGraph::update_node(const NavGraphNode &node)
{
  std::vector<NavGraphNode>::iterator n =
    std::find(nodes_.begin(), nodes_.end(), node);
  if (n != nodes_.end()) {
    *n = node;
  } else {
    throw Exception("No node with name %s known", node.name().c_str());
  }
}
示例#9
0
/** Add a node.
 * @param node node to add
 * @throw Exception thrown if node with the same name as @p node already exists
 */
void
NavGraph::add_node(const NavGraphNode &node)
{
  if (node_exists(node)) {
    throw Exception("Node with name %s already exists", node.name().c_str());
  } else {
    nodes_.push_back(node);
    apply_default_properties(nodes_.back());
    reachability_calced_ = false;
    notify_of_change();
  }
}
示例#10
0
/** Connect node to closest node.
 * @param n node to connect to closest node
 */
void
NavGraph::connect_node_to_closest_node(const NavGraphNode &n)
{
  NavGraphNode closest = closest_node_to(n.name());
  NavGraphEdge new_edge(n.name(), closest.name());
  new_edge.set_property("created-for", n.name() + "--" + closest.name());
  new_edge.set_property("generated", true);
  add_edge(new_edge);
}
示例#11
0
//노드를 추가한다.
int SparseGraph::AddNode(NavGraphNode node)
{
    if (node.GetIndex() < (int)m_Nodes.size())
    {
        //같은 인덱스를 가진 노드가 이미 있는지 확인한다.
        assert(m_Nodes[node.GetIndex()].GetIndex() == -1 &&
               "<SparseGraph::AddNode>: Attempting to add a node with a duplicate ID");
        
        m_Nodes[node.GetIndex()] = node;
        
        return m_iNextNodeIndex;
    }
    
    else
    {
        //노드 리스트 맨 뒤에 추가하는지 확인한다.
        assert(node.GetIndex() == m_iNextNodeIndex && "<SparseGraph::AddNode>:invalid index");
        
        m_Nodes.push_back(node);
        m_Edges.push_back(EdgeList());
        
        return m_iNextNodeIndex++;
    }
}
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;
}
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();
}
示例#14
0
/** 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);
  }
}
示例#15
0
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());
  }
}