/** 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());
}
}
