From 76b0d04a3ce61f6a1935e496ea43c809fb247097 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Sun, 11 Sep 2022 07:21:37 +0900 Subject: [PATCH] using pathlib based local module import (#722) * using pathlib based local module import * remove unused import --- .../drone_3d_trajectory_following/__init__.py | 5 ++-- ArmNavigation/n_joint_arm_3d/__init__.py | 4 +-- .../n_joint_arm_to_point_control.py | 2 +- .../rrt_star_seven_joint_arm_control.py | 13 ++++----- .../ensemble_kalman_filter.py | 5 ++-- .../extended_kalman_filter.py | 5 ++-- .../particle_filter/particle_filter.py | 4 +-- .../unscented_kalman_filter.py | 4 +-- Mapping/rectangle_fitting/__init_.py | 5 ++-- .../rectangle_fitting/rectangle_fitting.py | 5 ++-- Mapping/rectangle_fitting/simulator.py | 4 +-- .../closed_loop_rrt_star_car.py | 24 +++++---------- .../ClosedLoopRRTStar/pure_pursuit.py | 6 +++- .../DubinsPath/dubins_path_planner.py | 5 ++-- .../eta3_spline_trajectory.py | 10 ++----- .../frenet_optimal_trajectory.py | 16 ++++------ .../grid_based_sweep_coverage_path_planner.py | 17 +++-------- PathPlanning/HybridAStar/car.py | 7 +++-- PathPlanning/HybridAStar/hybrid_a_star.py | 18 ++++-------- .../InformedRRTStar/informed_rrt_star.py | 4 +-- .../{LQRplanner.py => lqr_planner.py} | 0 PathPlanning/LQRRRTStar/lqr_rrt_star.py | 16 ++++------ ...y_generator.py => trajectory_generator.py} | 7 +++-- PathPlanning/RRT/rrt_with_pathsmoothing.py | 13 +++------ PathPlanning/RRT/rrt_with_sobol_sampler.py | 5 ++-- PathPlanning/RRTDubins/rrt_dubins.py | 29 +++++++------------ PathPlanning/RRTStar/rrt_star.py | 12 +++----- PathPlanning/RRTStarDubins/rrt_star_dubins.py | 27 ++++++----------- .../RRTStarReedsShepp/rrt_star_reeds_shepp.py | 16 +++------- .../state_lattice_planner.py | 23 +++++++-------- .../VisibilityRoadMap/visibility_road_map.py | 10 +++---- PathPlanning/VoronoiRoadMap/__init__.py | 4 --- .../VoronoiRoadMap/voronoi_road_map.py | 6 +++- .../lqr_speed_steer_control.py | 12 ++------ .../lqr_steer_control/lqr_steer_control.py | 14 +++------ ...odel_predictive_speed_and_steer_control.py | 12 ++------ .../stanley_controller/stanley_controller.py | 12 ++------ SLAM/GraphBasedSLAM/graph_based_slam.py | 4 +-- tests/test_LQR_planner.py | 2 +- 39 files changed, 142 insertions(+), 245 deletions(-) rename PathPlanning/LQRPlanner/{LQRplanner.py => lqr_planner.py} (100%) rename PathPlanning/ModelPredictiveTrajectoryGenerator/{model_predictive_trajectory_generator.py => trajectory_generator.py} (95%) diff --git a/AerialNavigation/drone_3d_trajectory_following/__init__.py b/AerialNavigation/drone_3d_trajectory_following/__init__.py index 087dab64..2194d4c3 100644 --- a/AerialNavigation/drone_3d_trajectory_following/__init__.py +++ b/AerialNavigation/drone_3d_trajectory_following/__init__.py @@ -1,4 +1,3 @@ -import os import sys - -sys.path.append(os.path.dirname(os.path.abspath(__file__))) +import pathlib +sys.path.append(str(pathlib.Path(__file__).parent)) diff --git a/ArmNavigation/n_joint_arm_3d/__init__.py b/ArmNavigation/n_joint_arm_3d/__init__.py index ba5dac65..2194d4c3 100644 --- a/ArmNavigation/n_joint_arm_3d/__init__.py +++ b/ArmNavigation/n_joint_arm_3d/__init__.py @@ -1,3 +1,3 @@ import sys -import os -sys.path.append(os.path.dirname(os.path.abspath(__file__))) +import pathlib +sys.path.append(str(pathlib.Path(__file__).parent)) 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 36093df0..498ef41c 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 @@ -5,11 +5,11 @@ Author: Daniel Ingram (daniel-s-ingram) Atsushi Sakai (@Atsushi_twi) """ -import numpy as np import sys from pathlib import Path sys.path.append(str(Path(__file__).parent.parent.parent)) +import numpy as np from ArmNavigation.n_joint_arm_to_point_control.NLinkArm import NLinkArm # Simulation parameters diff --git a/ArmNavigation/rrt_star_seven_joint_arm_control/rrt_star_seven_joint_arm_control.py b/ArmNavigation/rrt_star_seven_joint_arm_control/rrt_star_seven_joint_arm_control.py index 0de4c7ed..fca55184 100755 --- a/ArmNavigation/rrt_star_seven_joint_arm_control/rrt_star_seven_joint_arm_control.py +++ b/ArmNavigation/rrt_star_seven_joint_arm_control/rrt_star_seven_joint_arm_control.py @@ -3,18 +3,15 @@ RRT* path planner for a seven joint arm Author: Mahyar Abdeetedal (mahyaret) """ import math -import os -import sys import random import numpy as np from mpl_toolkits import mplot3d import matplotlib.pyplot as plt -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../n_joint_arm_3d/") -try: - from NLinkArm3d import NLinkArm -except ImportError: - raise +import sys +import pathlib +sys.path.append(str(pathlib.Path(__file__).parent.parent)) + +from n_joint_arm_3d.NLinkArm3d import NLinkArm show_animation = True verbose = False diff --git a/Localization/ensemble_kalman_filter/ensemble_kalman_filter.py b/Localization/ensemble_kalman_filter/ensemble_kalman_filter.py index 1e8d1fc6..fc8280e7 100644 --- a/Localization/ensemble_kalman_filter/ensemble_kalman_filter.py +++ b/Localization/ensemble_kalman_filter/ensemble_kalman_filter.py @@ -9,10 +9,9 @@ Ensemble Kalman filtering (https://rmets.onlinelibrary.wiley.com/doi/10.1256/qj.05.135) """ - import sys -import os -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../utils/") +import pathlib +sys.path.append(str(pathlib.Path(__file__).parent.parent.parent)) import math import matplotlib.pyplot as plt diff --git a/Localization/extended_kalman_filter/extended_kalman_filter.py b/Localization/extended_kalman_filter/extended_kalman_filter.py index a5dea19f..c2fe8e3a 100644 --- a/Localization/extended_kalman_filter/extended_kalman_filter.py +++ b/Localization/extended_kalman_filter/extended_kalman_filter.py @@ -5,10 +5,9 @@ Extended kalman filter (EKF) localization sample author: Atsushi Sakai (@Atsushi_twi) """ - import sys -import os -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../utils/") +import pathlib +sys.path.append(str(pathlib.Path(__file__).parent.parent.parent)) import math import matplotlib.pyplot as plt diff --git a/Localization/particle_filter/particle_filter.py b/Localization/particle_filter/particle_filter.py index 1c45687e..17051623 100644 --- a/Localization/particle_filter/particle_filter.py +++ b/Localization/particle_filter/particle_filter.py @@ -6,8 +6,8 @@ author: Atsushi Sakai (@Atsushi_twi) """ import sys -import os -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../utils/") +import pathlib +sys.path.append(str(pathlib.Path(__file__).parent.parent.parent)) import math diff --git a/Localization/unscented_kalman_filter/unscented_kalman_filter.py b/Localization/unscented_kalman_filter/unscented_kalman_filter.py index 4f42cfd5..dbcdeef0 100644 --- a/Localization/unscented_kalman_filter/unscented_kalman_filter.py +++ b/Localization/unscented_kalman_filter/unscented_kalman_filter.py @@ -7,8 +7,8 @@ author: Atsushi Sakai (@Atsushi_twi) """ import sys -import os -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../utils/") +import pathlib +sys.path.append(str(pathlib.Path(__file__).parent.parent.parent)) import math diff --git a/Mapping/rectangle_fitting/__init_.py b/Mapping/rectangle_fitting/__init_.py index 087dab64..2194d4c3 100644 --- a/Mapping/rectangle_fitting/__init_.py +++ b/Mapping/rectangle_fitting/__init_.py @@ -1,4 +1,3 @@ -import os import sys - -sys.path.append(os.path.dirname(os.path.abspath(__file__))) +import pathlib +sys.path.append(str(pathlib.Path(__file__).parent)) diff --git a/Mapping/rectangle_fitting/rectangle_fitting.py b/Mapping/rectangle_fitting/rectangle_fitting.py index 6d3e63f0..124e48fe 100644 --- a/Mapping/rectangle_fitting/rectangle_fitting.py +++ b/Mapping/rectangle_fitting/rectangle_fitting.py @@ -13,9 +13,8 @@ efficient-l-shape-fitting-for-vehicle-detection-using-laser-scanners/ """ import sys -import os -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../utils/") - +import pathlib +sys.path.append(str(pathlib.Path(__file__).parent.parent.parent)) import matplotlib.pyplot as plt import numpy as np diff --git a/Mapping/rectangle_fitting/simulator.py b/Mapping/rectangle_fitting/simulator.py index e75738c3..faca2cb7 100644 --- a/Mapping/rectangle_fitting/simulator.py +++ b/Mapping/rectangle_fitting/simulator.py @@ -6,8 +6,8 @@ author: Atsushi Sakai """ import sys -import os -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../utils/") +import pathlib +sys.path.append(str(pathlib.Path(__file__).parent.parent.parent)) import numpy as np import matplotlib.pyplot as plt diff --git a/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py b/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py index dc9a5756..01ab8349 100644 --- a/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py +++ b/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py @@ -5,27 +5,17 @@ Path planning Sample Code with Closed loop RRT for car like robot. author: AtsushiSakai(@Atsushi_twi) """ -import os -import sys - import matplotlib.pyplot as plt import numpy as np -sys.path.append(os.path.dirname(os.path.abspath(__file__))) +import sys +import pathlib +sys.path.append(str(pathlib.Path(__file__).parent.parent)) -import pure_pursuit -import unicycle_model - -sys.path.append(os.path.dirname( - os.path.abspath(__file__)) + "/../ReedsSheppPath/") -sys.path.append(os.path.dirname( - os.path.abspath(__file__)) + "/../RRTStarReedsShepp/") - -try: - import reeds_shepp_path_planning - from rrt_star_reeds_shepp import RRTStarReedsShepp -except ImportError: - raise +from ClosedLoopRRTStar import pure_pursuit +from ClosedLoopRRTStar import unicycle_model +from ReedsSheppPath import reeds_shepp_path_planning +from RRTStarReedsShepp.rrt_star_reeds_shepp import RRTStarReedsShepp show_animation = True diff --git a/PathPlanning/ClosedLoopRRTStar/pure_pursuit.py b/PathPlanning/ClosedLoopRRTStar/pure_pursuit.py index ca763f0d..982ebeca 100644 --- a/PathPlanning/ClosedLoopRRTStar/pure_pursuit.py +++ b/PathPlanning/ClosedLoopRRTStar/pure_pursuit.py @@ -10,7 +10,11 @@ import math import matplotlib.pyplot as plt import numpy as np -import unicycle_model +import sys +import pathlib +sys.path.append(str(pathlib.Path(__file__).parent.parent)) + +from ClosedLoopRRTStar import unicycle_model Kp = 2.0 # speed propotional gain Lf = 0.5 # look-ahead distance diff --git a/PathPlanning/DubinsPath/dubins_path_planner.py b/PathPlanning/DubinsPath/dubins_path_planner.py index 4d4ee072..a7e8a100 100644 --- a/PathPlanning/DubinsPath/dubins_path_planner.py +++ b/PathPlanning/DubinsPath/dubins_path_planner.py @@ -6,9 +6,8 @@ author Atsushi Sakai(@Atsushi_twi) """ import sys -import os - -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../utils/") +import pathlib +sys.path.append(str(pathlib.Path(__file__).parent.parent.parent)) from math import sin, cos, atan2, sqrt, acos, pi, hypot import numpy as np diff --git a/PathPlanning/Eta3SplineTrajectory/eta3_spline_trajectory.py b/PathPlanning/Eta3SplineTrajectory/eta3_spline_trajectory.py index bd211046..d034cb8a 100644 --- a/PathPlanning/Eta3SplineTrajectory/eta3_spline_trajectory.py +++ b/PathPlanning/Eta3SplineTrajectory/eta3_spline_trajectory.py @@ -16,14 +16,10 @@ import numpy as np import matplotlib.pyplot as plt from matplotlib.collections import LineCollection import sys -import os -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../Eta3SplinePath") +import pathlib +sys.path.append(str(pathlib.Path(__file__).parent.parent)) -try: - from eta3_spline_path import Eta3Path, Eta3PathSegment -except ImportError: - raise +from Eta3SplinePath.eta3_spline_path import Eta3Path, Eta3PathSegment show_animation = True diff --git a/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py b/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py index ef20af22..b7d1b8e6 100644 --- a/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py +++ b/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py @@ -19,18 +19,12 @@ import matplotlib.pyplot as plt import copy import math import sys -import os +import pathlib +sys.path.append(str(pathlib.Path(__file__).parent.parent)) -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../QuinticPolynomialsPlanner/") -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../CubicSpline/") - -try: - from quintic_polynomials_planner import QuinticPolynomial - import cubic_spline_planner -except ImportError: - raise +from QuinticPolynomialsPlanner.quintic_polynomials_planner import \ + QuinticPolynomial +from CubicSpline import cubic_spline_planner SIM_LOOP = 500 diff --git a/PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py b/PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py index 2f4d32b1..31dc9175 100644 --- a/PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py +++ b/PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py @@ -4,25 +4,16 @@ Grid based sweep planner author: Atsushi Sakai """ -import sys -import os -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../utils/") - import math from enum import IntEnum - import numpy as np import matplotlib.pyplot as plt +import sys +import pathlib +sys.path.append(str(pathlib.Path(__file__).parent.parent.parent)) from utils.angle import rot_mat_2d - -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../../Mapping/") - -try: - from grid_map_lib.grid_map_lib import GridMap -except ImportError: - raise +from Mapping.grid_map_lib.grid_map_lib import GridMap do_animation = True diff --git a/PathPlanning/HybridAStar/car.py b/PathPlanning/HybridAStar/car.py index 870dd185..0367ab27 100644 --- a/PathPlanning/HybridAStar/car.py +++ b/PathPlanning/HybridAStar/car.py @@ -7,8 +7,9 @@ author: Zheng Zh (@Zhengzh) """ import sys -import os -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../utils/") +import pathlib +root_dir = pathlib.Path(__file__).parent.parent.parent +sys.path.append(str(root_dir)) from math import cos, sin, tan, pi @@ -76,7 +77,7 @@ def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): def plot_car(x, y, yaw): car_color = '-k' c, s = cos(yaw), sin(yaw) - rot = Rot.from_euler('z', -yaw).as_matrix()[0:2, 0:2] + rot = rot_mat_2d(-yaw) car_outline_x, car_outline_y = [], [] for rx, ry in zip(VRX, VRY): converted_xy = np.stack([rx, ry]).T @ rot diff --git a/PathPlanning/HybridAStar/hybrid_a_star.py b/PathPlanning/HybridAStar/hybrid_a_star.py index 4ef0c1dd..b87fe706 100644 --- a/PathPlanning/HybridAStar/hybrid_a_star.py +++ b/PathPlanning/HybridAStar/hybrid_a_star.py @@ -8,22 +8,16 @@ author: Zheng Zh (@Zhengzh) import heapq import math -import os -import sys - import matplotlib.pyplot as plt import numpy as np from scipy.spatial import cKDTree +import sys +import pathlib +sys.path.append(str(pathlib.Path(__file__).parent.parent)) -sys.path.append(os.path.dirname(os.path.abspath(__file__)) - + "/../ReedsSheppPath") -try: - from dynamic_programming_heuristic import calc_distance_heuristic - import reeds_shepp_path_planning as rs - from car import move, check_car_collision, MAX_STEER, WB, plot_car,\ - BUBBLE_R -except Exception: - raise +from dynamic_programming_heuristic import calc_distance_heuristic +from ReedsSheppPath import reeds_shepp_path_planning as rs +from car import move, check_car_collision, MAX_STEER, WB, plot_car, BUBBLE_R XY_GRID_RESOLUTION = 2.0 # [m] YAW_GRID_RESOLUTION = np.deg2rad(15.0) # [rad] diff --git a/PathPlanning/InformedRRTStar/informed_rrt_star.py b/PathPlanning/InformedRRTStar/informed_rrt_star.py index 10af3a81..25c50a0c 100644 --- a/PathPlanning/InformedRRTStar/informed_rrt_star.py +++ b/PathPlanning/InformedRRTStar/informed_rrt_star.py @@ -10,8 +10,8 @@ https://arxiv.org/pdf/1404.2334.pdf """ import sys -import os -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../utils/") +import pathlib +sys.path.append(str(pathlib.Path(__file__).parent.parent.parent)) import copy import math diff --git a/PathPlanning/LQRPlanner/LQRplanner.py b/PathPlanning/LQRPlanner/lqr_planner.py similarity index 100% rename from PathPlanning/LQRPlanner/LQRplanner.py rename to PathPlanning/LQRPlanner/lqr_planner.py diff --git a/PathPlanning/LQRRRTStar/lqr_rrt_star.py b/PathPlanning/LQRRRTStar/lqr_rrt_star.py index 28d332aa..e3f18a72 100644 --- a/PathPlanning/LQRRRTStar/lqr_rrt_star.py +++ b/PathPlanning/LQRRRTStar/lqr_rrt_star.py @@ -7,21 +7,15 @@ author: AtsushiSakai(@Atsushi_twi) """ import copy import math -import os import random -import sys - import matplotlib.pyplot as plt import numpy as np +import sys +import pathlib +sys.path.append(str(pathlib.Path(__file__).parent.parent)) -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../LQRPlanner/") -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../RRTStar/") - -try: - from LQRplanner import LQRPlanner - from rrt_star import RRTStar -except ImportError: - raise +from LQRPlanner.lqr_planner import LQRPlanner +from RRTStar.rrt_star import RRTStar show_animation = True diff --git a/PathPlanning/ModelPredictiveTrajectoryGenerator/model_predictive_trajectory_generator.py b/PathPlanning/ModelPredictiveTrajectoryGenerator/trajectory_generator.py similarity index 95% rename from PathPlanning/ModelPredictiveTrajectoryGenerator/model_predictive_trajectory_generator.py rename to PathPlanning/ModelPredictiveTrajectoryGenerator/trajectory_generator.py index 3ba1dd9f..a44e1ac9 100644 --- a/PathPlanning/ModelPredictiveTrajectoryGenerator/model_predictive_trajectory_generator.py +++ b/PathPlanning/ModelPredictiveTrajectoryGenerator/trajectory_generator.py @@ -7,11 +7,14 @@ author: Atsushi Sakai(@Atsushi_twi) """ import math - import matplotlib.pyplot as plt import numpy as np +import sys +import pathlib +path_planning_dir = pathlib.Path(__file__).parent.parent +sys.path.append(str(path_planning_dir)) -import motion_model +import ModelPredictiveTrajectoryGenerator.motion_model as motion_model # optimization parameter max_iter = 100 diff --git a/PathPlanning/RRT/rrt_with_pathsmoothing.py b/PathPlanning/RRT/rrt_with_pathsmoothing.py index 9826dba0..616c73a6 100644 --- a/PathPlanning/RRT/rrt_with_pathsmoothing.py +++ b/PathPlanning/RRT/rrt_with_pathsmoothing.py @@ -7,18 +7,13 @@ Path planning Sample Code with RRT with path smoothing """ import math -import os import random -import sys - import matplotlib.pyplot as plt +import sys +import pathlib +sys.path.append(str(pathlib.Path(__file__).parent)) -sys.path.append(os.path.dirname(os.path.abspath(__file__))) - -try: - from rrt import RRT -except ImportError: - raise +from rrt import RRT show_animation = True diff --git a/PathPlanning/RRT/rrt_with_sobol_sampler.py b/PathPlanning/RRT/rrt_with_sobol_sampler.py index 8045d8f7..73488210 100644 --- a/PathPlanning/RRT/rrt_with_sobol_sampler.py +++ b/PathPlanning/RRT/rrt_with_sobol_sampler.py @@ -28,12 +28,11 @@ Rojas (rafaelrojasmiliani@gmail.com) import math import random -from sobol import sobol_quasirand -import sys - import matplotlib.pyplot as plt import numpy as np +from sobol import sobol_quasirand + show_animation = True diff --git a/PathPlanning/RRTDubins/rrt_dubins.py b/PathPlanning/RRTDubins/rrt_dubins.py index f1bd03ca..f938419f 100644 --- a/PathPlanning/RRTDubins/rrt_dubins.py +++ b/PathPlanning/RRTDubins/rrt_dubins.py @@ -6,23 +6,17 @@ author: AtsushiSakai(@Atsushi_twi) """ import copy import math -import os import random -import sys - -import matplotlib.pyplot as plt import numpy as np +import matplotlib.pyplot as plt +import sys +import pathlib +sys.path.append(str(pathlib.Path(__file__).parent.parent.parent)) # root dir +sys.path.append(str(pathlib.Path(__file__).parent.parent)) -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../DubinsPath/") -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../RRT/") - -try: - from rrt import RRT - import dubins_path_planner -except ImportError: - raise +from RRT.rrt import RRT +from DubinsPath import dubins_path_planner +from utils.plot import plot_arrow show_animation = True @@ -130,10 +124,8 @@ class RRTDubins(RRT): plt.pause(0.01) def plot_start_goal_arrow(self): # pragma: no cover - dubins_path_planner.plot_arrow( - self.start.x, self.start.y, self.start.yaw) - dubins_path_planner.plot_arrow( - self.end.x, self.end.y, self.end.yaw) + plot_arrow(self.start.x, self.start.y, self.start.yaw) + plot_arrow(self.end.x, self.end.y, self.end.yaw) def steer(self, from_node, to_node): @@ -214,6 +206,7 @@ class RRTDubins(RRT): def main(): + print("Start " + __file__) # ====Search Path with RRT==== obstacleList = [ diff --git a/PathPlanning/RRTStar/rrt_star.py b/PathPlanning/RRTStar/rrt_star.py index e2c05456..a7aad49d 100644 --- a/PathPlanning/RRTStar/rrt_star.py +++ b/PathPlanning/RRTStar/rrt_star.py @@ -7,17 +7,12 @@ author: Atsushi Sakai(@Atsushi_twi) """ import math -import os import sys - import matplotlib.pyplot as plt +import pathlib +sys.path.append(str(pathlib.Path(__file__).parent.parent)) -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../RRT/") - -try: - from rrt import RRT -except ImportError: - raise +from RRT.rrt import RRT show_animation = True @@ -59,6 +54,7 @@ class RRTStar(RRT): self.connect_circle_dist = connect_circle_dist self.goal_node = self.Node(goal[0], goal[1]) self.search_until_max_iter = search_until_max_iter + self.node_list = [] def planning(self, animation=True): """ diff --git a/PathPlanning/RRTStarDubins/rrt_star_dubins.py b/PathPlanning/RRTStarDubins/rrt_star_dubins.py index e2a456d7..7c52879b 100644 --- a/PathPlanning/RRTStarDubins/rrt_star_dubins.py +++ b/PathPlanning/RRTStarDubins/rrt_star_dubins.py @@ -4,26 +4,19 @@ Path planning Sample Code with RRT and Dubins path author: AtsushiSakai(@Atsushi_twi) """ - import copy import math -import os import random -import sys - import matplotlib.pyplot as plt import numpy as np +import sys +import pathlib +sys.path.append(str(pathlib.Path(__file__).parent.parent.parent)) # root dir +sys.path.append(str(pathlib.Path(__file__).parent.parent)) -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../DubinsPath/") -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../RRTStar/") - -try: - import dubins_path_planner - from rrt_star import RRTStar -except ImportError: - raise +from DubinsPath import dubins_path_planner +from RRTStar.rrt_star import RRTStar +from utils.plot import plot_arrow show_animation = True @@ -136,10 +129,8 @@ class RRTStarDubins(RRTStar): plt.pause(0.01) def plot_start_goal_arrow(self): - dubins_path_planner.plot_arrow( - self.start.x, self.start.y, self.start.yaw) - dubins_path_planner.plot_arrow( - self.end.x, self.end.y, self.end.yaw) + plot_arrow(self.start.x, self.start.y, self.start.yaw) + plot_arrow(self.end.x, self.end.y, self.end.yaw) def steer(self, from_node, to_node): diff --git a/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py b/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py index 323bc76b..66185017 100644 --- a/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py +++ b/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py @@ -7,23 +7,15 @@ author: AtsushiSakai(@Atsushi_twi) """ import copy import math -import os import random import sys - +import pathlib import matplotlib.pyplot as plt import numpy as np +sys.path.append(str(pathlib.Path(__file__).parent.parent)) -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../ReedsSheppPath/") -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../RRTStar/") - -try: - import reeds_shepp_path_planning - from rrt_star import RRTStar -except ImportError: - raise +from ReedsSheppPath import reeds_shepp_path_planning +from RRTStar.rrt_star import RRTStar show_animation = True diff --git a/PathPlanning/StateLatticePlanner/state_lattice_planner.py b/PathPlanning/StateLatticePlanner/state_lattice_planner.py index b5fec697..fb80e1f5 100644 --- a/PathPlanning/StateLatticePlanner/state_lattice_planner.py +++ b/PathPlanning/StateLatticePlanner/state_lattice_planner.py @@ -4,11 +4,16 @@ State lattice planner with model predictive trajectory generator author: Atsushi Sakai (@Atsushi_twi) -- lookuptable.csv is generated with this script: https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/ModelPredictiveTrajectoryGenerator/lookuptable_generator.py +- lookuptable.csv is generated with this script: +https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning +/ModelPredictiveTrajectoryGenerator/lookuptable_generator.py Ref: -- State Space Sampling of Feasible Motions for High-Performance Mobile Robot Navigation in Complex Environments http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.187.8210&rep=rep1&type=pdf +- State Space Sampling of Feasible Motions for High-Performance Mobile Robot +Navigation in Complex Environments +http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.187.8210&rep=rep1 +&type=pdf """ import sys @@ -17,17 +22,11 @@ from matplotlib import pyplot as plt import numpy as np import math import pandas as pd +import pathlib +sys.path.append(str(pathlib.Path(__file__).parent.parent)) -sys.path.append(os.path.dirname(os.path.abspath(__file__)) - + "/../ModelPredictiveTrajectoryGenerator/") - - -try: - import model_predictive_trajectory_generator as planner - import motion_model -except ImportError: - raise - +import ModelPredictiveTrajectoryGenerator.trajectory_generator as planner +import ModelPredictiveTrajectoryGenerator.motion_model as motion_model table_path = os.path.dirname(os.path.abspath(__file__)) + "/lookuptable.csv" diff --git a/PathPlanning/VisibilityRoadMap/visibility_road_map.py b/PathPlanning/VisibilityRoadMap/visibility_road_map.py index 6458cd88..e9f8733c 100644 --- a/PathPlanning/VisibilityRoadMap/visibility_road_map.py +++ b/PathPlanning/VisibilityRoadMap/visibility_road_map.py @@ -6,17 +6,15 @@ author: Atsushi Sakai (@Atsushi_twi) """ -import os import sys import math import numpy as np import matplotlib.pyplot as plt +import pathlib +sys.path.append(str(pathlib.Path(__file__).parent.parent)) -from PathPlanning.VisibilityRoadMap.geometry import Geometry - -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../VoronoiRoadMap/") -from dijkstra_search import DijkstraSearch +from VisibilityRoadMap.geometry import Geometry +from VoronoiRoadMap.dijkstra_search import DijkstraSearch show_animation = True diff --git a/PathPlanning/VoronoiRoadMap/__init__.py b/PathPlanning/VoronoiRoadMap/__init__.py index 087dab64..e69de29b 100644 --- a/PathPlanning/VoronoiRoadMap/__init__.py +++ b/PathPlanning/VoronoiRoadMap/__init__.py @@ -1,4 +0,0 @@ -import os -import sys - -sys.path.append(os.path.dirname(os.path.abspath(__file__))) diff --git a/PathPlanning/VoronoiRoadMap/voronoi_road_map.py b/PathPlanning/VoronoiRoadMap/voronoi_road_map.py index d184230f..0a1f6f55 100644 --- a/PathPlanning/VoronoiRoadMap/voronoi_road_map.py +++ b/PathPlanning/VoronoiRoadMap/voronoi_road_map.py @@ -9,8 +9,12 @@ author: Atsushi Sakai (@Atsushi_twi) import math import numpy as np import matplotlib.pyplot as plt -from dijkstra_search import DijkstraSearch from scipy.spatial import cKDTree, Voronoi +import sys +import pathlib +sys.path.append(str(pathlib.Path(__file__).parent.parent)) + +from VoronoiRoadMap.dijkstra_search import DijkstraSearch show_animation = True 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 db5a7550..7e57abb8 100644 --- a/PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py +++ b/PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py @@ -7,19 +7,13 @@ author Atsushi Sakai (@Atsushi_twi) """ import math import sys -import os - import matplotlib.pyplot as plt import numpy as np import scipy.linalg as la +import pathlib +sys.path.append(str(pathlib.Path(__file__).parent.parent.parent)) -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../../PathPlanning/CubicSpline/") - -try: - import cubic_spline_planner -except ImportError: - raise +from PathPlanning.CubicSpline import cubic_spline_planner # === Parameters ===== diff --git a/PathTracking/lqr_steer_control/lqr_steer_control.py b/PathTracking/lqr_steer_control/lqr_steer_control.py index 5596ea0f..95a1b92c 100644 --- a/PathTracking/lqr_steer_control/lqr_steer_control.py +++ b/PathTracking/lqr_steer_control/lqr_steer_control.py @@ -10,16 +10,10 @@ import matplotlib.pyplot as plt import math import numpy as np import sys -import os - -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../../PathPlanning/CubicSpline/") - -try: - import cubic_spline_planner -except: - raise +import pathlib +sys.path.append(str(pathlib.Path(__file__).parent.parent.parent)) +from PathPlanning.CubicSpline import cubic_spline_planner Kp = 1.0 # speed proportional gain @@ -29,7 +23,7 @@ R = np.eye(1) # parameters dt = 0.1 # time tick[s] -L = 0.5 # Wheel base of the vehicle [m] +L = 0.5 # Wheelbase of the vehicle [m] max_steer = np.deg2rad(45.0) # maximum steering angle[rad] show_animation = True 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 ac8b883b..59e69aa9 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 @@ -10,16 +10,10 @@ import cvxpy import math import numpy as np import sys -import os - -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../../PathPlanning/CubicSpline/") - -try: - import cubic_spline_planner -except: - raise +import pathlib +sys.path.append(str(pathlib.Path(__file__).parent.parent.parent)) +from PathPlanning.CubicSpline import cubic_spline_planner NX = 4 # x = x, y, v, yaw NU = 2 # a = [accel, steer] diff --git a/PathTracking/stanley_controller/stanley_controller.py b/PathTracking/stanley_controller/stanley_controller.py index 3a67d626..9c475cba 100644 --- a/PathTracking/stanley_controller/stanley_controller.py +++ b/PathTracking/stanley_controller/stanley_controller.py @@ -12,16 +12,10 @@ Ref: import numpy as np import matplotlib.pyplot as plt import sys -import os - -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../../PathPlanning/CubicSpline/") - -try: - import cubic_spline_planner -except: - raise +import pathlib +sys.path.append(str(pathlib.Path(__file__).parent.parent.parent)) +from PathPlanning.CubicSpline import cubic_spline_planner k = 0.5 # control gain Kp = 1.0 # speed proportional gain diff --git a/SLAM/GraphBasedSLAM/graph_based_slam.py b/SLAM/GraphBasedSLAM/graph_based_slam.py index 0827732c..5510a411 100644 --- a/SLAM/GraphBasedSLAM/graph_based_slam.py +++ b/SLAM/GraphBasedSLAM/graph_based_slam.py @@ -11,8 +11,8 @@ Ref """ import sys -import os -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../utils/") +import pathlib +sys.path.append(str(pathlib.Path(__file__).parent.parent.parent)) import copy import itertools diff --git a/tests/test_LQR_planner.py b/tests/test_LQR_planner.py index b535b99b..be12195e 100644 --- a/tests/test_LQR_planner.py +++ b/tests/test_LQR_planner.py @@ -1,5 +1,5 @@ import conftest # Add root path to sys.path -from PathPlanning.LQRPlanner import LQRplanner as m +from PathPlanning.LQRPlanner import lqr_planner as m def test_1():