github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-02-10 17:14:56 -05:00
Code Issues Packages Projects Releases Wiki Activity

Using scipy.spatial.rotation matrix (#335)

Browse Source
This commit is contained in:
Atsushi Sakai
2020-06-07 20:28:29 +09:00
committed by GitHub
parent d1bde5835f
commit b8afdb10d8
26 changed files with 1002 additions and 931 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
PathPlanning/BidirectionalBreadthFirstSearch/bidirectional_breadth_first_search.py
View File

@@ -23,7 +23,7 @@ class BidirectionalBreadthFirstSearchPlanner:
ox: x position list of Obstacles [m]
oy: y position list of Obstacles [m]
reso: grid resolution [m]
resolution: grid resolution [m]
rr: robot radius[m]
"""
@@ -49,8 +49,8 @@ class BidirectionalBreadthFirstSearchPlanner:
Bidirectional Breadth First search based planning
input:
sx: start x position [m]
sy: start y position [m]
s_x: start x position [m]
s_y: start y position [m]
gx: goal x position [m]
gy: goal y position [m]
@@ -170,7 +170,7 @@ class BidirectionalBreadthFirstSearchPlanner:
# generate final course
rx, ry = [self.calc_grid_position(ngoal.x, self.minx)], [
self.calc_grid_position(ngoal.y, self.miny)]
n = closedset[ngoal.pind]
n = closedset[ngoal.parent_index]
while n is not None:
rx.append(self.calc_grid_position(n.x, self.minx))
ry.append(self.calc_grid_position(n.y, self.miny))
@@ -220,15 +220,15 @@ class BidirectionalBreadthFirstSearchPlanner:
self.miny = round(min(oy))
self.maxx = round(max(ox))
self.maxy = round(max(oy))
print("minx:", self.minx)
print("miny:", self.miny)
print("maxx:", self.maxx)
print("maxy:", self.maxy)
print("min_x:", self.minx)
print("min_y:", self.miny)
print("max_x:", self.maxx)
print("max_y:", self.maxy)
self.xwidth = round((self.maxx - self.minx) / self.reso)
self.ywidth = round((self.maxy - self.miny) / self.reso)
print("xwidth:", self.xwidth)
print("ywidth:", self.ywidth)
print("x_width:", self.xwidth)
print("y_width:", self.ywidth)
# obstacle map generation
self.obmap = [[False for _ in range(self.ywidth)]