Merge pull request AtsushiSakai#135 from AtsushiSakai/dev

code clean up for lgtm

Author: AtsushiSakai

AerialNavigation/drone_3d_trajectory_following/Quadrotor.py

@@ -7,7 +7,6 @@
 from math import cos, sin
 import numpy as np
 import matplotlib.pyplot as plt
-from mpl_toolkits.mplot3d import Axes3D
 
 
 class Quadrotor():

AerialNavigation/drone_3d_trajectory_following/drone_3d_trajectory_following.py

@@ -8,6 +8,7 @@
 import numpy as np
 from Quadrotor import Quadrotor
 from TrajectoryGenerator import TrajectoryGenerator
+from mpl_toolkits.mplot3d import Axes3D
 
 show_animation = True
 
@@ -69,8 +70,8 @@ def quad_sim(x_c, y_c, z_c):
 
     while True:
         while t <= T:
-            des_x_pos = calculate_position(x_c[i], t)
-            des_y_pos = calculate_position(y_c[i], t)
+            # des_x_pos = calculate_position(x_c[i], t)
+            # des_y_pos = calculate_position(y_c[i], t)
             des_z_pos = calculate_position(z_c[i], t)
             des_x_vel = calculate_velocity(x_c[i], t)
             des_y_vel = calculate_velocity(y_c[i], t)

ArmNavigation/__init__.py

@@ -4,7 +4,6 @@
 Author: Daniel Ingram (daniel-s-ingram)
         Atsushi Sakai (@Atsushi_twi)
 """
-import matplotlib.pyplot as plt
 import numpy as np
 
 from NLinkArm import NLinkArm
@@ -34,8 +33,8 @@ def main():
     state = WAIT_FOR_NEW_GOAL
     solution_found = False
     while True:
-        old_goal = goal_pos
-        goal_pos = arm.goal
+        old_goal = np.array(goal_pos)
+        goal_pos = np.array(arm.goal)
         end_effector = arm.end_effector
         errors, distance = distance_to_goal(end_effector, goal_pos)
 
@@ -51,7 +50,7 @@ def main():
                 elif solution_found:
                     state = MOVING_TO_GOAL
         elif state is MOVING_TO_GOAL:
-            if distance > 0.1 and (old_goal == goal_pos).all():
+            if distance > 0.1 and all(old_goal == goal_pos):
                 joint_angles = joint_angles + Kp * \
                     ang_diff(joint_goal_angles, joint_angles) * dt
             else:
@@ -93,8 +92,8 @@ def animation():
 
     i_goal = 0
     while True:
-        old_goal = goal_pos
-        goal_pos = arm.goal
+        old_goal = np.array(goal_pos)
+        goal_pos = np.array(arm.goal)
         end_effector = arm.end_effector
         errors, distance = distance_to_goal(end_effector, goal_pos)
 
@@ -111,7 +110,7 @@ def animation():
                 elif solution_found:
                     state = MOVING_TO_GOAL
         elif state is MOVING_TO_GOAL:
-            if distance > 0.1 and (old_goal == goal_pos).all():
+            if distance > 0.1 and all(old_goal == goal_pos):
                 joint_angles = joint_angles + Kp * \
                     ang_diff(joint_goal_angles, joint_angles) * dt
             else:

ArmNavigation/n_joint_arm_to_point_control/n_joint_arm_to_point_control.py

@@ -459,7 +459,6 @@ def expandVertex(self, vid):
         # add an edge to the edge queue is the path might improve the solution
         for neighbor in neigbors:
             sid = neighbor[0]
-            scoord = neighbor[1]
             estimated_f_score = self.computeDistanceCost(
                 self.startId, vid) + self.computeHeuristicCost(sid, self.goalId) + self.computeDistanceCost(vid, sid)
             if estimated_f_score < self.g_scores[self.goalId]:

PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py

@@ -269,7 +269,7 @@ def main():
         #  plt.pause(0.1)
         #  input()
 
-    flg, ax = plt.subplots(1)
+    plt.subplots(1)
     plt.plot(cx, cy, ".r", label="course")
     plt.plot(x, y, "-b", label="trajectory")
     plt.legend()
@@ -278,7 +278,7 @@ def main():
     plt.axis("equal")
     plt.grid(True)
 
-    flg, ax = plt.subplots(1)
+    subplots(1)
     plt.plot(t, [iv * 3.6 for iv in v], "-r")
     plt.xlabel("Time[s]")
     plt.ylabel("Speed[km/h]")
@@ -304,7 +304,7 @@ def main2():
     t, x, y, yaw, v, a, d, flag = closed_loop_prediction(
         cx, cy, cyaw, speed_profile, goal)
 
-    flg, ax = plt.subplots(1)
+    plt.subplots(1)
     plt.plot(cx, cy, ".r", label="course")
     plt.plot(x, y, "-b", label="trajectory")
     plt.plot(goal[0], goal[1], "xg", label="goal")
@@ -314,7 +314,7 @@ def main2():
     plt.axis("equal")
     plt.grid(True)
 
-    flg, ax = plt.subplots(1)
+    plt.subplots(1)
     plt.plot(t, [iv * 3.6 for iv in v], "-r")
     plt.xlabel("Time[s]")
     plt.ylabel("Speed[km/h]")

PathPlanning/ClosedLoopRRTStar/pure_pursuit.py

@@ -152,8 +152,9 @@ def __init__(self, start_pose, end_pose, eta=None, kappa=None):
             + (10. * eta[1] - 2. * eta[3] + 1. / 6 * eta[5]) * sb \
             - (2. * eta[1]**2 * kappa[2] - 1. / 6 * eta[1]**3 *
                kappa[3] - 1. / 2 * eta[1] * eta[3] * kappa[2]) * cb
-        
-        s_dot = lambda u : np.linalg.norm(self.coeffs[:, 1:].dot(np.array([1, 2.*u, 3.*u**2, 4.*u**3, 5.*u**4, 6.*u**5, 7.*u**6])))
+
+        def s_dot(u): return np.linalg.norm(self.coeffs[:, 1:].dot(
+            np.array([1, 2. * u, 3. * u**2, 4. * u**3, 5. * u**4, 6. * u**5, 7. * u**6])))
         self.segment_length = quad(lambda u: s_dot(u), 0, 1)[0]
 
     """
