diff --git a/GEMstack/onboard/perception/parking_detection.py b/GEMstack/onboard/perception/parking_detection.py index 94ff48e30..5e8133945 100644 --- a/GEMstack/onboard/perception/parking_detection.py +++ b/GEMstack/onboard/perception/parking_detection.py @@ -3,7 +3,7 @@ from sensor_msgs.msg import PointCloud2 from typing import Dict from ..component import Component -from ...state import AgentState, ObjectPose, ObjectFrameEnum, Obstacle, ObstacleMaterialEnum +from ...state import AgentState, ObjectPose, ObjectFrameEnum, Obstacle, ObstacleMaterialEnum, VehicleState, AllState from ..interface.gem import GEMInterface from .utils.detection_utils import * from .utils.parking_utils import * @@ -135,12 +135,8 @@ def update(self, agents: Dict[str, AgentState]): cone_pts_3D.append(cone_pt_3D) # Detect parking spot - if len(cone_pts_3D) == 4: + if len(cone_pts_3D) >= 4: goal_parking_spot, parking_obstacles_pose, parking_obstacles_dim, ordered_cone_ground_centers_2D = detect_parking_spot(cone_pts_3D) - elif len(cone_pts_3D) > 4 and len(cone_pts_3D) % 2 == 0: - ordered_centroids = sorted(cone_pts_3D, key=lambda x: (x[0])) - ordered_centroids = [ordered_centroids[i] for i in [0, 1, 2, 4]] - goal_parking_spot, parking_obstacles_pose, parking_obstacles_dim, ordered_cone_ground_centers_2D = detect_parking_spot(ordered_centroids) # Update local variables for visualization self.cone_pts_3D = cone_pts_3D self.ordered_cone_ground_centers_2D = ordered_cone_ground_centers_2D @@ -166,7 +162,7 @@ def update(self, agents: Dict[str, AgentState]): yaw=yaw, pitch=0.0, roll=0.0, - frame=ObjectFrameEnum.CURRENT + frame=ObjectFrameEnum.START ) new_obstacle = Obstacle( pose=obstacle_pose, @@ -193,4 +189,79 @@ def update(self, agents: Dict[str, AgentState]): ) new_state = [goal_pose, parking_obstacles] - return new_state \ No newline at end of file + return new_state + + +class FakeParkingSpaceDetector(Component): + def __init__(self, vehicle_interface: GEMInterface): + self.vehicle_interface = vehicle_interface + + # Hard-coded goal position for the parking spot + self.goal_parking_spot = (8.0, 20.0, 0.0) # (x, y, yaw) + + # Hard-coded obstacles blocking the side paths only (not the mouth of the parking) + self.parking_obstacles_pose = [ + (6.0, 18.0, 0.0, 0.0), # Left obstacle (x, y, z, yaw) + (10.0, 18.0, 0.0, 0.0) # Right obstacle (x, y, z, yaw) + ] + + # Obstacle dimensions (width, length, height) + self.parking_obstacles_dim = [ + [0.5, 0.5, 1.0], + [0.5, 0.5, 1.0] + ] + + def rate(self): + return 4.0 + + def state_inputs(self): + return ['agents'] + + def state_outputs(self): + return ['goal', 'obstacles'] + + def update(self, state: AllState): + # Current timestamp + current_time = self.vehicle_interface.time() + + # Constructing goal pose + x, y, yaw = self.goal_parking_spot + goal_pose = ObjectPose( + t=current_time, + x=x, + y=y, + z=0.0, + yaw=yaw, + pitch=0.0, + roll=0.0, + frame=ObjectFrameEnum.CURRENT + ) + + # Constructing obstacles for only the two side walls + obstacle_id = 0 + parking_obstacles = {} + for o_pose, o_dim in zip(self.parking_obstacles_pose, self.parking_obstacles_dim): + x, y, z, yaw = o_pose + obstacle_pose = ObjectPose( + t=current_time, + x=x, + y=y, + z=z, + yaw=yaw, + pitch=0.0, + roll=0.0, + frame=ObjectFrameEnum.START + ) + new_obstacle = Obstacle( + pose=obstacle_pose, + dimensions=o_dim, + outline=None, + material=ObstacleMaterialEnum.BARRIER, + collidable=True + ) + parking_obstacles[f"parking_obstacle_{obstacle_id}"] = new_obstacle + obstacle_id += 1 + + # Returning the hard-coded goal and obstacles + new_state = [goal_pose, parking_obstacles] + return new_state diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index f740ebaf0..9a156d9ae 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -4,7 +4,7 @@ from ..component import Component from ...state import AllState, VehicleState, EntityRelation, EntityRelationEnum, Path, Trajectory, Route, \ ObjectFrameEnum -from ...utils import serialization +from ...utils import serialization, settings from ...mathutils import transforms import numpy as np @@ -378,10 +378,10 @@ class YieldTrajectoryPlanner(Component): def __init__(self): self.route_progress = None self.t_last = None - self.acceleration = 5 - self.desired_speed = 2.0 - self.deceleration = 2.0 - self.emergency_brake = 8.0 + self.acceleration = settings.get("run.drive.planning.motion_planning.largs.acceleration", 5) + self.desired_speed = settings.get("run.drive.planning.motion_planning.largs.desired_speed",2.0) + self.deceleration = settings.get("run.drive.planning.motion_planning.largs.deceleration", 2.0) + self.emergency_brake = settings.get("run.drive.planning.motion_planning.largs.emergency_brake", 8.0) def state_inputs(self): return ['all'] diff --git a/GEMstack/onboard/planning/parking_route_planner.py b/GEMstack/onboard/planning/parking_route_planner.py index 886f2b16b..9d4a6e56e 100644 --- a/GEMstack/onboard/planning/parking_route_planner.py +++ b/GEMstack/onboard/planning/parking_route_planner.py @@ -220,10 +220,12 @@ def __init__(self, vehicle=None, obstacles=None, actions=None): self._vehicle = None # settings - self.T = settings.get("planning.dubins.integrator.time_step", 1.5) - self.dt = settings.get("planning.dubins.integrator.time_step", .25) - self.max_v = settings.get("planning.dubins.actions.max_velocity", 0.75) - self.max_turn_rate = settings.get("planning.dubins.actions.max_turn_rate", 0.3) + self.T = settings.get("run.drive.planning.route_planning_component.dubins.integrator.time_step", 1.5) + self.dt = settings.get("run.drive.planning.route_planning_component.dubins.integrator.dt", .25) + self.max_v = settings.get("run.drive.planning.route_planning_component.dubins.actions.max_velocity", 0.75) + self.max_turn_rate = settings.get("run.drive.planning.route_planning_component.dubins.actions.max_turn_rate", 0.3) + self.tolerance = settings.get("run.drive.planning.route_planning_component.dubins.tolerance", 0.5) + self.heuristic_string = settings.get("run.drive.planning.route_planning_component.astar.heuristic", "reeds_shepp") self.vehicle = DubinsCar() #x = (tx,ty,theta) and u = (fwd_velocity,turnRate). self.vehicle_sim = DubinsCarIntegrator(self.vehicle, self.T, self.dt) @@ -260,19 +262,24 @@ def is_goal_reached(self, current: List[float], goal: List[float]): # @TODO Currently, the threshold is just a random number, get rid of magic constants # print(f"Current Pose: {current}") # print(f"Goal Pose: {goal}") - # if np.abs(current[3]) > 1: return False # car must be stopped, this equality will only work in simulation + # if np.abs(current[3]) > 1: return False # car must be stopped, this equality will only work in simulation np.linalg.norm(np.array([current[0], current[1]]) - np.array([goal[0], goal[1]])) < 0.5 + # if np.abs(current[3] - goal[3]) > .25: return False return np.linalg.norm(np.array([current[0], current[1]]) - np.array([goal[0], goal[1]])) < 0.5 def heuristic_cost_estimate(self, state_1, state_2): - """computes the 'direct' distance between two (x,y) tuples""" - # Extract position and orientation from states - (x1, y1, theta1, t) = state_1 - (x2, y2, theta2, t) = state_2 - - return self.reed_shepp(state_1, state_2) # Using turning radius of 1.0 + """run the correct heuristic function based on the heuristic_string""" - return self.approx_reeds_shepp(state_1, state_2) # Using turning radius of 1.0 - return math.hypot(x2 - x1, y2 - y1, theta2 - theta1) + if self.heuristic_string == "reeds_shepp": + return self.reed_shepp(state_1, state_2) # Using turning radius of 1.0 + + elif self.heuristic_string == "approx_reeds_shepp": + return self.approx_reeds_shepp(state_1, state_2) # Using turning radius of 1.0 + + elif self.heuristic_string == "euclidian": + return self.euclidian_cost_esimate(state_1, state_2) + + else: + raise Exception(f"Unknown heuristic function: {self.heuristic_string}") def euclidian_cost_esimate(self, state_1, state_2): """computes the 'direct' distance between two (x,y) tuples""" @@ -399,7 +406,7 @@ def state_to_object(self, state): theta = state[2] t = state[3] - pose = ObjectPose(frame=ObjectFrameEnum.ABSOLUTE_CARTESIAN,t=t, x=x,y=y,z=0,yaw=theta) + pose = ObjectPose(frame=ObjectFrameEnum.CURRENT,t=t, x=x,y=y,z=0,yaw=theta) temp_obj = PhysicalObject(pose=pose, dimensions=self.vehicle.to_object().dimensions, @@ -432,7 +439,7 @@ def __init__(self): # self.planner = ParkingSolverSecondOrderDubins() self.planner = ParkingSolverFirstOrderDubins() - self.iterations = settings.get("planning.astar.iterations", 20000) + self.iterations = settings.get("run.drive.planning.route_planning_component.astar.iterations", 50000) def state_inputs(self): return ['all'] @@ -452,6 +459,7 @@ def vehicle_state_to_second_order(self, vehicle_state: VehicleState) -> Tuple[fl Returns: Tuple[float, float]: _description_ """ + vehicle_state.pose = vehicle_state.pose.to_frame() x = vehicle_state.pose.x y = vehicle_state.pose.y theta = vehicle_state.pose.yaw # check that this is correct @@ -478,6 +486,22 @@ def vehicle_state_to_first_order(self, vehicle_state: VehicleState) -> Tuple[flo t = 0 return (x,y,theta,t) + + def pose_to_first_order(self, pose: ObjectPose) -> Tuple[float, float]: + """Takes a vehicle state and outputs the state of a second order dubins car + + Args: + vehicle_state (VehicleState): _description_ + + Returns: + Tuple[float, float]: _description_ + """ + x = pose.x + y = pose.y + theta = pose.yaw # check that this is correct + t = 0 + + return (x,y,theta,t) def update(self, state : AllState) -> Route: """_summary_ @@ -495,30 +519,36 @@ def update(self, state : AllState) -> Route: all_obstacles = {**agents, **obstacles} # print(f"Obstacles {obstacles}") print(f"Agents {agents}") - # goal= state.goal - # print(goal.frame) - # assert goal.frame == ObjectFrameEnum.ABSOLUTE_CARTESIAN - # assert goal.v == 0 - # print(f"Goal {goal}") - goal_pose = ObjectPose(frame=ObjectFrameEnum.ABSOLUTE_CARTESIAN, t=0.0, x=state.mission_plan.goal_x,y=state.mission_plan.goal_y,z=0.0,yaw=state.mission_plan.goal_orientation) - goal = VehicleState.zero() - goal.pose = goal_pose - goal.v = 0 + goal_pose = state.goal + assert goal_pose.frame == ObjectFrameEnum.CURRENT + print("Checking VALUE: ", state.start_vehicle_pose) + print("Checking VALUE: ", state.vehicle) + start_state = state.vehicle.to_frame(ObjectFrameEnum.CURRENT, current_pose = state.vehicle.pose,start_pose_abs = state.start_vehicle_pose) + # start_pose = state.vehicle.pose.to_frame(ObjectFrameEnum.CURRENT, current_pose = state.vehicle.pose,start_pose_abs = state.start_vehicle_pose) + # start_cur = VehicleState.zero() + # start_cur.pose = start_pose # goal.pose = goal_pose + # goal.v = 0 + print("Checking VALUE: ", start_state) + + + # # Need to parse and create second order dubin car states # start_state = self.vehicle_state_to_dynamics(vehicle) # goal_state = self.vehicle_state_to_dynamics(goal) # Need to parse and create second order dubin car states - start_state = self.vehicle_state_to_first_order(vehicle) - goal_state = self.vehicle_state_to_first_order(goal) + start = self.vehicle_state_to_first_order(start_state) + # goal = self.vehicle_state_to_first_order(goal_state) + goal = self.pose_to_first_order(goal_pose) # Update the planner self.planner.obstacles = list(all_obstacles.values()) self.planner.vehicle = vehicle # Compute the new trajectory and return it - res = list(self.planner.astar(start_state, goal_state, reversePath=False, iterations=self.iterations)) + print(f"Iteartions ------------------------------ {self.iterations}") + res = list(self.planner.astar(start, goal, reversePath=False, iterations=self.iterations)) # points = [state[:2] for state in res] # change here to return the theta as well points = [] for state in res: diff --git a/GEMstack/onboard/planning/route_planning_component.py b/GEMstack/onboard/planning/route_planning_component.py index 672c0c76a..186d8281b 100644 --- a/GEMstack/onboard/planning/route_planning_component.py +++ b/GEMstack/onboard/planning/route_planning_component.py @@ -12,7 +12,7 @@ from .parking_route_planner import ParkingPlanner from .parking_scanning import StraightLineMotion from .longitudinal_planning import longitudinal_plan - +import time class RoutePlanningComponent(Component): @@ -65,6 +65,8 @@ def update(self, state: AllState): desired_points = [(state.vehicle.pose.x, state.vehicle.pose.y), (state.vehicle.pose.x, state.vehicle.pose.y)] desired_path = Path(state.vehicle.pose.frame, desired_points) + + desired_path = desired_path.to_frame(state.vehicle.pose.frame, current_pose=state.vehicle.pose, start_pose_abs=state.start_vehicle_pose) self.compute_parking_route = True @@ -75,9 +77,14 @@ def update(self, state: AllState): if not self.already_computed: self.planner = ParkingPlanner() self.done_computing = True + time_before = time.time() self.route = self.planner.update(state) + time_after = time.time() + print("COMPUTE TIME:", time_after - time_before) + self.route = self.route.to_frame(ObjectFrameEnum.START, current_pose=state.vehicle.pose, start_pose_abs=state.start_vehicle_pose) self.planner.visualize_trajectory(self.route) self.already_computed = True + return self.route elif state.mission_plan.planner_type.name == "RRT_STAR": print("I am in RRT mode") @@ -93,9 +100,10 @@ def update(self, state: AllState): print("I am in SCANNING mode") desired_points = [(state.vehicle.pose.x, state.vehicle.pose.y), (state.vehicle.pose.x + 1, state.vehicle.pose.y)] - desired_path = Path(state.vehicle.pose.frame, desired_points) + desired_path = Path(ObjectFrameEnum.CURRENT, desired_points) # @TODO these are constants we need to get from settings + desired_path = desired_path.to_frame(state.vehicle.pose.frame, current_pose=state.vehicle.pose, start_pose_abs=state.start_vehicle_pose) return desired_path # self.planner.update_speed() diff --git a/launch/parking_detection.yaml b/launch/parking_detection.yaml index 5c79c3092..9a792fe52 100644 --- a/launch/parking_detection.yaml +++ b/launch/parking_detection.yaml @@ -32,11 +32,18 @@ drive: # Full pipeline on board planning: parking_component: type: ParkingSim - # route_planning_component: - # type: RoutePlanningComponent - # trajectory_tracking: - # type: pure_pursuit.PurePursuitTrajectoryTracker - # print: False + route_planning_component: + type: RoutePlanningComponent + motion_planning: + type: longitudinal_planning.YieldTrajectoryPlanner + # type: yield_spline_planner.SplinePlanner + largs: + acceleration: 1 + deceleration: 2 + desired_speed: 1 + trajectory_tracking: + type: pure_pursuit.PurePursuitTrajectoryTracker + print: False #usually can keep this constant computation_graph: !include "../GEMstack/knowledge/defaults/computation_graph.yaml" @@ -85,13 +92,13 @@ variants: # route_planning_component: # type: RoutePlanningComponent - sim: # full pipeline + sim: # full pipeline, sim, real perception run: mode: simulation vehicle_interface: type: gem_simulator.GEMDoubleIntegratorSimulationInterface args: - scene: !relative_path '../scenes/parking_demo.yaml' + scene: !relative_path '../scenes/parking/demo_2.yaml' # visualization: !include "klampt_visualization.yaml" drive: @@ -130,7 +137,7 @@ variants: vehicle_interface: type: gem_simulator.GEMDoubleIntegratorSimulationInterface args: - scene: !relative_path '../scenes/parking_demo.yaml' + scene: !relative_path '../scenes/parking/demo_3.yaml' visualization: !include "klampt_visualization.yaml" drive: @@ -145,13 +152,74 @@ variants: type: ParkingSim route_planning_component: type: RoutePlanningComponent + dubins: + integrator: + timestep: 1 + dt: .25 + max_velocity: 0.75 + max_turning_rate: 0.3 + tolerance: 0.25 + astar: + iterations: 10000 # We should have some sort of max iterations/timeout + heuristic: reeds_shepp + + # route_planning: # type: StaticRoutePlanner # args: [!relative_path '../GEMstack/knowledge/routes/forward_15m.csv'] #forward_15m - motion_planning: longitudinal_planning.YieldTrajectoryPlanner + motion_planning: + type: longitudinal_planning.YieldTrajectoryPlanner + # type: yield_spline_planner.SplinePlanner + largs: + acceleration: 1 + deceleration: 2 + desired_speed: 1 + trajectory_tracking: type: pure_pursuit.PurePursuitTrajectoryTracker print: False + + real_sim: # real on-board planning w/ fake perception + run: + mode: hardware + vehicle_interface: + type: gem_hardware.GEMHardwareInterface + + drive: + perception: + state_estimation : GNSSStateEstimator + agent_detection : OmniscientAgentDetector + parking_detection: parking_detection.FakeParkingSpaceDetector + + + planning: + parking_component: + type: ParkingSim + route_planning_component: + type: RoutePlanningComponent + dubins: + integrator: + timestep: 1 + dt: .25 + max_velocity: 0.75 + max_turning_rate: 0.3 + tolerance: 0.25 + astar: + iterations: 200 # We should have some sort of max iterations/timeout + heuristic: reeds_shepp + + motion_planning: + type: longitudinal_planning.YieldTrajectoryPlanner + # type: yield_spline_planner.SplinePlanner + largs: + acceleration: 1 + deceleration: 2 + desired_speed: 1 + + trajectory_tracking: + type: pure_pursuit.PurePursuitTrajectoryTracker + print: False + log_ros: log: ros_topics : !include "../GEMstack/knowledge/defaults/standard_ros_topics.yaml" \ No newline at end of file diff --git a/scenes/parking/demo_1.yaml b/scenes/parking/demo_1.yaml new file mode 100644 index 000000000..2f4bb4316 --- /dev/null +++ b/scenes/parking/demo_1.yaml @@ -0,0 +1,33 @@ +vehicle_state: [5.0, 5.0, 0.0, 0.0, 0.0] +agents: + cone1: + type: cone + position: [50.0, 8.0] + nominal_velocity: 0.0 + target: [60.0, 8.0] + behavior: loop + yaw: 0 + + cone2: + type: cone + position: [56.0, 8.0] + nominal_velocity: 0.0 + target: [60.0, 8.0] + behavior: loop + yaw: 0 + + cone3: + type: cone + position: [50.0, 4.0] + nominal_velocity: 0.0 + target: [60.0, 8.0] + behavior: loop + yaw: 0 + + cone4: + type: cone + position: [56.0, 4.0] + nominal_velocity: 0.0 + target: [60.0, 8.0] + behavior: loop + yaw: 0 \ No newline at end of file diff --git a/scenes/parking/demo_10.yaml b/scenes/parking/demo_10.yaml new file mode 100644 index 000000000..6d2d1470d --- /dev/null +++ b/scenes/parking/demo_10.yaml @@ -0,0 +1,19 @@ +vehicle_state: [0.0, 0.0, 0.0, 0.0, 0.0] +agents: + ped1: + type: medium_truck + position: [2.5, 8.0] + nominal_velocity: 0.0 + target: [20.0, 8.0] + behavior: loop + yaw: 1.5708 + + ped2: + type: medium_truck + position: [7.5, 8.0] + nominal_velocity: 0.0 + target: [15.0, 4.0] + behavior: loop + yaw: 1.5708 + + \ No newline at end of file diff --git a/scenes/parking_demo.yaml b/scenes/parking/demo_2.yaml similarity index 75% rename from scenes/parking_demo.yaml rename to scenes/parking/demo_2.yaml index 5621db67a..cce5d173c 100644 --- a/scenes/parking_demo.yaml +++ b/scenes/parking/demo_2.yaml @@ -1,24 +1,9 @@ -vehicle_state: [0.0, 0.0, 0.0, 0.0, 0.0] -agents: - # ped1: - # type: medium_truck - # position: [20.0, 8.0] - # nominal_velocity: 0.0 - # target: [20.0, 8.0] - # behavior: loop - # yaw: 1.5708 +vehicle_state: [-1.0, -2.0, 0.0, 0.0, 0.0] - # ped2: - # type: medium_truck - # position: [30.0, 8.0] - # nominal_velocity: 0.0 - # target: [15.0, 4.0] - # behavior: loop - # yaw: 1.5708 - +agents: cone1: type: cone - position: [50.0, 8.0] + position: [3, 10.0] nominal_velocity: 0.0 target: [60.0, 8.0] behavior: loop @@ -26,7 +11,7 @@ agents: cone2: type: cone - position: [56.0, 8.0] + position: [7, 10.0] nominal_velocity: 0.0 target: [60.0, 8.0] behavior: loop @@ -34,7 +19,7 @@ agents: cone3: type: cone - position: [50.0, 4.0] + position: [3, 4.0] nominal_velocity: 0.0 target: [60.0, 8.0] behavior: loop @@ -42,8 +27,24 @@ agents: cone4: type: cone - position: [56.0, 4.0] + position: [7, 4.0] nominal_velocity: 0.0 target: [60.0, 8.0] behavior: loop - yaw: 0 \ No newline at end of file + yaw: 0 + + # ped1: + # type: medium_truck + # position: [2.25, 8.0] + # nominal_velocity: 0.0 + # target: [20.0, 8.0] + # behavior: loop + # yaw: 1.5708 + + # ped2: + # type: medium_truck + # position: [7.75, 8.0] + # nominal_velocity: 0.0 + # target: [15.0, 4.0] + # behavior: loop + # yaw: 1.5708 diff --git a/scenes/parking/demo_3.yaml b/scenes/parking/demo_3.yaml new file mode 100644 index 000000000..b7b66bae8 --- /dev/null +++ b/scenes/parking/demo_3.yaml @@ -0,0 +1,34 @@ +vehicle_state: [-1.0, -2.0, 0.0, 0.0, 0.0] + +agents: + cone1: + type: cone + position: [6, 9.0] + nominal_velocity: 0.0 + target: [60.0, 8.0] + behavior: loop + yaw: 0 + + cone2: + type: cone + position: [10, 9.0] + nominal_velocity: 0.0 + target: [60.0, 8.0] + behavior: loop + yaw: 0 + + cone3: + type: cone + position: [3, 4.0] + nominal_velocity: 0.0 + target: [60.0, 8.0] + behavior: loop + yaw: 0 + + cone4: + type: cone + position: [7, 4.0] + nominal_velocity: 0.0 + target: [60.0, 8.0] + behavior: loop + yaw: 0 \ No newline at end of file diff --git a/scenes/parking/demo_4.yaml b/scenes/parking/demo_4.yaml new file mode 100644 index 000000000..36a82cc04 --- /dev/null +++ b/scenes/parking/demo_4.yaml @@ -0,0 +1,30 @@ +vehicle_state: [-20, -15, 1, 0.0] +agents: + cone1: + type: cone + position: [-3.0, -2.0] + nominal_velocity: 0.0 + target: [-10.0, -8.0] + behavior: loop + yaw: 0 + cone2: + type: cone + position: [3.0, -2.0] + nominal_velocity: 0.0 + target: [-10.0, -8.0] + behavior: loop + yaw: 0 + cone3: + type: cone + position: [-3.0, 2.0] + nominal_velocity: 0.0 + target: [-10.0, -8.0] + behavior: loop + yaw: 0 + cone4: + type: cone + position: [3.0, 2.0] + nominal_velocity: 0.0 + target: [-10.0, -8.0] + behavior: loop + yaw: 0 \ No newline at end of file diff --git a/scenes/parking/demo_5.yaml b/scenes/parking/demo_5.yaml new file mode 100644 index 000000000..6d2d1470d --- /dev/null +++ b/scenes/parking/demo_5.yaml @@ -0,0 +1,19 @@ +vehicle_state: [0.0, 0.0, 0.0, 0.0, 0.0] +agents: + ped1: + type: medium_truck + position: [2.5, 8.0] + nominal_velocity: 0.0 + target: [20.0, 8.0] + behavior: loop + yaw: 1.5708 + + ped2: + type: medium_truck + position: [7.5, 8.0] + nominal_velocity: 0.0 + target: [15.0, 4.0] + behavior: loop + yaw: 1.5708 + + \ No newline at end of file diff --git a/scenes/parking/demo_6.yaml b/scenes/parking/demo_6.yaml new file mode 100644 index 000000000..6d2d1470d --- /dev/null +++ b/scenes/parking/demo_6.yaml @@ -0,0 +1,19 @@ +vehicle_state: [0.0, 0.0, 0.0, 0.0, 0.0] +agents: + ped1: + type: medium_truck + position: [2.5, 8.0] + nominal_velocity: 0.0 + target: [20.0, 8.0] + behavior: loop + yaw: 1.5708 + + ped2: + type: medium_truck + position: [7.5, 8.0] + nominal_velocity: 0.0 + target: [15.0, 4.0] + behavior: loop + yaw: 1.5708 + + \ No newline at end of file diff --git a/scenes/parking/demo_7.yaml b/scenes/parking/demo_7.yaml new file mode 100644 index 000000000..6d2d1470d --- /dev/null +++ b/scenes/parking/demo_7.yaml @@ -0,0 +1,19 @@ +vehicle_state: [0.0, 0.0, 0.0, 0.0, 0.0] +agents: + ped1: + type: medium_truck + position: [2.5, 8.0] + nominal_velocity: 0.0 + target: [20.0, 8.0] + behavior: loop + yaw: 1.5708 + + ped2: + type: medium_truck + position: [7.5, 8.0] + nominal_velocity: 0.0 + target: [15.0, 4.0] + behavior: loop + yaw: 1.5708 + + \ No newline at end of file diff --git a/scenes/parking/demo_8.yaml b/scenes/parking/demo_8.yaml new file mode 100644 index 000000000..6d2d1470d --- /dev/null +++ b/scenes/parking/demo_8.yaml @@ -0,0 +1,19 @@ +vehicle_state: [0.0, 0.0, 0.0, 0.0, 0.0] +agents: + ped1: + type: medium_truck + position: [2.5, 8.0] + nominal_velocity: 0.0 + target: [20.0, 8.0] + behavior: loop + yaw: 1.5708 + + ped2: + type: medium_truck + position: [7.5, 8.0] + nominal_velocity: 0.0 + target: [15.0, 4.0] + behavior: loop + yaw: 1.5708 + + \ No newline at end of file diff --git a/scenes/parking/demo_9.yaml b/scenes/parking/demo_9.yaml new file mode 100644 index 000000000..6d2d1470d --- /dev/null +++ b/scenes/parking/demo_9.yaml @@ -0,0 +1,19 @@ +vehicle_state: [0.0, 0.0, 0.0, 0.0, 0.0] +agents: + ped1: + type: medium_truck + position: [2.5, 8.0] + nominal_velocity: 0.0 + target: [20.0, 8.0] + behavior: loop + yaw: 1.5708 + + ped2: + type: medium_truck + position: [7.5, 8.0] + nominal_velocity: 0.0 + target: [15.0, 4.0] + behavior: loop + yaw: 1.5708 + + \ No newline at end of file