Merge pull request #38 from Palamanickam0806/rrt_path_planning

rrt path planning algorithm

Author: ShisatoYano

.gitignore

@@ -7098,3 +7098,4 @@ __pycache__
 .venv/share/python-wheels/urllib3-1.25.8-py2.py3-none-any.whl
 .venv/share/python-wheels/webencodings-0.5.1-py2.py3-none-any.whl
 .venv/share/python-wheels/wheel-0.34.2-py2.py3-none-any.whl
+.vscode/

src/components/plan/rrt/rrt_path_planner.py

@@ -0,0 +1 @@
+[[0, 0], [4.653604726991527, -0.3069193482143218], [9.308085271322161, -0.621448790726503], [13.399959986759717, 0.7869486899441993], [18.216905792229884, 0.22190827834104482], [21.35044077424777, -2.302664336350134], [24.67774179300573, -4.601488021668669], [29.584184813919446, -4.389205021123191], [33.6923759578744, -3.740362509028831], [36.09416006667666, -1.0473590046996233], [33.92497913721241, 2.2270499885152284], [35.843000517065626, 6.007109716507263], [35.67058203354173, 9.98031077869816], [39.067352218559435, 12.412638156746326], [42.8433452245675, 12.3976922443666], [46.090624978894844, 9.213270986173129], [48.10511036141835, 4.9405372754510815], [48.16924620001407, 0.3902405031486628], [48.821109642260396, -4.461920188781559], [50, -10]]

src/simulations/path_planning/rrt_path_planning/map.json

@@ -0,0 +1,111 @@
+"""
+astar_path_planning.py
+
+Author: Shantanu Parab
+"""
+
+# import path setting
+import numpy as np
+import sys
+from pathlib import Path
+
+abs_dir_path = str(Path(__file__).absolute().parent)
+relative_path = "/../../../components/"
+relative_simulations = "/../../../simulations/"
+
+
+sys.path.append(abs_dir_path + relative_path + "visualization")
+sys.path.append(abs_dir_path + relative_path + "state")
+sys.path.append(abs_dir_path + relative_path + "vehicle")
+sys.path.append(abs_dir_path + relative_path + "obstacle")
+sys.path.append(abs_dir_path + relative_path + "mapping/grid")
+sys.path.append(abs_dir_path + relative_path + "course/cubic_spline_course")
+sys.path.append(abs_dir_path + relative_path + "control/pure_pursuit")
+sys.path.append(abs_dir_path + relative_path + "plan/rrt")
+
+
+# import component modules
+from global_xy_visualizer import GlobalXYVisualizer
+from min_max import MinMax
+from time_parameters import TimeParameters
+from vehicle_specification import VehicleSpecification
+from state import State
+from four_wheels_vehicle import FourWheelsVehicle
+from obstacle import Obstacle
+from obstacle_list import ObstacleList
+from cubic_spline_course import CubicSplineCourse
+from pure_pursuit_controller import PurePursuitController
+from rrt_path_planner import RrtPathPlanner
+from binary_occupancy_grid import BinaryOccupancyGrid
+import json
+
+
+# flag to show plot figure
+# when executed as unit test, this flag is set as false
+show_plot = True
+
+
+def main():
+    """
+    Main process function
+    """
+
+    # set simulation parameters
+    x_lim, y_lim = MinMax(-5, 55), MinMax(-20, 25)
+    navigation_gif_path = abs_dir_path + relative_simulations + "path_planning/rrt_path_planning/rrt_navigate.gif"
+    map_path = abs_dir_path + relative_simulations + "path_planning/rrt_path_planning/map.json"
+    path_filename = abs_dir_path + relative_simulations + "path_planning/rrt_path_planning/path.json"
+    search_gif_path = abs_dir_path + relative_simulations + "path_planning/rrt_path_planning/rrt_search.gif"
+
+
+    vis = GlobalXYVisualizer(x_lim, y_lim, TimeParameters(span_sec=25), show_zoom=False, gif_name=navigation_gif_path)
+    occ_grid = BinaryOccupancyGrid(x_lim, y_lim, resolution=0.5, clearance=1.5, map_path=map_path)
+
+    obst_list = ObstacleList()
+    obst_list.add_obstacle(Obstacle(State(x_m=10.0, y_m=15.0), length_m=10, width_m=8))
+    obst_list.add_obstacle(Obstacle(State(x_m=40.0, y_m=0.0), length_m=2, width_m=10))
+    obst_list.add_obstacle(Obstacle(State(x_m=10.0, y_m=-10.0, yaw_rad=np.rad2deg(45)), length_m=5, width_m=5))
+    obst_list.add_obstacle(Obstacle(State(x_m=30.0, y_m=15.0, yaw_rad=np.rad2deg(10)), length_m=5, width_m=2))
+    obst_list.add_obstacle(Obstacle(State(x_m=50.0, y_m=15.0, yaw_rad=np.rad2deg(15)), length_m=5, width_m=2))
+    obst_list.add_obstacle(Obstacle(State(x_m=25.0, y_m=0.0), length_m=2, width_m=2))
+    obst_list.add_obstacle(Obstacle(State(x_m=35.0, y_m=-15.0), length_m=7, width_m=2))
+
+    vis.add_object(obst_list)
+    occ_grid.add_object(obst_list)
+    occ_grid.save_map()
+    # Easy Goal = (50,22)
+    # Hard Goal = (50,-10)
+    planner = RrtPathPlanner((0, 0), (50, -10), map_path, x_lim=x_lim, y_lim=y_lim, path_filename=path_filename, gif_name=search_gif_path,
+                             max_iterations=5000,step_size=0.5,goal_sample_rate=0.05)
+
+    # Load sparse path from json file
+    with open(path_filename, 'r') as f:
+        sparse_path = json.load(f)
+
+    # Extract x and y coordinates
+    sparse_x = [point[0] for point in sparse_path]
+    sparse_y = [point[1] for point in sparse_path]
+
+    # Use with CubicSplineCourse
+    course = CubicSplineCourse(sparse_x, sparse_y, 20)
+    vis.add_object(course)
+
+    # create vehicle instance
+    spec = VehicleSpecification()
+    pure_pursuit = PurePursuitController(spec, course)
+    vehicle = FourWheelsVehicle(State(color=spec.color), spec,
+                                controller=pure_pursuit,
+                                show_zoom=False)
+
+    vis.add_object(vehicle)
+
+    # plot figure is not shown when executed as unit test
+    if not show_plot: vis.not_show_plot()
+
+    # show plot figure
+    vis.draw()
+
+
+# execute main process
+if __name__ == "__main__":
+    main()

src/simulations/path_planning/rrt_path_planning/path.json

@@ -0,0 +1,18 @@
+"""
+Test of A* path planning and navigation simulation
+
+Author: Shisato Yano
+"""
+
+from pathlib import Path
+import sys
+import pytest
+
+sys.path.append(str(Path(__file__).absolute().parent) + "/../src/simulations/path_planning/rrt_path_planning")
+import rrt_path_planning
+
+
+def test_simulation():
+    rrt_path_planning.show_plot = False
+
+    rrt_path_planning.main()