Merge pull request #44 from guilyx/feature/bidir-rrt-star

Feature: Bidirectional 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)
+        * [Bidirectional RRT*](#bidirectional-rrt)
         * [RRT*](#rrt-star)
         * [Informed RRT*](#informed-rrt)
     * [Path Tracking](#path-tracking)
@@ -131,6 +132,11 @@ Planning
 ![](src/simulations/path_planning/rrt_path_planning/rrt_search.gif)  
 Navigation  
 ![](src/simulations/path_planning/rrt_path_planning/rrt_navigate.gif)  
+#### Bidirectional RRT*
+Planning  
+![](src/simulations/path_planning/rrt_star_bidirectional_path_planning/rrt_star_bidirectional_search.gif)  
+Navigation  
+![](src/simulations/path_planning/rrt_star_bidirectional_path_planning/rrt_star_bidirectional_navigate.gif)  
 #### RRT*
 Planning  
 ![](src/simulations/path_planning/rrt_star_path_planning/rrt_star_search.gif)  

src/components/plan/rrt_star_bidirectional/rrt_star_bidirectional_path_planner.py

@@ -0,0 +1 @@
+[[0, 0], [3.306675098631942, -0.42947535541010623], [7.266890562011799, 0.7147543450073774], [11.45225925964133, 2.0049339764105047], [15.736776709072611, 1.820070331204639], [19.775857525204184, 2.8949775462759386], [23.628310262708975, 4.727371914446149], [26.68249838720731, 7.440849834320994], [30.129719004771033, 7.025608977845063], [32.249532731013176, 10.252281275576141], [34.708565711401114, 11.561115630367935], [38.42381732000173, 12.119439651270069], [42.328816945798835, 12.051681155695906], [45.12052411757108, 9.787285018594513], [43.63043184179346, 6.038420987999425], [44.44838919898779, 3.0431468916922384], [45.577856385209074, -1.0057366238705423], [46.87286792250144, -4.321069039069234], [46.48665294279576, -8.045120952334871], [50, -10]]

src/simulations/path_planning/rrt_star_bidirectional_path_planning/map.json

@@ -0,0 +1,111 @@
+"""
+rrt_star_bidirectional_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_bidirectional")
+
+
+# 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_bidirectional_path_planner import RrtStarBidirectionalPathPlanner
+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_bidirectional_path_planning/rrt_star_bidirectional_navigate.gif"
+    map_path = abs_dir_path + relative_simulations + "path_planning/rrt_star_bidirectional_path_planning/map.json"
+    path_filename = abs_dir_path + relative_simulations + "path_planning/rrt_star_bidirectional_path_planning/path.json"
+    search_gif_path = abs_dir_path + relative_simulations + "path_planning/rrt_star_bidirectional_path_planning/rrt_star_bidirectional_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 = RrtStarBidirectionalPathPlanner((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_bidirectional_path_planning/path.json

@@ -0,0 +1,18 @@
+"""
+Test of Bidirectional 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_bidirectional_path_planning")
+import rrt_star_bidirectional_path_planning
+
+
+def test_simulation():
+    rrt_star_bidirectional_path_planning.show_plot = False
+
+    rrt_star_bidirectional_path_planning.main()