feat: astar prameter tuning tool

planning/planning_tuning_tools/scripts/astar_tuning_tools/.gitignore

@@ -0,0 +1,5 @@
+result/
+costmap/
+opt_param/
+*.egg-info/
+build/

planning/planning_tuning_tools/scripts/astar_tuning_tools/README.md

@@ -0,0 +1,141 @@
+# Parameter tuning tools for freespace planning algorithms
+
+## Setup
+
+1. Move to the target autoware directory and build the package
+
+   ```sh
+   colcon build --packages-up-to autoware_freespace_planning_algorithms --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release
+   ```
+
+2. Source the setup srcript
+
+   ```sh
+   source install/setup.bash
+   ```
+
+3. Move to this directory and run
+
+   ```sh
+   pip3 install .
+   ```
+
+## Quick start
+
+1. Select the parameters which is used for the optimization in the [configuration file](config/optimization_config.yaml).
+
+2. Run the bash file with an arbitrary file name.
+
+   ```sh
+   ./optimization_operator.sh costmap_default [file name]
+   ```
+
+3. You can find the optimized parameter at `opt_param/[file name].yaml`. Substitude the config parameters of freespace planner (in your autoware) into the optimized parameter manually.
+
+Default costmap used for the optimization is below:
+![default costmap](figs/costmap_default.png)
+
+## Optimization
+
+### Usage
+
+1. Select the parameters which is used for the optimization in the [configuration file](config/optimization_config.yaml).
+
+2. Run the bash file with an arbitrary file name.
+
+   ```sh
+   ./optimization_operator.sh [costmap name] [file name]
+   ```
+
+3. You can find the optimized parameter at `opt_param/[file name].yaml`. Substitude the config parameters of freespace planner (in your autoware) into the optimized parameter manually.
+
+### Options
+
+| Option         | Description                                                          | Defualt value   |
+| -------------- | -------------------------------------------------------------------- | --------------- |
+| `costmap name` | costmap name to use for optimization, generated in the next section. | costmap_default |
+| `file name`    | directory name to save the results                                   | test            |
+
+## Costmap generation
+
+### Usage
+
+1. Run the script
+
+   ```sh
+   python3 generate_costmap.py --save_name [filename]
+   ```
+
+2. Then the GUI shows up
+   - Drag and drop to put an obstacle
+   - Drag and drop the same points again to remove the obstacle
+   - Push "Generate Costmap" button to save the costmap to `costmap/filename.txt`
+3. Close the window to finish it
+
+![GUI of costmap generation function](figs/costmap.png)
+
+### Options
+
+| Option         | Type   | Description                   | Defualt value     |
+| -------------- | ------ | ----------------------------- | ----------------- |
+| `--save_name`  | string | file name to save the costmap | costmap_generated |
+| `--height`     | int    | height of the costmap         | 350               |
+| `--width`      | int    | width of the costmap          | 350               |
+| `--resolution` | float  | resolution of the costmap     | 0.2               |
+
+## Search for trajectory to goal on grid
+
+Search for goals on grid: discretized on x, y, yaw axes.
+
+### Usage
+
+1. Run the script
+
+   ```sh
+   python3 search.py --save_name [save dir name]
+   ```
+
+   or execute in parallel using [eos run-many](https://github.com/iory/eos/blob/master/eos/run_many.py)
+
+   ```sh
+   run-many 'python3 search.py --save_name [save dir name]' -n 10 -j 10
+   ```
+
+2. Then the trajectories for goals on grid are searched and saved
+
+### Options
+
+| Option           | Type   | Description                                            | Defualt value   |
+| ---------------- | ------ | ------------------------------------------------------ | --------------- |
+| `--save_name`    | string | directory name to save the results                     | default_dir     |
+| `--costmap`      | string | costmap name for search, generated in previous section | costmap_default |
+| `--x_resolution` | float  | goal grid sersolution of the x axis [m]                | 1.0             |
+| `--y_resolution` | float  | goal grid sersolution of the y axis [m]                | 1.0             |
+| `--yaw_discrete` | int    | goal discretize number of yaw                          | 10              |
+
+## Visualize the searched results and the trajectories
+
+### Usage
+
+1. Run the script
+
+   ```sh
+   python3 visualize_trajectories.py --save_name [save dir name]
+   ```
+
+2. Then the two GUIs show up
+   - Viewer of the searched trajectories
+     - Drag and drop to put a goal in specified direction
+     - Drag and drop the same point and direction again to remove the goal
+     - Then the trajectories for put goal are shown
+       ![visualization of searched trajectories](figs/trajectories.png)
+   - Viewer of the success or failure of the search
+     - White, red, and black cells indicate success, fail, and obstacle cell, respectively.
+       ![visualization of search results](figs/results.png)
+3. Close the windows to finish it
+
+### Options
+
+| Option        | Type   | Description                                                       | Defualt value |
+| ------------- | ------ | ----------------------------------------------------------------- | ------------- |
+| `--save_name` | string | directory name to save the results, generated in previous section | default_dir   |

planning/planning_tuning_tools/scripts/astar_tuning_tools/common/common_classes.py

@@ -0,0 +1,17 @@
+class SearchInfo:
+    def __init__(self, costmap, xs, ys, yaws):
+        self.costmap = costmap
+        self.xs = xs
+        self.ys = ys
+        self.yaws = yaws
+
+
+class Result:
+    def __init__(self, x, y, yaw, find, waypoints, distance_to_obstacles, steerings):
+        self.xs = x
+        self.ys = y
+        self.yaws = yaw
+        self.find = find
+        self.waypoints = waypoints
+        self.distance_to_obstacles = distance_to_obstacles
+        self.steerings = steerings

planning/planning_tuning_tools/scripts/astar_tuning_tools/common/common_functions.py

@@ -0,0 +1,23 @@
+from config.astar_params import astar_param
+from config.astar_params import planner_param
+import numpy as np
+
+
+def param_vector2obj(vector):
+    planner_param.curve_weight = vector[0]
+    planner_param.reverse_weight = vector[1]
+
+    # -- A* search Configurations --
+    astar_param.distance_heuristic_weight = vector[2]
+
+    return planner_param, astar_param
+
+
+def param_obj2vector(planner_param_, astar_param_):
+    vector = np.zeros(7)
+    vector[0] = planner_param_.curve_weight
+    vector[1] = planner_param_.reverse_weight
+
+    # -- A* search Configurations --
+    vector[2] = astar_param_.distance_heuristic_weight
+    return vector

planning/planning_tuning_tools/scripts/astar_tuning_tools/config/astar_params.py

@@ -0,0 +1,39 @@
+import autoware_freespace_planning_algorithms.astar_search as fp
+
+# -- Vehicle Shape --
+vehicle_shape = fp.VehicleShape()
+vehicle_shape.length = 4.89
+vehicle_shape.width = 1.896
+vehicle_shape.base_length = 2.79
+vehicle_shape.max_steering = 0.7
+vehicle_shape.base2back = 1.1
+vehicle_shape.setMinMaxDimension()
+
+# -- Planner Common Parameter --
+planner_param = fp.PlannerCommonParam()
+# base configs
+planner_param.time_limit = 30000.0
+planner_param.max_turning_ratio = 0.5
+planner_param.turning_steps = 1
+# search configs
+planner_param.theta_size = 144
+planner_param.angle_goal_range = 6.0
+planner_param.lateral_goal_range = 0.5
+planner_param.longitudinal_goal_range = 2.0
+planner_param.curve_weight = 0.5
+planner_param.reverse_weight = 1.0
+planner_param.direction_change_weight = 1.5
+# costmap configs
+planner_param.obstacle_threshold = 100
+
+
+# -- A* search Configurations --
+astar_param = fp.AstarParam()
+astar_param.search_method = "forward"  # options: forward, backward
+astar_param.only_behind_solutions = False
+astar_param.use_back = True
+astar_param.adapt_expansion_distance = True
+astar_param.expansion_distance = 0.5
+astar_param.distance_heuristic_weight = 1.5
+astar_param.smoothness_weight = 0.5
+astar_param.obstacle_distance_weight = 1.5