From 13e4a818392a5220fca61c7a1623cd1079d3c1c9 Mon Sep 17 00:00:00 2001 From: Videh Patel Date: Fri, 1 Dec 2023 18:55:30 +0530 Subject: [PATCH 1/4] switched to using utils.angle_mod() --- .../n_joint_arm_to_point_control.py | 3 ++- .../two_joint_arm_to_point_control.py | 3 ++- Control/move_to_pose/move_to_pose.py | 7 +++---- PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py | 9 ++------- 4 files changed, 9 insertions(+), 13 deletions(-) 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 498ef41c1f..ef1875bb35 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 @@ -11,6 +11,7 @@ import numpy as np from ArmNavigation.n_joint_arm_to_point_control.NLinkArm import NLinkArm +from utils.angle import angle_mod # Simulation parameters Kp = 2 @@ -159,7 +160,7 @@ def ang_diff(theta1, theta2): """ Returns the difference between two angles in the range -pi to +pi """ - return (theta1 - theta2 + np.pi) % (2 * np.pi) - np.pi + return angle_mod(theta1 - theta2) if __name__ == '__main__': diff --git a/ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py b/ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py index 66d0ebeb23..c2227f18e3 100644 --- a/ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py +++ b/ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py @@ -15,6 +15,7 @@ import matplotlib.pyplot as plt import numpy as np import math +from utils.angle import angle_mod # Simulation parameters @@ -110,7 +111,7 @@ def plot_arm(theta1, theta2, target_x, target_y): # pragma: no cover def ang_diff(theta1, theta2): # Returns the difference between two angles in the range -pi to +pi - return (theta1 - theta2 + np.pi) % (2 * np.pi) - np.pi + return angle_mod(theta1 - theta2) def click(event): # pragma: no cover diff --git a/Control/move_to_pose/move_to_pose.py b/Control/move_to_pose/move_to_pose.py index 73604ccd7f..cacce31aee 100644 --- a/Control/move_to_pose/move_to_pose.py +++ b/Control/move_to_pose/move_to_pose.py @@ -13,7 +13,7 @@ import matplotlib.pyplot as plt import numpy as np from random import random - +from utils.angle import angle_mod class PathFinderController: """ @@ -71,9 +71,8 @@ def calc_control_command(self, x_diff, y_diff, theta, theta_goal): # from 0 rad to 2*pi rad with slight turn rho = np.hypot(x_diff, y_diff) - alpha = (np.arctan2(y_diff, x_diff) - - theta + np.pi) % (2 * np.pi) - np.pi - beta = (theta_goal - theta - alpha + np.pi) % (2 * np.pi) - np.pi + alpha = angle_mod(np.arctan2(y_diff, x_diff) - theta) + beta = angle_mod(theta_goal - theta - alpha) v = self.Kp_rho * rho w = self.Kp_alpha * alpha - controller.Kp_beta * beta diff --git a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py b/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py index c55d2629e5..031b90e3c1 100644 --- a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py +++ b/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py @@ -9,6 +9,7 @@ import matplotlib.pyplot as plt import numpy as np +from utils.angle import angle_mod show_animation = True @@ -42,13 +43,7 @@ def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): def mod2pi(x): # Be consistent with fmod in cplusplus here. - v = np.mod(x, np.copysign(2.0 * math.pi, x)) - if v < -math.pi: - v += 2.0 * math.pi - else: - if v > math.pi: - v -= 2.0 * math.pi - return v + return angle_mod(x) def straight_left_straight(x, y, phi): From a5d0f0182597f79836dd3f6a77df79017503fa16 Mon Sep 17 00:00:00 2001 From: Videh Patel Date: Fri, 1 Dec 2023 19:24:09 +0530 Subject: [PATCH 2/4] switched to using utils.angle_mod() --- .../ensemble_kalman_filter/ensemble_kalman_filter.py | 3 ++- PathPlanning/ClosedLoopRRTStar/unicycle_model.py | 3 ++- .../ModelPredictiveTrajectoryGenerator/motion_model.py | 3 ++- PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py | 6 +----- .../lqr_speed_steer_control/lqr_speed_steer_control.py | 4 ++-- PathTracking/lqr_steer_control/lqr_steer_control.py | 3 ++- .../model_predictive_speed_and_steer_control.py | 9 ++------- PathTracking/rear_wheel_feedback/rear_wheel_feedback.py | 9 ++------- PathTracking/stanley_controller/stanley_controller.py | 9 ++------- SLAM/EKFSLAM/ekf_slam.py | 3 ++- SLAM/FastSLAM1/fast_slam1.py | 3 ++- SLAM/FastSLAM2/fast_slam2.py | 3 ++- SLAM/GraphBasedSLAM/graph_based_slam.py | 3 ++- 13 files changed, 25 insertions(+), 36 deletions(-) diff --git a/Localization/ensemble_kalman_filter/ensemble_kalman_filter.py b/Localization/ensemble_kalman_filter/ensemble_kalman_filter.py index fc8280e7bf..2bab3b49c1 100644 --- a/Localization/ensemble_kalman_filter/ensemble_kalman_filter.py +++ b/Localization/ensemble_kalman_filter/ensemble_kalman_filter.py @@ -16,6 +16,7 @@ import math import matplotlib.pyplot as plt import numpy as np +from utils.angle import angle_mod from utils.angle import rot_mat_2d @@ -179,7 +180,7 @@ def plot_covariance_ellipse(xEst, PEst): # pragma: no cover def pi_2_pi(angle): - return (angle + math.pi) % (2 * math.pi) - math.pi + return angle_mod(angle) def main(): diff --git a/PathPlanning/ClosedLoopRRTStar/unicycle_model.py b/PathPlanning/ClosedLoopRRTStar/unicycle_model.py index 5a0fd17a4e..c05f76c84e 100644 --- a/PathPlanning/ClosedLoopRRTStar/unicycle_model.py +++ b/PathPlanning/ClosedLoopRRTStar/unicycle_model.py @@ -8,6 +8,7 @@ import math import numpy as np +from utils.angle import angle_mod dt = 0.05 # [s] L = 0.9 # [m] @@ -39,7 +40,7 @@ def update(state, a, delta): def pi_2_pi(angle): - return (angle + math.pi) % (2 * math.pi) - math.pi + return angle_mod(angle) if __name__ == '__main__': # pragma: no cover diff --git a/PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py b/PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py index 531bf91ef5..5ef6d2e23f 100644 --- a/PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py +++ b/PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py @@ -1,6 +1,7 @@ import math import numpy as np from scipy.interpolate import interp1d +from utils.angle import angle_mod # motion parameter L = 1.0 # wheel base @@ -18,7 +19,7 @@ def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0): def pi_2_pi(angle): - return (angle + math.pi) % (2 * math.pi) - math.pi + return angle_mod(angle) def update(state, v, delta, dt, L): diff --git a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py b/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py index 031b90e3c1..5b06619064 100644 --- a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py +++ b/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py @@ -291,10 +291,6 @@ def interpolate(dist, length, mode, max_curvature, origin_x, origin_y, return x, y, yaw, 1 if length > 0.0 else -1 -def pi_2_pi(angle): - return (angle + math.pi) % (2 * math.pi) - math.pi - - def calc_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size): q0 = [sx, sy, syaw] q1 = [gx, gy, gyaw] @@ -310,7 +306,7 @@ def calc_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size): (ix, iy) in zip(xs, ys)] path.y = [-math.sin(-q0[2]) * ix + math.cos(-q0[2]) * iy + q0[1] for (ix, iy) in zip(xs, ys)] - path.yaw = [pi_2_pi(yaw + q0[2]) for yaw in yaws] + path.yaw = [mod2pi(yaw + q0[2]) for yaw in yaws] path.directions = directions path.lengths = [length / maxc for length in path.lengths] path.L = path.L / maxc 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 7e57abb80e..a70e30d378 100644 --- a/PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py +++ b/PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py @@ -11,6 +11,7 @@ import numpy as np import scipy.linalg as la import pathlib +from utils.angle import angle_mod sys.path.append(str(pathlib.Path(__file__).parent.parent.parent)) from PathPlanning.CubicSpline import cubic_spline_planner @@ -52,8 +53,7 @@ def update(state, a, delta): def pi_2_pi(angle): - return (angle + math.pi) % (2 * math.pi) - math.pi - + return angle_mod(angle) def solve_dare(A, B, Q, R): """ diff --git a/PathTracking/lqr_steer_control/lqr_steer_control.py b/PathTracking/lqr_steer_control/lqr_steer_control.py index 95a1b92ce5..4078ea56db 100644 --- a/PathTracking/lqr_steer_control/lqr_steer_control.py +++ b/PathTracking/lqr_steer_control/lqr_steer_control.py @@ -11,6 +11,7 @@ import numpy as np import sys import pathlib +from utils.angle import angle_mod sys.path.append(str(pathlib.Path(__file__).parent.parent.parent)) from PathPlanning.CubicSpline import cubic_spline_planner @@ -61,7 +62,7 @@ def PIDControl(target, current): def pi_2_pi(angle): - return (angle + math.pi) % (2 * math.pi) - math.pi + return angle_mod(angle) def solve_DARE(A, B, Q, R): diff --git a/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py b/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py index 3015313198..c55018cf26 100644 --- a/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py +++ b/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py @@ -11,6 +11,7 @@ import numpy as np import sys import pathlib +from utils.angle import angle_mod sys.path.append(str(pathlib.Path(__file__).parent.parent.parent)) from PathPlanning.CubicSpline import cubic_spline_planner @@ -69,13 +70,7 @@ def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0): def pi_2_pi(angle): - while(angle > math.pi): - angle = angle - 2.0 * math.pi - - while(angle < -math.pi): - angle = angle + 2.0 * math.pi - - return angle + return angle_mod(angle) def get_linear_model_matrix(v, phi, delta): diff --git a/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py b/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py index ff72454f34..2851a94559 100644 --- a/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py +++ b/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py @@ -8,6 +8,7 @@ import matplotlib.pyplot as plt import math import numpy as np +from utils.angle import angle_mod from scipy import interpolate from scipy import optimize @@ -96,13 +97,7 @@ def pid_control(target, current): return a def pi_2_pi(angle): - while(angle > math.pi): - angle = angle - 2.0 * math.pi - - while(angle < -math.pi): - angle = angle + 2.0 * math.pi - - return angle + return angle_mod(angle) def rear_wheel_feedback_control(state, e, k, yaw_ref): v = state.v diff --git a/PathTracking/stanley_controller/stanley_controller.py b/PathTracking/stanley_controller/stanley_controller.py index 9c475cbaf2..171df21871 100644 --- a/PathTracking/stanley_controller/stanley_controller.py +++ b/PathTracking/stanley_controller/stanley_controller.py @@ -13,6 +13,7 @@ import matplotlib.pyplot as plt import sys import pathlib +from utils.angle import angle_mod sys.path.append(str(pathlib.Path(__file__).parent.parent.parent)) from PathPlanning.CubicSpline import cubic_spline_planner @@ -106,13 +107,7 @@ def normalize_angle(angle): :param angle: (float) :return: (float) Angle in radian in [-pi, pi] """ - while angle > np.pi: - angle -= 2.0 * np.pi - - while angle < -np.pi: - angle += 2.0 * np.pi - - return angle + return angle_mod(angle) def calc_target_index(state, cx, cy): diff --git a/SLAM/EKFSLAM/ekf_slam.py b/SLAM/EKFSLAM/ekf_slam.py index bb648ce9d9..a624e8765b 100644 --- a/SLAM/EKFSLAM/ekf_slam.py +++ b/SLAM/EKFSLAM/ekf_slam.py @@ -8,6 +8,7 @@ import matplotlib.pyplot as plt import numpy as np +from utils.angle import angle_mod # EKF state covariance Cx = np.diag([0.5, 0.5, np.deg2rad(30.0)]) ** 2 @@ -192,7 +193,7 @@ def jacob_h(q, delta, x, i): def pi_2_pi(angle): - return (angle + math.pi) % (2 * math.pi) - math.pi + return angle_mod(angle) def main(): diff --git a/SLAM/FastSLAM1/fast_slam1.py b/SLAM/FastSLAM1/fast_slam1.py index 7b89f52df4..17f6d5e572 100644 --- a/SLAM/FastSLAM1/fast_slam1.py +++ b/SLAM/FastSLAM1/fast_slam1.py @@ -10,6 +10,7 @@ import matplotlib.pyplot as plt import numpy as np +from utils.angle import angle_mod # Fast SLAM covariance Q = np.diag([3.0, np.deg2rad(10.0)]) ** 2 @@ -321,7 +322,7 @@ def motion_model(x, u): def pi_2_pi(angle): - return (angle + math.pi) % (2 * math.pi) - math.pi + return angle_mod(angle) def main(): diff --git a/SLAM/FastSLAM2/fast_slam2.py b/SLAM/FastSLAM2/fast_slam2.py index aa77aa956a..2b57e3bcc3 100644 --- a/SLAM/FastSLAM2/fast_slam2.py +++ b/SLAM/FastSLAM2/fast_slam2.py @@ -10,6 +10,7 @@ import matplotlib.pyplot as plt import numpy as np +from utils.angle import angle_mod # Fast SLAM covariance Q = np.diag([3.0, np.deg2rad(10.0)]) ** 2 @@ -346,7 +347,7 @@ def motion_model(x, u): def pi_2_pi(angle): - return (angle + math.pi) % (2 * math.pi) - math.pi + return angle_mod(angle) def main(): diff --git a/SLAM/GraphBasedSLAM/graph_based_slam.py b/SLAM/GraphBasedSLAM/graph_based_slam.py index 4d66ef6732..edd20a959c 100644 --- a/SLAM/GraphBasedSLAM/graph_based_slam.py +++ b/SLAM/GraphBasedSLAM/graph_based_slam.py @@ -21,6 +21,7 @@ import matplotlib.pyplot as plt import numpy as np from scipy.spatial.transform import Rotation as Rot +from utils.angle import angle_mod # Simulation parameter Q_sim = np.diag([0.2, np.deg2rad(1.0)]) ** 2 @@ -249,7 +250,7 @@ def motion_model(x, u): def pi_2_pi(angle): - return (angle + math.pi) % (2 * math.pi) - math.pi + return angle_mod(angle) def main(): From c970d4712937816bea4314e5430b3efd3e123b78 Mon Sep 17 00:00:00 2001 From: Videh Patel Date: Fri, 1 Dec 2023 21:08:12 +0530 Subject: [PATCH 3/4] renamed mod2pi to pi_2_pi --- .../ReedsSheppPath/reeds_shepp_path_planning.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py b/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py index 5b06619064..5f4dd5a6be 100644 --- a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py +++ b/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py @@ -41,13 +41,13 @@ def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): plt.plot(x, y) -def mod2pi(x): +def pi_2_pi(x): # Be consistent with fmod in cplusplus here. return angle_mod(x) def straight_left_straight(x, y, phi): - phi = mod2pi(phi) + phi = pi_2_pi(phi) # only take phi in (0.01*math.pi, 0.99*math.pi) for the sake of speed. # phi in (0, 0.01*math.pi) will make test2() in test_rrt_star_reeds_shepp.py # extremely time-consuming, since the value of xd, t will be very large. @@ -103,7 +103,7 @@ def polar(x, y): def left_straight_left(x, y, phi): u, t = polar(x - math.sin(phi), y - 1.0 + math.cos(phi)) if t >= 0.0: - v = mod2pi(phi - t) + v = pi_2_pi(phi - t) if v >= 0.0: return True, t, u, v @@ -115,8 +115,8 @@ def left_right_left(x, y, phi): if u1 <= 4.0: u = -2.0 * math.asin(0.25 * u1) - t = mod2pi(t1 + 0.5 * u + math.pi) - v = mod2pi(phi - t + u) + t = pi_2_pi(t1 + 0.5 * u + math.pi) + v = pi_2_pi(phi - t + u) if t >= 0.0 >= u: return True, t, u, v @@ -206,8 +206,8 @@ def left_straight_right(x, y, phi): if u1 >= 4.0: u = math.sqrt(u1 - 4.0) theta = math.atan2(2.0, u) - t = mod2pi(t1 + theta) - v = mod2pi(t - phi) + t = pi_2_pi(t1 + theta) + v = pi_2_pi(t - phi) if t >= 0.0 and v >= 0.0: return True, t, u, v @@ -306,7 +306,7 @@ def calc_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size): (ix, iy) in zip(xs, ys)] path.y = [-math.sin(-q0[2]) * ix + math.cos(-q0[2]) * iy + q0[1] for (ix, iy) in zip(xs, ys)] - path.yaw = [mod2pi(yaw + q0[2]) for yaw in yaws] + path.yaw = [pi_2_pi(yaw + q0[2]) for yaw in yaws] path.directions = directions path.lengths = [length / maxc for length in path.lengths] path.L = path.L / maxc From 92b202d25ff589e0c623d14cf888f9dcde9983a7 Mon Sep 17 00:00:00 2001 From: Videh Patel Date: Mon, 4 Dec 2023 11:35:32 +0530 Subject: [PATCH 4/4] Removed linting errors --- .../n_joint_arm_to_point_control.py | 2 +- Control/move_to_pose/move_to_pose.py | 12 +++++------- .../trajectory_generator.py | 2 +- .../rear_wheel_feedback/rear_wheel_feedback.py | 14 +++++++------- .../stanley_controller/stanley_controller.py | 4 ++-- 5 files changed, 16 insertions(+), 18 deletions(-) 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 ef1875bb35..a237523336 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 @@ -165,4 +165,4 @@ def ang_diff(theta1, theta2): if __name__ == '__main__': # main() - animation() \ No newline at end of file + animation() diff --git a/Control/move_to_pose/move_to_pose.py b/Control/move_to_pose/move_to_pose.py index cacce31aee..8cad627dbf 100644 --- a/Control/move_to_pose/move_to_pose.py +++ b/Control/move_to_pose/move_to_pose.py @@ -172,16 +172,14 @@ def transformation_matrix(x, y, theta): def main(): for i in range(5): - x_start = 20 * random() - y_start = 20 * random() - theta_start = 2 * np.pi * random() - np.pi + x_start: float = 20 * random() + y_start: float = 20 * random() + theta_start: float = 2 * np.pi * random() - np.pi x_goal = 20 * random() y_goal = 20 * random() theta_goal = 2 * np.pi * random() - np.pi - print("Initial x: %.2f m\nInitial y: %.2f m\nInitial theta: %.2f rad\n" % - (x_start, y_start, theta_start)) - print("Goal x: %.2f m\nGoal y: %.2f m\nGoal theta: %.2f rad\n" % - (x_goal, y_goal, theta_goal)) + print(f"Initial x: {round(x_start)} m\nInitial y: {round(y_start)} m\nInitial theta: {round(theta_start)} rad\n") + print(f"Goal x: {round(x_goal)} m\nGoal y: {round(y_goal)} m\nGoal theta: {round(theta_goal)} rad\n") move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal) diff --git a/PathPlanning/ModelPredictiveTrajectoryGenerator/trajectory_generator.py b/PathPlanning/ModelPredictiveTrajectoryGenerator/trajectory_generator.py index 97c0ad8b80..6084fc1a07 100644 --- a/PathPlanning/ModelPredictiveTrajectoryGenerator/trajectory_generator.py +++ b/PathPlanning/ModelPredictiveTrajectoryGenerator/trajectory_generator.py @@ -18,7 +18,7 @@ # optimization parameter max_iter = 100 -h = np.array([0.5, 0.02, 0.02]).T # parameter sampling distance +h: np.ndarray = np.array([0.5, 0.02, 0.02]).T # parameter sampling distance cost_th = 0.1 show_animation = True diff --git a/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py b/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py index 2851a94559..1702bd47dd 100644 --- a/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py +++ b/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py @@ -52,21 +52,21 @@ def __init__(self, x, y): self.ddY = self.Y.derivative(2) self.length = s[-1] - + def calc_yaw(self, s): dx, dy = self.dX(s), self.dY(s) return np.arctan2(dy, dx) - + def calc_curvature(self, s): dx, dy = self.dX(s), self.dY(s) ddx, ddy = self.ddX(s), self.ddY(s) return (ddy * dx - ddx * dy) / ((dx ** 2 + dy ** 2)**(3 / 2)) - + def __find_nearest_point(self, s0, x, y): def calc_distance(_s, *args): _x, _y= self.X(_s), self.Y(_s) return (_x - args[0])**2 + (_y - args[1])**2 - + def calc_distance_jacobian(_s, *args): _x, _y = self.X(_s), self.Y(_s) _dx, _dy = self.dX(_s), self.dY(_s) @@ -77,7 +77,7 @@ def calc_distance_jacobian(_s, *args): def calc_track_error(self, x, y, s0): ret = self.__find_nearest_point(s0, x, y) - + s = ret[0][0] e = ret[1] @@ -165,7 +165,7 @@ def simulate(path_ref, goal): plt.plot(path_ref.X(s0), path_ref.Y(s0), "xg", label="target") plt.axis("equal") plt.grid(True) - plt.title("speed[km/h]:{:.2f}, target s-param:{:.2f}".format(round(state.v * 3.6, 2), s0)) + plt.title(f"speed[km/h]:{round(state.v * 3.6, 2):.2f}, target s-param:{s0:.2f}") plt.pause(0.0001) return t, x, y, yaw, v, goal_flag @@ -179,7 +179,7 @@ def calc_target_speed(state, yaw_ref): if switch: state.direction *= -1 return 0.0 - + if state.direction != 1: return -target_speed diff --git a/PathTracking/stanley_controller/stanley_controller.py b/PathTracking/stanley_controller/stanley_controller.py index 171df21871..08aa049395 100644 --- a/PathTracking/stanley_controller/stanley_controller.py +++ b/PathTracking/stanley_controller/stanley_controller.py @@ -27,7 +27,7 @@ show_animation = True -class State(object): +class State: """ Class representing the state of a vehicle. @@ -39,7 +39,7 @@ class State(object): def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0): """Instantiate the object.""" - super(State, self).__init__() + super().__init__() self.x = x self.y = y self.yaw = yaw