@@ -164,6 +165,7 @@ def __init__(self, start_pose, end_pose, eta=None, kappa=None):
     returns
         (x,y) of point along the segment
     """
+
     def calc_point(self, u):
         assert(u >= 0 and u <= 1)
         return self.coeffs.dot(np.array([1, u, u**2, u**3, u**4, u**5, u**6, u**7]))
@@ -187,8 +189,8 @@ def test1():
         # interpolate at several points along the path
         ui = np.linspace(0, len(path_segments), 1001)
         pos = np.empty((2, ui.size))
-        for i, u in enumerate(ui):
-            pos[:, i] = path.calc_path_point(u)
+        for j, u in enumerate(ui):
+            pos[:, j] = path.calc_path_point(u)
 
         if show_animation:
             # plot the path
@@ -217,8 +219,8 @@ def test2():
         # interpolate at several points along the path
         ui = np.linspace(0, len(path_segments), 1001)
         pos = np.empty((2, ui.size))
-        for i, u in enumerate(ui):
-            pos[:, i] = path.calc_path_point(u)
+        for j, u in enumerate(ui):
+            pos[:, j] = path.calc_path_point(u)
 
         if show_animation:
             # plot the path

PathPlanning/Eta3SplinePath/eta3_spline_path.py

@@ -13,8 +13,8 @@
 import numpy as np
 import scipy.spatial
 import matplotlib.pyplot as plt
-import reeds_shepp_path_planning as rs
-import heapq
+# import reeds_shepp_path_planning as rs
+# import heapq
 
 EXTEND_AREA = 5.0  # [m]
 H_COST = 1.0
@@ -118,40 +118,40 @@ def hybrid_a_star_planning(start, goal, ox, oy, xyreso, yawreso):
     yawreso: yaw angle resolution [rad]
     """
 
-    start[2], goal[2] = rs.pi_2_pi(start[2]), rs.pi_2_pi(goal[2])
-    tox, toy = ox[:], oy[:]
+    # start[2], goal[2] = rs.pi_2_pi(start[2]), rs.pi_2_pi(goal[2])
+    # tox, toy = ox[:], oy[:]
 
