fix parking and state change

Author: derekzyn

GEMstack/onboard/planning/mission_planning.py

@@ -173,15 +173,13 @@ def update(self, state: AllState):
             if route:
                 if route.points[-1] != self.end_of_driving_route:
                     _, closest_index = route.closest_point([vehicle.pose.x, vehicle.pose.y], edges=False)
-                    if vehicle.v < 0.01 and (closest_index == len(route.points) - 1 or check_distance(route.points[-1], vehicle.pose) < 0.5):
-                        print("Distance to the end to the route.", check_distance(route.points[-1], vehicle.pose))
+                    print("=============================================================")
+                    print(closest_index, len(route.points) - 1, vehicle.v)
+                    print(check_distance(route.points[-1], vehicle.pose))
+                    print("+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++")
+                    if vehicle.v < 0.01 and (closest_index >= len(route.points) - 1 or check_distance(route.points[-1], vehicle.pose) < 0.5):
                         # Set everything to idle
                         mission.type = self.state_machine.next_state()
-                        self.new_goal = False
-                        self.goal_pose = None
-                        self.goal_location = None
-                        self.goal_frame = None
-                        mission.goal_pose = self.goal_pose
                         print("============== Next state:", mission.type)
             else:
                 self.search_count += 1

GEMstack/onboard/planning/route_planning.py

@@ -403,9 +403,6 @@ def update(self, state: AllState):
 
                 self.parking_velocity_is_zero = True
 
-                # TODO: Test only. Remove it later.
-                # self.reedssheppparking.static_horizontal_curb_xy_coordinates = [(10, -3),(40, -3)]
-                # self.obstacle_list= [(12.44, -3), (17.32, -3)]
                 print("Parking area start and end:", self.reedssheppparking.static_horizontal_curb_xy_coordinates)
                 print("Obstacle list:", self.obstacle_list)
 
@@ -414,16 +411,18 @@ def update(self, state: AllState):
                     self.route = None
 
                 elif not self.parking_route_existed:
-                    self.current_pose = [vehicle.pose.x+10, vehicle.pose.y, vehicle.pose.yaw]
+                    self.current_pose = [vehicle.pose.x, vehicle.pose.y, vehicle.pose.yaw]
                     self.reedssheppparking.find_available_parking_spots_and_search_vector(self.obstacle_list,
                                                                                           self.current_pose)
                     self.reedssheppparking.find_collision_free_trajectory_to_park(self.obstacle_list, self.current_pose, True)
                     self.parking_route_existed = True
 
                 else:
                     self.waypoints_to_go = self.reedssheppparking.waypoints_to_go
-                    self.route = Route(frame=ObjectFrameEnum.START, points=self.waypoints_to_go.tolist())
-                    if self.route is None:
+                    if len(self.waypoints_to_go) > 0:
+                        self.route = Route(frame=ObjectFrameEnum.START, points=self.waypoints_to_go.tolist())
+                    else:
+                        self.route = None
                         print("No route found, stop.")
             else:
                 print("No parking lots, stop.")

scenes/summoning_map_sim_setting.yaml

@@ -1,4 +1,4 @@
-start_pose_global: [-88.235164, 40.0927934, 3.14]    # Global pose for global map simulation
+start_pose_global: [-88.235317, 40.0927934, 0.0]    # Global pose for global map simulation
 obstacles:
     frame: 'global'
     positions: [[-88.235500, 40.0927650]]