void MotionPlanner::shortest_path(const Point &from, const Point &to, Polyline* polyline) { if (!this->initialized) this->initialize(); if (this->islands.empty()) { polyline->points.push_back(from); polyline->points.push_back(to); return; } // Are both points in the same island? int island_idx = -1; for (ExPolygons::const_iterator island = this->islands.begin(); island != this->islands.end(); ++island) { if (island->contains(from) && island->contains(to)) { // since both points are in the same island, is a direct move possible? // if so, we avoid generating the visibility environment if (island->contains(Line(from, to))) { polyline->points.push_back(from); polyline->points.push_back(to); return; } island_idx = island - this->islands.begin(); break; } } // Now check whether points are inside the environment. Point inner_from = from; Point inner_to = to; bool from_is_inside, to_is_inside; if (island_idx == -1) { if (!(from_is_inside = this->outer.contains(from))) { // Find the closest inner point to start from. from.nearest_point(this->outer, &inner_from); } if (!(to_is_inside = this->outer.contains(to))) { // Find the closest inner point to start from. to.nearest_point(this->outer, &inner_to); } } else { if (!(from_is_inside = this->inner[island_idx].contains(from))) { // Find the closest inner point to start from. from.nearest_point(this->inner[island_idx], &inner_from); } if (!(to_is_inside = this->inner[island_idx].contains(to))) { // Find the closest inner point to start from. to.nearest_point(this->inner[island_idx], &inner_to); } } // perform actual path search MotionPlannerGraph* graph = this->init_graph(island_idx); graph->shortest_path(graph->find_node(inner_from), graph->find_node(inner_to), polyline); polyline->points.insert(polyline->points.begin(), from); polyline->points.push_back(to); }
MotionPlannerGraph* MotionPlanner::init_graph(int island_idx) { if (this->graphs[island_idx + 1] == NULL) { Polygons pp; if (island_idx == -1) { pp = this->outer; } else { pp = this->inner[island_idx]; } MotionPlannerGraph* graph = this->graphs[island_idx + 1] = new MotionPlannerGraph(); // add polygon boundaries as edges size_t node_idx = 0; Lines lines; for (Polygons::const_iterator polygon = pp.begin(); polygon != pp.end(); ++polygon) { graph->nodes.push_back(polygon->points.back()); node_idx++; for (Points::const_iterator p = polygon->points.begin(); p != polygon->points.end(); ++p) { graph->nodes.push_back(*p); double dist = graph->nodes[node_idx-1].distance_to(*p); graph->add_edge(node_idx-1, node_idx, dist); graph->add_edge(node_idx, node_idx-1, dist); node_idx++; } polygon->lines(&lines); } // add Voronoi edges as internal edges { typedef voronoi_diagram<double> VD; typedef std::map<const VD::vertex_type*,size_t> t_vd_vertices; VD vd; t_vd_vertices vd_vertices; boost::polygon::construct_voronoi(lines.begin(), lines.end(), &vd); for (VD::const_edge_iterator edge = vd.edges().begin(); edge != vd.edges().end(); ++edge) { if (edge->is_infinite()) continue; const VD::vertex_type* v0 = edge->vertex0(); const VD::vertex_type* v1 = edge->vertex1(); Point p0 = Point(v0->x(), v0->y()); Point p1 = Point(v1->x(), v1->y()); // contains() should probably be faster than contains(), // and should it fail on any boundary points it's not a big problem if (island_idx == -1) { if (!this->outer.contains(p0) || !this->outer.contains(p1)) continue; } else { if (!this->inner[island_idx].contains(p0) || !this->inner[island_idx].contains(p1)) continue; } t_vd_vertices::const_iterator i_v0 = vd_vertices.find(v0); size_t v0_idx; if (i_v0 == vd_vertices.end()) { graph->nodes.push_back(p0); v0_idx = node_idx; vd_vertices[v0] = node_idx; node_idx++; } else { v0_idx = i_v0->second; } t_vd_vertices::const_iterator i_v1 = vd_vertices.find(v1); size_t v1_idx; if (i_v1 == vd_vertices.end()) { graph->nodes.push_back(p1); v1_idx = node_idx; vd_vertices[v1] = node_idx; node_idx++; } else { v1_idx = i_v1->second; } double dist = graph->nodes[v0_idx].distance_to(graph->nodes[v1_idx]); graph->add_edge(v0_idx, v1_idx, dist); } } return graph; } return this->graphs[island_idx + 1]; }
MotionPlannerGraph* MotionPlanner::init_graph(int island_idx) { if (this->graphs[island_idx + 1] == NULL) { // if this graph doesn't exist, initialize it MotionPlannerGraph* graph = this->graphs[island_idx + 1] = new MotionPlannerGraph(); /* We don't add polygon boundaries as graph edges, because we'd need to connect them to the Voronoi-generated edges by recognizing coinciding nodes. */ typedef voronoi_diagram<double> VD; VD vd; // mapping between Voronoi vertices and graph nodes typedef std::map<const VD::vertex_type*,size_t> t_vd_vertices; t_vd_vertices vd_vertices; // get boundaries as lines ExPolygonCollection env = this->get_env(island_idx); Lines lines = env.lines(); boost::polygon::construct_voronoi(lines.begin(), lines.end(), &vd); // traverse the Voronoi diagram and generate graph nodes and edges for (VD::const_edge_iterator edge = vd.edges().begin(); edge != vd.edges().end(); ++edge) { if (edge->is_infinite()) continue; const VD::vertex_type* v0 = edge->vertex0(); const VD::vertex_type* v1 = edge->vertex1(); Point p0 = Point(v0->x(), v0->y()); Point p1 = Point(v1->x(), v1->y()); // skip edge if any of its endpoints is outside our configuration space if (!env.contains_b(p0) || !env.contains_b(p1)) continue; t_vd_vertices::const_iterator i_v0 = vd_vertices.find(v0); size_t v0_idx; if (i_v0 == vd_vertices.end()) { graph->nodes.push_back(p0); vd_vertices[v0] = v0_idx = graph->nodes.size()-1; } else { v0_idx = i_v0->second; } t_vd_vertices::const_iterator i_v1 = vd_vertices.find(v1); size_t v1_idx; if (i_v1 == vd_vertices.end()) { graph->nodes.push_back(p1); vd_vertices[v1] = v1_idx = graph->nodes.size()-1; } else { v1_idx = i_v1->second; } // Euclidean distance is used as weight for the graph edge double dist = graph->nodes[v0_idx].distance_to(graph->nodes[v1_idx]); graph->add_edge(v0_idx, v1_idx, dist); } return graph; } return this->graphs[island_idx + 1]; }
Polyline MotionPlanner::shortest_path(const Point &from, const Point &to) { // lazy generation of configuration space if (!this->initialized) this->initialize(); // if we have an empty configuration space, return a straight move if (this->islands.empty()) { Polyline p; p.points.push_back(from); p.points.push_back(to); return p; } // Are both points in the same island? int island_idx = -1; for (ExPolygons::const_iterator island = this->islands.begin(); island != this->islands.end(); ++island) { if (island->contains(from) && island->contains(to)) { // since both points are in the same island, is a direct move possible? // if so, we avoid generating the visibility environment if (island->contains(Line(from, to))) { Polyline p; p.points.push_back(from); p.points.push_back(to); return p; } island_idx = island - this->islands.begin(); break; } } // get environment ExPolygonCollection env = this->get_env(island_idx); if (env.expolygons.empty()) { // if this environment is empty (probably because it's too small), perform straight move // and avoid running the algorithms on empty dataset Polyline p; p.points.push_back(from); p.points.push_back(to); return p; // bye bye } // Now check whether points are inside the environment. Point inner_from = from; Point inner_to = to; if (!env.contains(from)) { // Find the closest inner point to start from. inner_from = this->nearest_env_point(env, from, to); } if (!env.contains(to)) { // Find the closest inner point to start from. inner_to = this->nearest_env_point(env, to, inner_from); } // perform actual path search MotionPlannerGraph* graph = this->init_graph(island_idx); Polyline polyline = graph->shortest_path(graph->find_node(inner_from), graph->find_node(inner_to)); polyline.points.insert(polyline.points.begin(), from); polyline.points.push_back(to); { // grow our environment slightly in order for simplify_by_visibility() // to work best by considering moves on boundaries valid as well ExPolygonCollection grown_env; offset(env, &grown_env.expolygons, +SCALED_EPSILON); // remove unnecessary vertices polyline.simplify_by_visibility(grown_env); } /* SVG svg("shortest_path.svg"); svg.draw(this->outer); svg.arrows = false; for (MotionPlannerGraph::adjacency_list_t::const_iterator it = graph->adjacency_list.begin(); it != graph->adjacency_list.end(); ++it) { Point a = graph->nodes[it - graph->adjacency_list.begin()]; for (std::vector<MotionPlannerGraph::neighbor>::const_iterator n = it->begin(); n != it->end(); ++n) { Point b = graph->nodes[n->target]; svg.draw(Line(a, b)); } } svg.arrows = true; svg.draw(from); svg.draw(inner_from, "red"); svg.draw(to); svg.draw(inner_to, "red"); svg.draw(*polyline, "red"); svg.Close(); */ return polyline; }
Polyline MotionPlanner::shortest_path(const Point &from, const Point &to) { // if we have an empty configuration space, return a straight move if (this->islands.empty()) return Line(from, to); // Are both points in the same island? int island_idx = -1; for (std::vector<MotionPlannerEnv>::const_iterator island = this->islands.begin(); island != this->islands.end(); ++island) { if (island->island.contains(from) && island->island.contains(to)) { // since both points are in the same island, is a direct move possible? // if so, we avoid generating the visibility environment if (island->island.contains(Line(from, to))) return Line(from, to); island_idx = island - this->islands.begin(); break; } } // lazy generation of configuration space this->initialize(); // get environment MotionPlannerEnv env = this->get_env(island_idx); if (env.env.expolygons.empty()) { // if this environment is empty (probably because it's too small), perform straight move // and avoid running the algorithms on empty dataset return Line(from, to); } // Now check whether points are inside the environment. Point inner_from = from; Point inner_to = to; if (island_idx == -1) { // TODO: instead of using the nearest_env_point() logic, we should // create a temporary graph where we connect 'from' and 'to' to the // nodes which don't require more than one crossing, and let Dijkstra // figure out the entire path - this should also replace the call to // find_node() below if (!env.island.contains(inner_from)) { // Find the closest inner point to start from. inner_from = env.nearest_env_point(from, to); } if (!env.island.contains(inner_to)) { // Find the closest inner point to start from. inner_to = env.nearest_env_point(to, inner_from); } } // perform actual path search MotionPlannerGraph* graph = this->init_graph(island_idx); Polyline polyline = graph->shortest_path(graph->find_node(inner_from), graph->find_node(inner_to)); polyline.points.insert(polyline.points.begin(), from); polyline.points.push_back(to); { // grow our environment slightly in order for simplify_by_visibility() // to work best by considering moves on boundaries valid as well ExPolygonCollection grown_env(offset_ex((Polygons)env.env, +SCALED_EPSILON)); if (island_idx == -1) { /* If 'from' or 'to' are not inside our env, they were connected using the nearest_env_point() search which maybe produce ugly paths since it does not include the endpoint in the Dijkstra search; the simplify_by_visibility() call below will not work in many cases where the endpoint is not contained in grown_env (whose contour was arbitrarily constructed with MP_OUTER_MARGIN, which may not be enough for, say, including a skirt point). So we prune the extra points manually. */ if (!grown_env.contains(from)) { // delete second point while the line connecting first to third crosses the // boundaries as many times as the current first to second while (polyline.points.size() > 2 && intersection((Lines)Line(from, polyline.points[2]), grown_env).size() == 1) { polyline.points.erase(polyline.points.begin() + 1); } } if (!grown_env.contains(to)) { while (polyline.points.size() > 2 && intersection((Lines)Line(*(polyline.points.end() - 3), to), grown_env).size() == 1) { polyline.points.erase(polyline.points.end() - 2); } } } // remove unnecessary vertices // Note: this is computationally intensive and does not look very necessary // now that we prune the endpoints with the logic above, // so we comment it for now until a good test case arises //polyline.simplify_by_visibility(grown_env); /* SVG svg("shortest_path.svg"); svg.draw(grown_env.expolygons); svg.arrows = false; for (MotionPlannerGraph::adjacency_list_t::const_iterator it = graph->adjacency_list.begin(); it != graph->adjacency_list.end(); ++it) { Point a = graph->nodes[it - graph->adjacency_list.begin()]; for (std::vector<MotionPlannerGraph::neighbor>::const_iterator n = it->begin(); n != it->end(); ++n) { Point b = graph->nodes[n->target]; svg.draw(Line(a, b)); } } svg.arrows = true; svg.draw(from); svg.draw(inner_from, "red"); svg.draw(to); svg.draw(inner_to, "red"); svg.draw(polyline, "red"); svg.Close(); */ } return polyline; }