code clean up

Mapping/circle_fitting/circle_fitting.py

@@ -75,13 +75,13 @@ def ray_casting_filter(xl, yl, thetal, rangel, angle_reso):
     rangedb = [float("inf") for _ in range(
         int(math.floor((math.pi * 2.0) / angle_reso)) + 1)]
 
-    for i in range(len(thetal)):
+    for i, _ in enumerate(thetal):
         angleid = math.floor(thetal[i] / angle_reso)
 
         if rangedb[angleid] > rangel[i]:
             rangedb[angleid] = rangel[i]
 
-    for i in range(len(rangedb)):
+    for i, _ in enumerate(rangedb):
         t = i * angle_reso
         if rangedb[i] != float("inf"):
             rx.append(rangedb[i] * math.cos(t))

Mapping/rectangle_fitting/rectangle_fitting.py

@@ -1,138 +0,0 @@
-"""
-
-Object shape recognition with rectangle fitting
-
-author: Atsushi Sakai (@Atsushi_twi)
-
-"""
-
-import matplotlib.pyplot as plt
-import math
-import random
-import numpy as np
-
-show_animation = True
-
-
-def circle_fitting(x, y):
-    """
-    Circle Fitting with least squared
-        input: point x-y positions
-        output  cxe x center position
-                cye y center position
-                re  radius of circle
-                error: prediction error
-    """
-
-    sumx = sum(x)
-    sumy = sum(y)
-    sumx2 = sum([ix ** 2 for ix in x])
-    sumy2 = sum([iy ** 2 for iy in y])
-    sumxy = sum([ix * iy for (ix, iy) in zip(x, y)])
-
-    F = np.array([[sumx2, sumxy, sumx],
-                  [sumxy, sumy2, sumy],
-                  [sumx, sumy, len(x)]])
-
-    G = np.array([[-sum([ix ** 3 + ix * iy ** 2 for (ix, iy) in zip(x, y)])],
-                  [-sum([ix ** 2 * iy + iy ** 3 for (ix, iy) in zip(x, y)])],
-                  [-sum([ix ** 2 + iy ** 2 for (ix, iy) in zip(x, y)])]])
-
-    T = np.linalg.inv(F).dot(G)
-
-    cxe = float(T[0] / -2)
-    cye = float(T[1] / -2)
-    re = math.sqrt(cxe**2 + cye**2 - T[2])
-
-    error = sum([np.hypot(cxe - ix, cye - iy) - re for (ix, iy) in zip(x, y)])
-
-    return (cxe, cye, re, error)
-
-
-def get_sample_points(cx, cy, cr, angle_reso):
-    x, y, angle, r = [], [], [], []
-
-    # points sampling
-    for theta in np.arange(0.0, 2.0 * math.pi, angle_reso):
-        nx = cx + cr * math.cos(theta)
-        ny = cy + cr * math.sin(theta)
-        nangle = math.atan2(ny, nx)
-        nr = math.hypot(nx, ny) * random.uniform(0.95, 1.05)
-
-        x.append(nx)
-        y.append(ny)
-        angle.append(nangle)
-        r.append(nr)
-
-    # ray casting filter
-    rx, ry = ray_casting_filter(x, y, angle, r, angle_reso)
-
-    return rx, ry
-
-
-def ray_casting_filter(xl, yl, thetal, rangel, angle_reso):
-    rx, ry = [], []
-    rangedb = [float("inf") for _ in range(
-        int(math.floor((math.pi * 2.0) / angle_reso)) + 1)]
-
-    for i in range(len(thetal)):
-        angleid = math.floor(thetal[i] / angle_reso)
-
-        if rangedb[angleid] > rangel[i]:
-            rangedb[angleid] = rangel[i]
-
-    for i in range(len(rangedb)):
-        t = i * angle_reso
-        if rangedb[i] != float("inf"):
-            rx.append(rangedb[i] * math.cos(t))
-            ry.append(rangedb[i] * math.sin(t))
-
-    return rx, ry
-
-
-def plot_circle(x, y, size, color="-b"):
-    deg = list(range(0, 360, 5))
-    deg.append(0)
-    xl = [x + size * math.cos(np.deg2rad(d)) for d in deg]
-    yl = [y + size * math.sin(np.deg2rad(d)) for d in deg]
-    plt.plot(xl, yl, color)
-
-
-def main():
-
-    # simulation parameters
-    simtime = 15.0  # simulation time
-    dt = 1.0  # time tick
-
-    cx = -2.0  # initial x position of obstacle
-    cy = -8.0  # initial y position of obstacle
-    cr = 1.0  # obstacle radious
-    theta = np.deg2rad(30.0)  # obstacle moving direction
-    angle_reso = np.deg2rad(3.0)  # sensor angle resolution
-
-    time = 0.0
-    while time <= simtime:
-        time += dt
-
-        cx += math.cos(theta)
-        cy += math.cos(theta)
-
-        x, y = get_sample_points(cx, cy, cr, angle_reso)
-
-        ex, ey, er, error = circle_fitting(x, y)
-        print("Error:", error)
-
-        if show_animation:
-            plt.cla()
-            plt.axis("equal")
-            plt.plot(0.0, 0.0, "*r")
-            plot_circle(cx, cy, cr)
-            plt.plot(x, y, "xr")
-            plot_circle(ex, ey, er, "-r")
-            plt.pause(dt)
-
-    print("Done")
-
-
-if __name__ == '__main__':
-    main()

PathPlanning/Eta3SplineTrajectory/eta3_spline_trajectory.py

