github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-04-22 03:00:22 -04:00
Code Issues Packages Projects Releases Wiki Activity

add visibility road map path doc (#592)

Browse Source 
* add visibility road map path doc

* add visibility road map path doc

* add visibility road map path doc

* add visibility road map path doc

* add visibility road map path doc

* add visibility road map path doc

* add visibility road map path doc
This commit is contained in:
Atsushi Sakai
2021-12-04 20:56:54 +09:00
committed by GitHub
parent 4d60c3c0b1
commit c70add32a1
9 changed files with 87 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
PathPlanning/VisibilityRoadMap/geometry.py
View File

@@ -1,4 +1,5 @@
class Geometry:
class Point:
def __init__(self, x, y):
self.x = x
@@ -40,4 +41,4 @@ class Geometry:
if (o4 == 0) and on_segment(p2, q1, q2):
return True
return False
return False

21
PathPlanning/VisibilityRoadMap/visibility_road_map.py
View File

@@ -23,14 +23,14 @@ show_animation = True
class VisibilityRoadMap:
def __init__(self, robot_radius, do_plot=False):
self.robot_radius = robot_radius
def __init__(self, expand_distance, do_plot=False):
self.expand_distance = expand_distance
self.do_plot = do_plot
def planning(self, start_x, start_y, goal_x, goal_y, obstacles):
nodes = self.generate_graph_node(start_x, start_y, goal_x, goal_y,
obstacles)
nodes = self.generate_visibility_nodes(start_x, start_y,
goal_x, goal_y, obstacles)
road_map_info = self.generate_road_map_info(nodes, obstacles)
@@ -48,7 +48,8 @@ class VisibilityRoadMap:
return rx, ry
def generate_graph_node(self, start_x, start_y, goal_x, goal_y, obstacles):
def generate_visibility_nodes(self, start_x, start_y, goal_x, goal_y,
obstacles):
# add start and goal as nodes
nodes = [DijkstraSearch.Node(start_x, start_y),
@@ -129,8 +130,8 @@ class VisibilityRoadMap:
offset_vec = math.atan2(math.sin(p_vec) + math.sin(n_vec),
math.cos(p_vec) + math.cos(
n_vec)) + math.pi / 2.0
offset_x = x + self.robot_radius * math.cos(offset_vec)
offset_y = y + self.robot_radius * math.sin(offset_vec)
offset_x = x + self.expand_distance * math.cos(offset_vec)
offset_y = y + self.expand_distance * math.sin(offset_vec)
return offset_x, offset_y
@staticmethod
@@ -184,7 +185,7 @@ def main():
sx, sy = 10.0, 10.0 # [m]
gx, gy = 50.0, 50.0 # [m]
robot_radius = 5.0 # [m]
expand_distance = 5.0 # [m]
obstacles = [
ObstaclePolygon(
@@ -209,8 +210,8 @@ def main():
plt.axis("equal")
plt.pause(1.0)
rx, ry = VisibilityRoadMap(robot_radius, do_plot=show_animation).planning(
sx, sy, gx, gy, obstacles)
rx, ry = VisibilityRoadMap(expand_distance, do_plot=show_animation)\
.planning(sx, sy, gx, gy, obstacles)
if show_animation: # pragma: no cover
plt.plot(rx, ry, "-r")