Merge pull request #43 from guilyx/feature/rrt-starr

Feature: RRT* Path Planning

Author: ShisatoYano

README.md

@@ -27,6 +27,7 @@ Python sample codes and documents about Autonomous vehicle control algorithm. Th
         * [Hybrid A*](#hybrid-a)
         * [Dijkstra](#dijkstra)
         * [RRT](#rrt)
+        * [RRT*](#rrt-star)
         * [Informed RRT*](#informed-rrt)
     * [Path Tracking](#path-tracking)
         * [Pure pursuit Path Tracking](#pure-pursuit-path-tracking)
@@ -129,6 +130,11 @@ Planning
 ![](src/simulations/path_planning/rrt_path_planning/rrt_search.gif)  
 Navigation  
 ![](src/simulations/path_planning/rrt_path_planning/rrt_navigate.gif)  
+#### RRT*
+Planning  
+![](src/simulations/path_planning/rrt_star_path_planning/rrt_star_search.gif)  
+Navigation  
+![](src/simulations/path_planning/rrt_star_path_planning/rrt_star_navigate.gif)  
 #### Informed RRT*
 Planning  
 ![](src/simulations/path_planning/informed_rrt_star_path_planning/informed_rrt_star_search.gif)  

src/components/plan/rrt_star/rrt_star_path_planner.py

@@ -0,0 +1 @@
+[[0, 0], [4.75087218654987, 0.07895810741711168], [9.582869356011766, -1.0506567611604611], [14.114352936818678, 0.6439502740755565], [18.67372261735205, -0.006304262910310179], [21.34144317754782, -2.3197552417594336], [25.846605971495077, -4.388931138778045], [29.950736447698546, -4.04278169288967], [33.86200715713186, -2.499180139597338], [34.4037782089103, 2.249546008168026], [35.537387898648554, 7.175364663221906], [36.955413401808045, 12.982359157562843], [41.1508007268567, 15.033334469141959], [45.32268614027619, 15.769149518629249], [45.605148069872165, 10.080477147028189], [48.10681315024125, 6.434419004985622], [50.769896921715016, 2.5701130714834957], [50.34038694284904, -1.4179024281044705], [50.0881233442712, -5.288258291790932], [50, -10]]

src/simulations/path_planning/rrt_star_path_planning/map.json

@@ -0,0 +1,111 @@
+"""
+rrt_star_path_planning.py
+
+Author: Auto-generated
+"""
+
+# 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_star")
+
+
+# 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_star_path_planner import RrtStarPathPlanner
+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_star_path_planning/rrt_star_navigate.gif"
+    map_path = abs_dir_path + relative_simulations + "path_planning/rrt_star_path_planning/map.json"
+    path_filename = abs_dir_path + relative_simulations + "path_planning/rrt_star_path_planning/path.json"
+    search_gif_path = abs_dir_path + relative_simulations + "path_planning/rrt_star_path_planning/rrt_star_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 = RrtStarPathPlanner((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_star_path_planning/path.json

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