Testing Global frame

Author: udymd

GEMstack/onboard/planning/mission_planning.py

@@ -96,7 +96,7 @@ def update(self, state: AllState):
                     goal_location = None
                     goal_frame = None
                 else:
-                    goal_location = [data['lat'] , data['lon']]
+                    goal_location = [data['lon'] , data['lat']]
                     goal_frame = 'global'
                     print("Goal location:", goal_location)
                     print("Goal frame:", goal_frame)
@@ -130,8 +130,9 @@ def update(self, state: AllState):
             # 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)
+            start_pose_global = ObjectPose(frame=ObjectFrameEnum.GLOBAL, t=time.time(), x=-88.235968, y=40.0927432, yaw=1.57079633)
             self.goal_pose = self.goal_pose.to_frame(ObjectFrameEnum.START, start_pose_abs=start_pose_global)
+            print("Goal pose:", self.goal_pose)
 
         # Initiate state
         if mission is None:

GEMstack/onboard/planning/route_planning.py

@@ -291,7 +291,7 @@ def update(self, state: AllState):
         # 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)
+        start_pose_global = ObjectPose(frame=ObjectFrameEnum.GLOBAL, t=time.time(), x=-88.235968, y=40.0927432, yaw=1.57079633)
         self.roadgraph = self.roadgraph.to_frame(ObjectFrameEnum.START, start_pose_abs=start_pose_global)
 
         # Get all the points of lanes

launch/summoning.yaml

@@ -19,15 +19,16 @@ drive:
         mission_planning:
             type: SummoningMissionPlanner
             args:
-                use_webapp: True   # goal should be defined when use_webapp is false
+                use_webapp: False   # goal should be defined when use_webapp is false
                 webapp_url: 'https://summon-app-production.up.railway.app/'
                 # Goal test points for summoning_roadgraph_sim.json, frame is 'start' or 'cartesian'.
                     # Key points:[0, 0], [0, 30], [37.5, 7.5], [33, 12]，[28.5, 7.5]，[15, 3], [1.5, 7.5], [15, 12], [-3, 12], [-7.5, 7.5]
                     # Points not in the lane:[15, -3], [15, 6], [15, 9], [15, 15]
                 # 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':[32.5, 12], 'frame':'cartesian'}
+                goal: {'location':[-88.235527, 40.0927436], 'frame':'global'}
+                # 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]