parking fix, map update

Author: Demon16th

GEMstack/knowledge/routes/summoning_roadgraph_sim.json

@@ -384,7 +384,7 @@ def __init__(self):
         self.route_progress = None
         self.t_last = None
         self.acceleration = 5
-        self.desired_speed = 1.0
+        self.desired_speed = 2.0
         self.deceleration = 2.0
         self.emergency_brake = 8.0
 

GEMstack/onboard/planning/longitudinal_planning.py

@@ -127,7 +127,11 @@ def update(self, state: AllState):
             raise ValueError("Invalid frame argument")
 
         if self.goal_pose:
-            self.goal_pose = self.goal_pose.to_frame(ObjectFrameEnum.START, start_pose_abs=state.start_vehicle_pose)
+            # self.goal_pose = self.goal_pose.to_frame(ObjectFrameEnum.START, start_pose_abs=state.start_vehicle_pose)
+
+            # For global map test in simulation only
+            start_pose_global = ObjectPose(frame=ObjectFrameEnum.GLOBAL, t=time.time(), x=-88.235968, y=40.0927432, yaw=1.507)
+            self.goal_pose = self.goal_pose.to_frame(ObjectFrameEnum.START, start_pose_abs=start_pose_global)
 
         # Initiate state
         if mission is None:
@@ -139,6 +143,7 @@ def update(self, state: AllState):
             if self.new_goal:
                 mission.goal_pose = self.goal_pose
                 mission.type = self.state_machine.next_state()
+                self.new_goal = False
                 print("============== Next state:", mission.type)
 
         # Reach the end of the route, begin to search for parking
@@ -157,6 +162,7 @@ def update(self, state: AllState):
                 _, closest_index = route.closest_point([vehicle.pose.x, vehicle.pose.y], edges=False)
                 if closest_index == len(route.points) - 1 or check_distance(route.points[-1], vehicle.pose) < 0.1:
                     mission.type = self.state_machine.next_state()
+                    self.new_goal = False
                     self.goal_pose = None
                     mission.goal_pose = self.goal_pose
                     print("============== Next state:", mission.type)

GEMstack/onboard/planning/mission_planning.py

@@ -6,6 +6,7 @@
 import numpy as np
 from .RRT import BiRRT
 from .reeds_shepp_parking import ReedsSheppParking
+import time
 
 
 class StaticRoutePlanner(Component):