@@ -190,9 +190,11 @@ class eta3_trajectory(eta3_path):
         self.total_time = self.times.sum()
 
     def get_interp_param(self, seg_id, s, ui, tol=0.001):
-        def f(u): return self.segments[seg_id].f_length(u)[0] - s
+        def f(u):
+            return self.segments[seg_id].f_length(u)[0] - s
 
-        def fprime(u): return self.segments[seg_id].s_dot(u)
+        def fprime(u):
+            return self.segments[seg_id].s_dot(u)
         while (ui >= 0 and ui <= 1) and abs(f(ui)) > tol:
             ui -= f(ui) / fprime(ui)
         ui = max(0, min(ui, 1))

PathPlanning/LQRRRTStar/lqr_rrt_star.py

@@ -6,15 +6,19 @@ author: AtsushiSakai(@Atsushi_twi)
 
 """
 
+import matplotlib.pyplot as plt
+import numpy as np
+import copy
+import math
+import random
 import sys
 sys.path.append("../LQRPlanner/")
 
-import random
-import math
-import copy
-import numpy as np
-import matplotlib.pyplot as plt
-import LQRplanner
+try:
+    import LQRplanner
+except:
+    raise
+
 
 show_animation = True
 
@@ -83,7 +87,7 @@ class RRT():
         return path
 
     def choose_parent(self, newNode, nearinds):
-        if len(nearinds) == 0:
+        if not nearinds:
             return newNode
 
         dlist = []
@@ -109,7 +113,7 @@ class RRT():
         return newNode
 
     def pi_2_pi(self, angle):
-        return (angle + math.pi) % (2*math.pi) - math.pi
+        return (angle + math.pi) % (2 * math.pi) - math.pi
 
     def sample_path(self, wx, wy, step):
 
@@ -173,7 +177,7 @@ class RRT():
             if self.calc_dist_to_goal(node.x, node.y) <= XYTH:
                 goalinds.append(i)
 
-        if len(goalinds) == 0:
+        if not goalinds:
             return None
 
         mincost = min([self.nodeList[i].cost for i in goalinds])

PathPlanning/PotentialFieldPlanning/potential_field_planning.py

@@ -52,7 +52,7 @@ def calc_repulsive_potential(x, y, ox, oy, rr):
     # search nearest obstacle
     minid = -1
     dmin = float("inf")
-    for i in range(len(ox)):
+    for i, _ in enumerate(ox):
         d = np.hypot(x - ox[i], y - oy[i])
         if dmin >= d:
             dmin = d
@@ -106,7 +106,7 @@ def potential_field_planning(sx, sy, gx, gy, ox, oy, reso, rr):
     while d >= reso:
         minp = float("inf")
         minix, miniy = -1, -1
-        for i in range(len(motion)):
+        for i, _ in enumerate(motion):
             inx = int(ix + motion[i][0])
             iny = int(iy + motion[i][1])
             if inx >= len(pmap) or iny >= len(pmap[0]):
@@ -160,9 +160,6 @@ def main():
     rx, ry = potential_field_planning(
         sx, sy, gx, gy, ox, oy, grid_size, robot_radius)
 
-    print(rx)
-    print(ry)
-
     if show_animation:
         plt.show()
 

PathPlanning/RRTStarDubins/rrt_star_dubins.py

@@ -74,7 +74,7 @@ class RRT():
         return path
 
     def choose_parent(self, newNode, nearinds):
-        if len(nearinds) == 0:
+        if not nearinds:
             return newNode
 
         dlist = []
@@ -152,7 +152,7 @@ class RRT():
             if abs(self.nodeList[i].yaw - self.end.yaw) <= YAWTH:
                 fgoalinds.append(i)
 
-        if len(fgoalinds) == 0:
+        if not fgoalinds:
             return None
 
         mincost = min([self.nodeList[i].cost for i in fgoalinds])

appveyor.yml

@@ -13,7 +13,7 @@ environment:
 init:
   - "ECHO %MINICONDA% %PYTHON_VERSION% %PYTHON_ARCH%"
 
-  
+ 
 install:
   # If there is a newer build queued for the same PR, cancel this one.
   # The AppVeyor 'rollout builds' option is supposed to serve the same
@@ -39,7 +39,7 @@ install:
   - conda info -a
   - conda env create -f C:\\projects\pythonrobotics\environment.yml
   - activate python_robotics
-  
+
   # Check that we have the expected version and architecture for Python
   - "python --version"
   - "python -c \"import struct; print(struct.calcsize('P') * 8)\""

docs/jupyternotebook2rst.py

@@ -1,7 +1,3 @@
-import subprocess
-import os.path
-import os
-import glob
 """
 
 Jupyter notebook converter to rst file
@@ -9,6 +5,11 @@ Jupyter notebook converter to rst file
 author: Atsushi Sakai
 
 """
+import subprocess
+import os.path
+import os
+import glob
+
 
 NOTEBOOK_DIR = "../"
 
@@ -19,8 +20,9 @@ def get_notebook_path_list(ndir):
 
 
 def convert_rst(rstpath):
-    with open(rstpath, "r") as file:
-        filedata = file.read()
+
+    with open(rstpath, "r") as bfile:
+        filedata = bfile.read()
 
         # convert from code directive to code-block
         # because showing code in Sphinx
@@ -28,8 +30,8 @@ def convert_rst(rstpath):
         after = ".. code-block:: ipython3"
         filedata = filedata.replace(before, after)
 
-    with open(rstpath, "w") as ffile:
-        ffile.write(filedata)
+    with open(rstpath, "w") as afile:
+        afile.write(filedata)
 
 
 def generate_rst(npath):