add debug statemetns

Author: maoyifei

GEMstack/onboard/planning/parking_route_planner.py

@@ -572,7 +572,9 @@ def is_successfully_parked(self, final_state: List[float], goal_pose: ObjectPose
             print("Vehicle is not contained within parking spot")
             try:
                 with open(self.success_file, 'a') as f:
-                    f.write("Not Contained with parking spot")
+                    f.write("\nOrdered Vertices{ordered_vertices}")
+                    f.write("\nVehicle_Polygon{vehicle_polygon}")
+                    f.write("\nNot Contained with parking spot")
             except Exception as e:
                 print(f"Error saving parking status: {e}")
             return False