modularize and some modification

Signed-off-by: Takumi Ito <[email protected]>

planning/planning_tuning_tools/scripts/astar_tuning_tools/.gitignore

@@ -1,2 +1,3 @@
 result/
 costmap/
+*.egg-info/

planning/planning_tuning_tools/scripts/astar_tuning_tools/README.md

@@ -14,11 +14,22 @@
    source install/setup.bash
    ```
 
+3. Move to this directory and run
+
+   ```sh
+   pip3 install .
+   ```
+
 ## Costmap generation
 
 ### Usage
 
-1. Run the script `python3 generate_costmap.py --save_name [filename]`
+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
@@ -70,7 +81,12 @@ Search for goals on grid: discretized on x, y, yaw axes.
 
 ### Usage
 
-1. Run the script `python3 visualize_trajectories.py --dir_name [save dir name]`
+1. Run the script
+
+   ```sh
+   python3 visualize_trajectories.py --dir_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

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

@@ -7,9 +7,9 @@ def __init__(self, costmap, xs, ys, yaws):
 
 
 class Result:
-    def __init__(self, x, y, yaw, find, trajectory):
+    def __init__(self, x, y, yaw, find, waypoints):
         self.xs = x
         self.ys = y
         self.yaws = yaw
         self.find = find
-        self.trajectory = trajectory
+        self.waypoints = waypoints

planning/planning_tuning_tools/scripts/astar_tuning_tools/search_gridgoal.py

@@ -18,8 +18,6 @@
 import pickle
 import time
 
-from autoware_auto_planning_msgs.msg import Trajectory
-from autoware_auto_planning_msgs.msg import TrajectoryPoint
 from common.common_classes import Result
 from common.common_classes import SearchInfo
 import freespace_planning_algorithms.astar_search as fp
@@ -46,16 +44,6 @@ def float_range(start, end, step):
     return f_range
 
 
-def createTrajectory(waypoints):
-    trajectory = Trajectory()
-    for waypoint in waypoints.waypoints:
-        trajectory_point = TrajectoryPoint()
-        trajectory_point.pose = waypoint
-        trajectory.points.append(trajectory_point)
-
-    return trajectory
-
-
 if __name__ == "__main__":
     parser = argparse.ArgumentParser(description="place of save result")
     parser.add_argument(
@@ -139,12 +127,11 @@ def createTrajectory(waypoints):
                         goal_pose.orientation.z = quaterinon.z
 
                         find = astar.makePlan(start_pose, goal_pose)
-                        trajectory = []
+                        waypoints = fp.PlannerWaypoints()
                         if find:
                             waypoints = astar.getWaypoints()
-                            trajectory = createTrajectory(waypoints)
 
-                        result = Result(x, y, yaw, find, trajectory)
+                        result = Result(x, y, yaw, find, waypoints)
 
                         pickle.dump(result, f)
 

planning/planning_tuning_tools/scripts/astar_tuning_tools/setup.py

@@ -0,0 +1,4 @@
+from setuptools import find_packages
+from setuptools import setup
+
+setup(name="astar_tuning_tools", version="1.0.0", packages=find_packages())

planning/planning_tuning_tools/scripts/astar_tuning_tools/visualize_trajectories.py

@@ -19,6 +19,7 @@
 import sys
 
 from autoware_auto_planning_msgs.msg import Trajectory
+from autoware_auto_planning_msgs.msg import TrajectoryPoint
 import matplotlib.colors as mcolors
 import matplotlib.pyplot as plt
 import numpy as np
@@ -32,6 +33,16 @@
 COLOR_KEYS = list(TAB_COLORS.keys())
 
 
+def createTrajectory(waypoints):
+    trajectory = Trajectory()
+    for waypoint in waypoints.waypoints:
+        trajectory_point = TrajectoryPoint()
+        trajectory_point.pose = waypoint.pose
+        trajectory.points.append(trajectory_point)
+
+    return trajectory
+
+
 class DrawClickedTrajectory:
     def __init__(self, fig, xs, ys, yaws, trajectories):
         self.press_point = None
@@ -180,7 +191,7 @@ def drawnow(self):
                 with open(filename, "rb") as f:
                     result = pickle.load(f)
                     results[k][i][j] = result.find
-                    trajectories[k][i][j] = result.trajectory
+                    trajectories[k][i][j] = createTrajectory(result.waypoints)
 
     # detect obstacle
     obstacle_x = []