diff --git a/planning/__init__.py b/planning/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/planning/planning_tuning_tools/scripts/astar_tuning_tools/.gitignore b/planning/planning_tuning_tools/scripts/astar_tuning_tools/.gitignore index da392e7a..01a4f64c 100644 --- a/planning/planning_tuning_tools/scripts/astar_tuning_tools/.gitignore +++ b/planning/planning_tuning_tools/scripts/astar_tuning_tools/.gitignore @@ -1,2 +1,3 @@ result/ costmap/ +*.egg-info/ diff --git a/planning/planning_tuning_tools/scripts/astar_tuning_tools/README.md b/planning/planning_tuning_tools/scripts/astar_tuning_tools/README.md index 2ed2c376..289557d0 100644 --- a/planning/planning_tuning_tools/scripts/astar_tuning_tools/README.md +++ b/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 diff --git a/planning/planning_tuning_tools/scripts/astar_tuning_tools/common/__init__.py b/planning/planning_tuning_tools/scripts/astar_tuning_tools/common/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/planning/planning_tuning_tools/scripts/astar_tuning_tools/common/common_classes.py b/planning/planning_tuning_tools/scripts/astar_tuning_tools/common/common_classes.py index c855167a..6fa3a4a0 100644 --- a/planning/planning_tuning_tools/scripts/astar_tuning_tools/common/common_classes.py +++ b/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 diff --git a/planning/planning_tuning_tools/scripts/astar_tuning_tools/param/__init__.py b/planning/planning_tuning_tools/scripts/astar_tuning_tools/param/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/planning/planning_tuning_tools/scripts/astar_tuning_tools/search_gridgoal.py b/planning/planning_tuning_tools/scripts/astar_tuning_tools/search_gridgoal.py index 031ac566..c2eecb8c 100644 --- a/planning/planning_tuning_tools/scripts/astar_tuning_tools/search_gridgoal.py +++ b/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) diff --git a/planning/planning_tuning_tools/scripts/astar_tuning_tools/setup.py b/planning/planning_tuning_tools/scripts/astar_tuning_tools/setup.py new file mode 100644 index 00000000..7d6db5c7 --- /dev/null +++ b/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()) diff --git a/planning/planning_tuning_tools/scripts/astar_tuning_tools/visualize_trajectories.py b/planning/planning_tuning_tools/scripts/astar_tuning_tools/visualize_trajectories.py index e8bf0db7..772a6c67 100644 --- a/planning/planning_tuning_tools/scripts/astar_tuning_tools/visualize_trajectories.py +++ b/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 = []