-    obkdtree = KDTree(np.vstack((tox, toy)).T)
+    # obkdtree = KDTree(np.vstack((tox, toy)).T)
 
-    c = Config(tox, toy, xyreso, yawreso)
+    # c = Config(tox, toy, xyreso, yawreso)
 
-    nstart = Node(int(start[0] / xyreso), int(start[1] / xyreso), int(start[2] / yawreso),
-                  True, [start[0]], [start[1]], [start[2]], [True], 0.0, 0.0, -1)
-    ngoal = Node(int(goal[0] / xyreso), int(goal[1] / xyreso), int(goal[2] / yawreso),
-                 True, [goal[0]], [goal[1]], [goal[2]], [True], 0.0, 0.0, -1)
+    # nstart = Node(int(start[0] / xyreso), int(start[1] / xyreso), int(start[2] / yawreso),
+    # True, [start[0]], [start[1]], [start[2]], [True], 0.0, 0.0, -1)
+    # ngoal = Node(int(goal[0] / xyreso), int(goal[1] / xyreso), int(goal[2] / yawreso),
+    # True, [goal[0]], [goal[1]], [goal[2]], [True], 0.0, 0.0, -1)
 
-    openList, closedList = {}, {}
-    h = []
-    #  goalqueue = queue.PriorityQueue()
-    pq = []
-    openList[calc_index(nstart, c)] = nstart
-    heapq.heappush(pq, (calc_index(nstart, c), calc_cost(nstart, h, ngoal, c)))
+    # openList, closedList = {}, {}
+    # h = []
+    # #  goalqueue = queue.PriorityQueue()
+    # pq = []
+    # openList[calc_index(nstart, c)] = nstart
+    # heapq.heappush(pq, (calc_index(nstart, c), calc_cost(nstart, h, ngoal, c)))
 
-    while True:
-        if not openList:
-            print("Error: Cannot find path, No open set")
-            return [], [], []
+    # while True:
+    # if not openList:
+    # print("Error: Cannot find path, No open set")
+    # return [], [], []
 
-        c_id, cost = heapq.heappop(pq)
-        current = openList.pop(c_id)
-        closedList[c_id] = current
+    # c_id, cost = heapq.heappop(pq)
+    # current = openList.pop(c_id)
+    # closedList[c_id] = current
 
-        isupdated, fpath = analytic_expantion(
-            current, ngoal, c, ox, oy, obkdtree)
+    # isupdated, fpath = analytic_expantion(
+    # current, ngoal, c, ox, oy, obkdtree)
 
-        #  print(current)
+    # #  print(current)
 
-    # rx, ry, ryaw = [], [], []
+    rx, ry, ryaw = [], [], []
 
     return rx, ry, ryaw
 
@@ -208,8 +208,8 @@ def main():
         start, goal, ox, oy, xyreso, yawreso)
 
     plt.plot(ox, oy, ".k")
-    rs.plot_arrow(start[0], start[1], start[2])
-    rs.plot_arrow(goal[0], goal[1], goal[2])
+    # rs.plot_arrow(start[0], start[1], start[2])
+    # rs.plot_arrow(goal[0], goal[1], goal[2])
 
     plt.grid(True)
     plt.axis("equal")

PathPlanning/HybridAStar/hybrid_a_star.py

@@ -160,6 +160,9 @@ 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/PotentialFieldPlanning/potential_field_planning.py

@@ -6,6 +6,7 @@
 
 """
 import math
+import matplotlib.pyplot as plt
 
 
 def mod2pi(theta):
@@ -260,7 +261,6 @@ def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
     u"""
     Plot arrow
     """
-    import matplotlib.pyplot as plt
 
     if not isinstance(x, float):
         for (ix, iy, iyaw) in zip(x, y, yaw):
@@ -273,7 +273,6 @@ def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
 
 if __name__ == '__main__':
     print("Dubins path planner sample start!!")
-    import matplotlib.pyplot as plt
 
     start_x = 1.0  # [m]
     start_y = 1.0  # [m]