@@ -156,6 +157,8 @@ def find_parallel_parking_lots(roadgraph: Roadgraph, goal_pose: ObjectPose, max_
     goal_lane = find_closest_lane([goal_pose.x, goal_pose.y], roadgraph)
     goal_lane_points = np.array(goal_lane.right.segments[0])
 
+    print("Goal lane>>>>>>>>>>>>>>>>>>>>>", goal_lane)
+
     # Find the parking lots that attached to the lane
     parking_lots = []
     for region in roadgraph.regions.values():
@@ -208,6 +211,8 @@ def next_point(index, outline, direction='ccw'):
         parking_area_end = (np.array(closest_end_point) + np.array(
             next_point(closest_end_index, farthest_lot, direction='cw'))) / 2
         parking_area_start_end = [parking_area_start, parking_area_end]
+    
+    print("Parking area>>>>>>>>>>>>>>>>>>>>>>>>>>>>>", parking_area_start_end)
 
     return parking_lots, parking_area_start_end
 
@@ -283,7 +288,12 @@ def update(self, state: AllState):
         """ Transform offline map to start frame """
         # if self.roadgraph.frame is not ObjectFrameEnum.START:
         print("=+++++++++++++++++++++++++",state.start_vehicle_pose)
-        self.roadgraph = self.roadgraph.to_frame(ObjectFrameEnum.START, start_pose_abs=state.start_vehicle_pose)
+        # self.roadgraph = self.roadgraph.to_frame(ObjectFrameEnum.START, start_pose_abs=state.start_vehicle_pose)
+
+        # For global map test in simulation only
+        start_pose_global = ObjectPose(frame=ObjectFrameEnum.GLOBAL, t=time.time(), x=-88.235968, y=40.0927432, yaw=1.507)
+        self.roadgraph = self.roadgraph.to_frame(ObjectFrameEnum.START, start_pose_abs=start_pose_global)
+
         # Get all the points of lanes
         if self.map_type == 'roadgraph':
             self.lane_points = get_lane_points_from_roadgraph(self.roadgraph)
@@ -360,7 +370,7 @@ def update(self, state: AllState):
                 print("Parking area start and end:", self.reedssheppparking.static_horizontal_curb_xy_coordinates)
                 print("Obstacle list:", self.obstacle_list)
 
-                if not self.reedssheppparking.add_static_horizontal_curb_as_obstacle:
+                if not self.reedssheppparking.static_horizontal_curb_xy_coordinates:
                     print("No parking area found, start parking here.")
                     self.route = None
 

GEMstack/onboard/planning/route_planning.py

@@ -27,7 +27,7 @@ drive:
                 # Goal test points for summoning_roadgraph_highbay.json, frame is 'global'.
                     # Key points: [-88.235317, 40.0927934], [-88.235252, 40.0927527], [-88.235164, 40.0927934], [-88.235211, 40.0928573],
                     # [-88.235527, 40.0927436], [-88.235968, 40.0927432], [-88.236046, 40.0927917], [-88.236008, 40.0928604], [-88.235905, 40.0927917]
-                goal: {'location':[1, 0], 'frame':'start'}
+                goal: {'location':[32.5, 12], 'frame':'cartesian'}
                 # goal: {'location':[5, -3], 'frame':'cartesian'}
                 # state_machine: [MissionEnum.IDLE, MissionEnum.SUMMONING_DRIVE, MissionEnum.PARALLEL_PARKING]
                 state_machine: [MissionEnum.IDLE, MissionEnum.SUMMON_DRIVING, MissionEnum.PARALLEL_PARKING]
@@ -39,8 +39,8 @@ drive:
                 # Make sure 'roadgraph' map_type match the structure of Roadgraph object.
                 # 'pointlist' is a list of points, carefully define the goal to make sure it is in the lanes.
                 # Map_frame should be 'global', 'cartesian, or 'start'.
-            args: [!relative_path '../GEMstack/knowledge/routes/summoning_roadgraph_sim.json', 'roadgraph', 'cartesian']
-            # args: [!relative_path '../GEMstack/knowledge/routes/summoning_roadgraph_highbay.json', 'roadgraph', 'global']
+            # args: [!relative_path '../GEMstack/knowledge/routes/summoning_roadgraph_sim.json', 'roadgraph', 'cartesian']
+            args: [!relative_path '../GEMstack/knowledge/routes/summoning_roadgraph_highbay.json', 'roadgraph', 'global']
         motion_planning: longitudinal_planning.YieldTrajectoryPlanner
         trajectory_tracking:
             type: pure_pursuit.PurePursuitTrajectoryTracker

launch/summoning.yaml

@@ -217,20 +217,13 @@ def create_lane(left_back : Tuple, left_forward : Tuple,
                                            route_name=''
                                            )
     roadgraph.lanes['t_1_1'] = create_lane(left_back=(33.0, 10.5, 0.0), left_forward=(30.0, 7.5, 0.0),
-                                            right_back=(33.0, 13.5, 0.0), right_forward=(28.5, 13.5, 0.0),
+                                            right_back=(24.0, 10.5, 0.0), right_forward=(27.0, 7.5, 0.0),
                                             left_crossable=False, left_type='arc', left_radius=3.0, left_direction='ccw',
-                                            right_crossable=False,
-                                            resolution=resolution,
-                                            route_name=''
-                                            )
-    roadgraph.lanes['t_1_2'] = create_lane(left_back=(27.0, 7.5, 0.0), left_forward=(24.0, 10.5, 0.0),
-                                            right_back=(28.5, 13.5, 0.0), right_forward=(24.0, 13.5, 0.0),
-                                            left_crossable=False, left_type='arc', left_radius=3.0, left_direction='ccw',
-                                            right_crossable=False,
+                                            right_crossable=False, right_type='arc', right_radius=3.0, right_direction='cw',
                                             resolution=resolution,
                                             route_name=''
                                             )
-    roadgraph.lanes['t_1_3'] = create_lane(left_back=(30.0, 7.5, 0.0), left_forward=(24.0, 1.5, 0.0),
+    roadgraph.lanes['t_1_2'] = create_lane(left_back=(30.0, 7.5, 0.0), left_forward=(24.0, 1.5, 0.0),
                                             right_back=(27.0, 7.5, 0.0), right_forward=(24.0, 4.5, 0.0),
                                             left_crossable=False, left_type='arc', left_radius=6.0, left_direction='cw',
                                             right_crossable=False, right_type='arc', right_radius=3.0, right_direction='cw',
@@ -251,22 +244,16 @@ def create_lane(left_back : Tuple, left_forward : Tuple,
                                             resolution=resolution,
                                             route_name=''
                                             )
-    roadgraph.lanes['t_2_2'] = create_lane(left_back=(6.0, 10.5, 0.0), left_forward=(3.0, 7.5, 0.0),
-                                            right_back=(6.0, 13.5, 0.0), right_forward=(1.5, 13.5, 0.0),
+    roadgraph.lanes['t_2_2'] = create_lane(left_back=(0.0, 7.5, 0.0), left_forward=(-3.0, 10.5, 0.0),
+                                            right_back=(3.0, 7.5, 0.0), right_forward=(6.0, 10.5, 0.0),
                                             left_crossable=False, left_type='arc', left_radius=3.0, left_direction='ccw',
-                                            right_crossable=False,
-                                            resolution=resolution,
-                                            route_name=''
-                                            )
-    roadgraph.lanes['t_2_3'] = create_lane(left_back=(0.0, 7.5, 0.0), left_forward=(-3.0, 10.5, 0.0),
-                                            right_back=(1.5, 13.5, 0.0), right_forward=(-3.0, 13.5, 0.0),
-                                            left_crossable=False, left_type='arc', left_radius=3.0, left_direction='ccw',
-                                            right_crossable=False,
+                                            right_crossable=False, right_type='arc', right_radius=3.0, right_direction='cw',
                                             resolution=resolution,
                                             route_name=''
                                             )
     roadgraph.lanes['lane_2'] = create_lane(left_back=(24.0, 10.5, 0.0), left_forward=(6.0, 10.5, 0.0),
-                                            right_back=(24.0, 13.5, 0.0), right_forward=(6.0, 13.5, 0.0),
+                                            # right_back=(24.0, 13.5, 0.0), right_forward=(6.0, 13.5, 0.0),
+                                            right_back=(33.0, 13.5, 0.0), right_forward=(-3.0, 13.5, 0.0),
                                             left_crossable=False,
                                             right_crossable=False,
                                             resolution=resolution,

testing/roadgraph_generator.py roadgraph_generator.pytesting/roadgraph_generator.py renamed to roadgraph_generator.py

@@ -1,4 +1,4 @@
-vehicle_state: [15.0, 14.0, 3.14, 0.0, 0.0]
+vehicle_state: [10.0, 0.0, 0.0, 0.0, 0.0]
 agents:
   car1:
     type: car