From 3a7f3c5587f055a0ad50e88d88d87a4878a6128f Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Fri, 23 Nov 2018 11:03:50 +0900 Subject: [PATCH] code clean up for lgtm --- .../arm_obstacle_navigation.py | 2 +- .../n_joint_arm_to_point_control/NLinkArm.py | 2 +- .../n_joint_arm_to_point_control.py | 4 ++-- .../batch_informed_rrtstar.py | 2 +- .../closed_loop_rrt_star_car.py | 9 ++++---- .../ClosedLoopRRTStar/pure_pursuit.py | 6 ++--- .../ClosedLoopRRTStar/unicycle_model.py | 5 +++-- .../CubicSpline/cubic_spline_planner.py | 6 ++--- .../DubinsPath/dubins_path_planning.py | 22 +++++++++---------- .../cubic_spline_planner.py | 8 +++---- PathPlanning/HybridAStar/hybrid_a_star.py | 2 +- .../RRTDubins/dubins_path_planning.py | 22 +++++++++---------- .../RRTStarDubins/dubins_path_planning.py | 22 +++++++++---------- .../reeds_shepp_path_planning.py | 1 - .../lqr_speed_steer_control.py | 7 +++--- .../lqr_steer_control/lqr_steer_control.py | 11 +++------- .../rear_wheel_feedback.py | 10 +++------ .../iterative_closest_point.py | 8 ++++--- 18 files changed, 69 insertions(+), 80 deletions(-) diff --git a/ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation.py b/ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation.py index b1f9bc29..2210ab77 100644 --- a/ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation.py +++ b/ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation.py @@ -210,7 +210,7 @@ class NLinkArm(object): def __init__(self, link_lengths, joint_angles): self.n_links = len(link_lengths) - if self.n_links is not len(joint_angles): + if self.n_links != len(joint_angles): raise ValueError() self.link_lengths = np.array(link_lengths) diff --git a/ArmNavigation/n_joint_arm_to_point_control/NLinkArm.py b/ArmNavigation/n_joint_arm_to_point_control/NLinkArm.py index 71cab16c..36a7d429 100644 --- a/ArmNavigation/n_joint_arm_to_point_control/NLinkArm.py +++ b/ArmNavigation/n_joint_arm_to_point_control/NLinkArm.py @@ -12,7 +12,7 @@ class NLinkArm(object): def __init__(self, link_lengths, joint_angles, goal, show_animation): self.show_animation = show_animation self.n_links = len(link_lengths) - if self.n_links is not len(joint_angles): + if self.n_links != len(joint_angles): raise ValueError() self.link_lengths = np.array(link_lengths) diff --git a/ArmNavigation/n_joint_arm_to_point_control/n_joint_arm_to_point_control.py b/ArmNavigation/n_joint_arm_to_point_control/n_joint_arm_to_point_control.py index 33fdf8c8..27e9f9e4 100644 --- a/ArmNavigation/n_joint_arm_to_point_control/n_joint_arm_to_point_control.py +++ b/ArmNavigation/n_joint_arm_to_point_control/n_joint_arm_to_point_control.py @@ -51,7 +51,7 @@ def main(): elif solution_found: state = MOVING_TO_GOAL elif state is MOVING_TO_GOAL: - if distance > 0.1 and (old_goal is goal_pos): + if distance > 0.1 and (old_goal == goal_pos).all(): joint_angles = joint_angles + Kp * \ ang_diff(joint_goal_angles, joint_angles) * dt else: @@ -111,7 +111,7 @@ def animation(): elif solution_found: state = MOVING_TO_GOAL elif state is MOVING_TO_GOAL: - if distance > 0.1 and (old_goal is goal_pos): + if distance > 0.1 and (old_goal == goal_pos).all(): joint_angles = joint_angles + Kp * \ ang_diff(joint_goal_angles, joint_angles) * dt else: diff --git a/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py b/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py index 05bd254b..66bcffbf 100644 --- a/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py +++ b/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py @@ -474,7 +474,7 @@ class BITStar(object): for v, edges in self.tree.vertices.items(): if v != vid and (v, vid) not in self.edge_queue and (vid, v) not in self.edge_queue: vcoord = self.tree.nodeIdToRealWorldCoord(v) - if(np.linalg.norm(vcoord - currCoord, 2) <= self.r and v != vid): + if(np.linalg.norm(vcoord - currCoord, 2) <= self.r): neigbors.append((vid, vcoord)) for neighbor in neigbors: diff --git a/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py b/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py index 8199c2d9..b3c27825 100644 --- a/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py +++ b/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py @@ -427,7 +427,6 @@ def main(): if not flag: print("cannot find feasible path") - # flg, ax = plt.subplots(1) # Draw final path if show_animation: rrt.DrawGraph() @@ -435,26 +434,26 @@ def main(): plt.grid(True) plt.pause(0.001) - flg, ax = plt.subplots(1) + plt.subplots(1) plt.plot(t, [np.rad2deg(iyaw) for iyaw in yaw[:-1]], '-r') plt.xlabel("time[s]") plt.ylabel("Yaw[deg]") 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("velocity[km/h]") plt.grid(True) - flg, ax = plt.subplots(1) + plt.subplots(1) plt.plot(t, a, '-r') plt.xlabel("time[s]") plt.ylabel("accel[m/ss]") plt.grid(True) - flg, ax = plt.subplots(1) + plt.subplots(1) plt.plot(t, [np.rad2deg(td) for td in d], '-r') plt.xlabel("time[s]") plt.ylabel("Steering angle[deg]") diff --git a/PathPlanning/ClosedLoopRRTStar/pure_pursuit.py b/PathPlanning/ClosedLoopRRTStar/pure_pursuit.py index 7ad06326..abe73d58 100644 --- a/PathPlanning/ClosedLoopRRTStar/pure_pursuit.py +++ b/PathPlanning/ClosedLoopRRTStar/pure_pursuit.py @@ -1,6 +1,4 @@ -#! /usr/bin/python -# -*- coding: utf-8 -*- -u""" +""" Path tracking simulation with pure pursuit steering control and PID speed control. @@ -251,7 +249,7 @@ def main(): while T >= time and lastIndex > target_ind: ai = PIDControl(target_speed, state.v) - di, target_ind = pure_pursuit_control(state, cx, cy, target_ind) + di, target_ind, _ = pure_pursuit_control(state, cx, cy, target_ind) state = unicycle_model.update(state, ai, di) time = time + unicycle_model.dt diff --git a/PathPlanning/ClosedLoopRRTStar/unicycle_model.py b/PathPlanning/ClosedLoopRRTStar/unicycle_model.py index 9ea0f017..2b7fbe72 100644 --- a/PathPlanning/ClosedLoopRRTStar/unicycle_model.py +++ b/PathPlanning/ClosedLoopRRTStar/unicycle_model.py @@ -7,6 +7,7 @@ author Atsushi Sakai """ import math +import numpy as np dt = 0.05 # [s] L = 0.9 # [m] @@ -66,12 +67,12 @@ if __name__ == '__main__': yaw.append(state.yaw) v.append(state.v) - flg, ax = plt.subplots(1) + plt.subplots(1) plt.plot(x, y) plt.axis("equal") plt.grid(True) - flg, ax = plt.subplots(1) + plt.subplots(1) plt.plot(v) plt.grid(True) diff --git a/PathPlanning/CubicSpline/cubic_spline_planner.py b/PathPlanning/CubicSpline/cubic_spline_planner.py index cffe1c3b..071b1171 100644 --- a/PathPlanning/CubicSpline/cubic_spline_planner.py +++ b/PathPlanning/CubicSpline/cubic_spline_planner.py @@ -209,7 +209,7 @@ def main(): ryaw.append(sp.calc_yaw(i_s)) rk.append(sp.calc_curvature(i_s)) - flg, ax = plt.subplots(1) + plt.subplots(1) plt.plot(x, y, "xb", label="input") plt.plot(rx, ry, "-r", label="spline") plt.grid(True) @@ -218,14 +218,14 @@ def main(): plt.ylabel("y[m]") plt.legend() - flg, ax = plt.subplots(1) + plt.subplots(1) plt.plot(s, [np.rad2deg(iyaw) for iyaw in ryaw], "-r", label="yaw") plt.grid(True) plt.legend() plt.xlabel("line length[m]") plt.ylabel("yaw angle[deg]") - flg, ax = plt.subplots(1) + plt.subplots(1) plt.plot(s, rk, "-r", label="curvature") plt.grid(True) plt.legend() diff --git a/PathPlanning/DubinsPath/dubins_path_planning.py b/PathPlanning/DubinsPath/dubins_path_planning.py index 6063290e..b5a8bcbd 100644 --- a/PathPlanning/DubinsPath/dubins_path_planning.py +++ b/PathPlanning/DubinsPath/dubins_path_planning.py @@ -235,18 +235,18 @@ def generate_course(length, mode, c): elif m is "R": # right turn pyaw.append(pyaw[-1] - d) pd += d - else: - d = l - pd - px.append(px[-1] + d / c * math.cos(pyaw[-1])) - py.append(py[-1] + d / c * math.sin(pyaw[-1])) - if m is "L": # left turn - pyaw.append(pyaw[-1] + d) - elif m is "S": # Straight - pyaw.append(pyaw[-1]) - elif m is "R": # right turn - pyaw.append(pyaw[-1] - d) - pd += d + d = l - pd + px.append(px[-1] + d / c * math.cos(pyaw[-1])) + py.append(py[-1] + d / c * math.sin(pyaw[-1])) + + if m is "L": # left turn + pyaw.append(pyaw[-1] + d) + elif m is "S": # Straight + pyaw.append(pyaw[-1]) + elif m is "R": # right turn + pyaw.append(pyaw[-1] - d) + pd += d return px, py, pyaw diff --git a/PathPlanning/FrenetOptimalTrajectory/cubic_spline_planner.py b/PathPlanning/FrenetOptimalTrajectory/cubic_spline_planner.py index 490098ff..81c6f839 100644 --- a/PathPlanning/FrenetOptimalTrajectory/cubic_spline_planner.py +++ b/PathPlanning/FrenetOptimalTrajectory/cubic_spline_planner.py @@ -10,7 +10,7 @@ import bisect class Spline: - u""" + """ Cubic Spline class """ @@ -209,7 +209,7 @@ def main(): ryaw.append(sp.calc_yaw(i_s)) rk.append(sp.calc_curvature(i_s)) - flg, ax = plt.subplots(1) + plt.subplots(1) plt.plot(x, y, "xb", label="input") plt.plot(rx, ry, "-r", label="spline") plt.grid(True) @@ -218,14 +218,14 @@ def main(): plt.ylabel("y[m]") plt.legend() - flg, ax = plt.subplots(1) + plt.subplots(1) plt.plot(s, [np.rad2deg(iyaw) for iyaw in ryaw], "-r", label="yaw") plt.grid(True) plt.legend() plt.xlabel("line length[m]") plt.ylabel("yaw angle[deg]") - flg, ax = plt.subplots(1) + plt.subplots(1) plt.plot(s, rk, "-r", label="curvature") plt.grid(True) plt.legend() diff --git a/PathPlanning/HybridAStar/hybrid_a_star.py b/PathPlanning/HybridAStar/hybrid_a_star.py index caedfa5a..8b7c1052 100644 --- a/PathPlanning/HybridAStar/hybrid_a_star.py +++ b/PathPlanning/HybridAStar/hybrid_a_star.py @@ -151,7 +151,7 @@ def hybrid_a_star_planning(start, goal, ox, oy, xyreso, yawreso): # print(current) - rx, ry, ryaw = [], [], [] + # rx, ry, ryaw = [], [], [] return rx, ry, ryaw diff --git a/PathPlanning/RRTDubins/dubins_path_planning.py b/PathPlanning/RRTDubins/dubins_path_planning.py index 915d81e8..8cee38ab 100644 --- a/PathPlanning/RRTDubins/dubins_path_planning.py +++ b/PathPlanning/RRTDubins/dubins_path_planning.py @@ -245,18 +245,18 @@ def generate_course(length, mode, c): elif m is "R": # right turn pyaw.append(pyaw[-1] - d) pd += d - else: - d = l - pd - px.append(px[-1] + d * c * math.cos(pyaw[-1])) - py.append(py[-1] + d * c * math.sin(pyaw[-1])) - if m is "L": # left turn - pyaw.append(pyaw[-1] + d) - elif m is "S": # Straight - pyaw.append(pyaw[-1]) - elif m is "R": # right turn - pyaw.append(pyaw[-1] - d) - pd += d + d = l - pd + px.append(px[-1] + d * c * math.cos(pyaw[-1])) + py.append(py[-1] + d * c * math.sin(pyaw[-1])) + + if m is "L": # left turn + pyaw.append(pyaw[-1] + d) + elif m is "S": # Straight + pyaw.append(pyaw[-1]) + elif m is "R": # right turn + pyaw.append(pyaw[-1] - d) + pd += d return px, py, pyaw diff --git a/PathPlanning/RRTStarDubins/dubins_path_planning.py b/PathPlanning/RRTStarDubins/dubins_path_planning.py index 35ed991d..cb37b24e 100644 --- a/PathPlanning/RRTStarDubins/dubins_path_planning.py +++ b/PathPlanning/RRTStarDubins/dubins_path_planning.py @@ -240,18 +240,18 @@ def generate_course(length, mode, c): elif m is "R": # right turn pyaw.append(pyaw[-1] - d) pd += d - else: - d = l - pd - px.append(px[-1] + d * c * math.cos(pyaw[-1])) - py.append(py[-1] + d * c * math.sin(pyaw[-1])) - if m is "L": # left turn - pyaw.append(pyaw[-1] + d) - elif m is "S": # Straight - pyaw.append(pyaw[-1]) - elif m is "R": # right turn - pyaw.append(pyaw[-1] - d) - pd += d + d = l - pd + px.append(px[-1] + d * c * math.cos(pyaw[-1])) + py.append(py[-1] + d * c * math.sin(pyaw[-1])) + + if m is "L": # left turn + pyaw.append(pyaw[-1] + d) + elif m is "S": # Straight + pyaw.append(pyaw[-1]) + elif m is "R": # right turn + pyaw.append(pyaw[-1] - d) + pd += d return px, py, pyaw diff --git a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py b/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py index f1ecea68..5b04ebb0 100644 --- a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py +++ b/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py @@ -290,7 +290,6 @@ def generate_local_course(L, lengths, mode, maxc, step_size): else: d = -step_size - pd = d ll = 0.0 for (m, l, i) in zip(mode, lengths, range(len(mode))): diff --git a/PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py b/PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py index 23ee89b9..5aca719f 100644 --- a/PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py +++ b/PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py @@ -173,7 +173,6 @@ def closed_loop_prediction(cx, cy, cyaw, ck, speed_profile, goal): yaw = [state.yaw] v = [state.v] t = [0.0] - target_ind = calc_nearest_index(state, cx, cy, cyaw) e, e_th = 0.0, 0.0 @@ -261,7 +260,7 @@ def main(): if show_animation: plt.close() - flg, _ = plt.subplots(1) + plt.subplots(1) plt.plot(ax, ay, "xb", label="waypoints") plt.plot(cx, cy, "-r", label="target course") plt.plot(x, y, "-g", label="tracking") @@ -271,14 +270,14 @@ def main(): plt.ylabel("y[m]") plt.legend() - flg, ax = plt.subplots(1) + plt.subplots(1) plt.plot(s, [np.rad2deg(iyaw) for iyaw in cyaw], "-r", label="yaw") plt.grid(True) plt.legend() plt.xlabel("line length[m]") plt.ylabel("yaw angle[deg]") - flg, ax = plt.subplots(1) + plt.subplots(1) plt.plot(s, ck, "-r", label="curvature") plt.grid(True) plt.legend() diff --git a/PathTracking/lqr_steer_control/lqr_steer_control.py b/PathTracking/lqr_steer_control/lqr_steer_control.py index ece3e97b..0c73f975 100644 --- a/PathTracking/lqr_steer_control/lqr_steer_control.py +++ b/PathTracking/lqr_steer_control/lqr_steer_control.py @@ -171,7 +171,6 @@ def closed_loop_prediction(cx, cy, cyaw, ck, speed_profile, goal): yaw = [state.yaw] v = [state.v] t = [0.0] - target_ind = calc_nearest_index(state, cx, cy, cyaw) e, e_th = 0.0, 0.0 @@ -237,10 +236,6 @@ def calc_speed_profile(cx, cy, cyaw, target_speed): speed_profile[-1] = 0.0 - # flg, ax = plt.subplots(1) - # plt.plot(speed_profile, "-r") - # plt.show() - return speed_profile @@ -260,7 +255,7 @@ def main(): if show_animation: plt.close() - flg, _ = plt.subplots(1) + plt.subplots(1) plt.plot(ax, ay, "xb", label="input") plt.plot(cx, cy, "-r", label="spline") plt.plot(x, y, "-g", label="tracking") @@ -270,14 +265,14 @@ def main(): plt.ylabel("y[m]") plt.legend() - flg, ax = plt.subplots(1) + plt.subplots(1) plt.plot(s, [np.rad2deg(iyaw) for iyaw in cyaw], "-r", label="yaw") plt.grid(True) plt.legend() plt.xlabel("line length[m]") plt.ylabel("yaw angle[deg]") - flg, ax = plt.subplots(1) + plt.subplots(1) plt.plot(s, ck, "-r", label="curvature") plt.grid(True) plt.legend() diff --git a/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py b/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py index d75fcfed..9d6e3302 100644 --- a/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py +++ b/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py @@ -181,10 +181,6 @@ def calc_speed_profile(cx, cy, cyaw, target_speed): speed_profile[-1] = 0.0 - # flg, ax = plt.subplots(1) - # plt.plot(speed_profile, "-r") - # plt.show() - return speed_profile @@ -208,7 +204,7 @@ def main(): if show_animation: plt.close() - flg, _ = plt.subplots(1) + plt.subplots(1) plt.plot(ax, ay, "xb", label="input") plt.plot(cx, cy, "-r", label="spline") plt.plot(x, y, "-g", label="tracking") @@ -218,14 +214,14 @@ def main(): plt.ylabel("y[m]") plt.legend() - flg, ax = plt.subplots(1) + plt.subplots(1) plt.plot(s, [np.rad2deg(iyaw) for iyaw in cyaw], "-r", label="yaw") plt.grid(True) plt.legend() plt.xlabel("line length[m]") plt.ylabel("yaw angle[deg]") - flg, ax = plt.subplots(1) + plt.subplots(1) plt.plot(s, ck, "-r", label="curvature") plt.grid(True) plt.legend() diff --git a/SLAM/iterative_closest_point/iterative_closest_point.py b/SLAM/iterative_closest_point/iterative_closest_point.py index 18bfb723..a1412f4a 100644 --- a/SLAM/iterative_closest_point/iterative_closest_point.py +++ b/SLAM/iterative_closest_point/iterative_closest_point.py @@ -45,7 +45,7 @@ def ICP_matching(ppoints, cpoints): Rt, Tt = SVD_motion_estimation(ppoints[:, inds], cpoints) # update current points - cpoints = (Rt @ cpoints) + Tt[:,np.newaxis] + cpoints = (Rt @ cpoints) + Tt[:, np.newaxis] H = update_homogenerous_matrix(H, Rt, Tt) @@ -114,8 +114,8 @@ def SVD_motion_estimation(ppoints, cpoints): pm = np.mean(ppoints, axis=1) cm = np.mean(cpoints, axis=1) - pshift = ppoints - pm[:,np.newaxis] - cshift = cpoints - cm[:,np.newaxis] + pshift = ppoints - pm[:, np.newaxis] + cshift = cpoints - cm[:, np.newaxis] W = cshift @ pshift.T u, s, vh = np.linalg.svd(W) @@ -151,6 +151,8 @@ def main(): cpoints = np.vstack((cx, cy)) R, T = ICP_matching(ppoints, cpoints) + print("R:", R) + print("T:", T) if __name__ == '__main__':