AUTO-4369: Updated coordinates after rotation

Author: taehyung1.kim

tests/test_EGO.py

@@ -34,7 +34,7 @@ def test_different_spawns(self): # Check if EGO is spawned in the spawn position
             cmEqual(self, agent2.state.rotation, spawns[1].rotation, "Spawn Rotation 1")
 
     def test_agent_velocity(self): # Check EGO velocity
-        with SimConnection() as sim:
+        with SimConnection(60) as sim:
             state = spawnState(sim)
             agent = self.create_EGO(sim)
             cmEqual(self, agent.state.velocity, state.velocity, "0 Velocity")
@@ -47,7 +47,7 @@ def test_agent_velocity(self): # Check EGO velocity
             cmEqual(self, agent.state.velocity, state.velocity, "50 Velocity")
 
     def test_ego_different_directions(self): # Check that the xyz velocities equate to xyz changes in position
-        with SimConnection(40) as sim:
+        with SimConnection(60) as sim:
             state = spawnState(sim)
             forward = lgsvl.utils.transform_to_forward(state.transform)
             up = lgsvl.utils.transform_to_up(state.transform)
@@ -87,7 +87,7 @@ def test_speed(self): # check that speed returns a reasonable number
     def test_rotation_on_highway_ramp(self): # Check that vehicle is rotated when spawned on the highway ramp
         with SimConnection() as sim:
             state = spawnState(sim)
-            state.transform.position = lgsvl.Vector(100.4229, 15.67488, -469.6401)
+            state.transform.position = lgsvl.Vector(469.6401, 15.67488, 100.4229)
             ego = sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, state)
             self.assertAlmostEqual(ego.state.rotation.z, state.rotation.z)
             sim.run(0.5)
@@ -204,7 +204,7 @@ def test_not_sticky_control(self): # Check that the a non sticky control is remo
             self.assertGreater(stickySpeed, finalSpeed)
 
     def test_vary_throttle(self): # Check that different throttle values accelerate differently
-        with SimConnection() as sim:
+        with SimConnection(40) as sim:
             ego = sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, spawnState(sim))
             control = lgsvl.VehicleControl()
             control.throttle = 0.5
@@ -221,7 +221,7 @@ def test_vary_throttle(self): # Check that different throttle values accelerate
             self.assertLess(ego.state.speed, initialSpeed)
 
     def test_vary_steering(self): # Check that different steering values turn the car differently
-        with SimConnection() as sim:
+        with SimConnection(40) as sim:
             ego = sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, spawnState(sim))
             control = lgsvl.VehicleControl()
             control.throttle = 0.5

tests/test_NPC.py

@@ -69,7 +69,7 @@ def test_NPC_follow_lane(self): #Check if NPC can follow lane
             agentState = agent.state
             self.assertGreater(agentState.speed, 0)
             # self.assertAlmostEqual(agent.state.speed, 5.6, delta=1)   
-            self.assertLess(agent.state.position.x - sim.get_spawn()[0].position.x, 5.6)
+            self.assertLess(agent.state.position.x - sim.get_spawn()[0].position.x, 5.6*2)
 
     def test_rotate_NPC(self): # Check if NPC can be rotated
         with SimConnection() as sim:
@@ -78,11 +78,11 @@ def test_rotate_NPC(self): # Check if NPC can be rotated
             state.transform.position = state.position - 5 * right
             sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, state)
             agent = self.create_NPC(sim, "SUV")
-            self.assertAlmostEqual(agent.state.transform.rotation.y, 194.823394, places=3)
+            self.assertAlmostEqual(agent.state.transform.rotation.y, 104.823394, places=3)
             x = agent.state
-            x.transform.rotation.y = 100
+            x.transform.rotation.y = 10
             agent.state = x
-            self.assertAlmostEqual(agent.state.transform.rotation.y, 100, delta=0.1)
+            self.assertAlmostEqual(agent.state.transform.rotation.y, 10, delta=0.1)
 
     def test_blank_agent(self): # Check that an exception is raised if a blank name is given
         with SimConnection() as sim:
@@ -299,10 +299,10 @@ def test_spawn_speed(self): # Checks that a spawned agent keeps the correct spee
     def test_lane_change_right(self):
         with SimConnection(40) as sim:
             state = lgsvl.AgentState()
-            state.transform = sim.map_point_on_lane(lgsvl.Vector(40.85, -1.57, -4.49))
+            state.transform = sim.map_point_on_lane(lgsvl.Vector(4.49, -1.57, 40.85))
             npc = sim.add_agent("SUV", lgsvl.AgentType.NPC, state)
 
-            state.transform = sim.map_point_on_lane(lgsvl.Vector(42.73, -1.57, -0.63))
+            state.transform = sim.map_point_on_lane(lgsvl.Vector(0.63, -1.57, 42.73))
             sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, state)
             forward = lgsvl.utils.transform_to_forward(state.transform)
             target = state.position + 31 * forward
@@ -324,12 +324,12 @@ def on_lane_change(agent):
     def test_lane_change_right_missing_lane(self):
         with SimConnection(40) as sim:
             state = lgsvl.AgentState()
-            state.transform = sim.map_point_on_lane(lgsvl.Vector(42.73, -1.57, -0.63))
+            state.transform = sim.map_point_on_lane(lgsvl.Vector(0.63, -1.57, 42.73))
             npc = sim.add_agent("Hatchback", lgsvl.AgentType.NPC, state)
             forward = lgsvl.utils.transform_to_forward(state.transform)
             target = state.position + 42.75 * forward
 
-            state.transform = sim.map_point_on_lane(lgsvl.Vector(40.85, -1.57, -4.49))
+            state.transform = sim.map_point_on_lane(lgsvl.Vector(4.49, -1.57, 40.85))
             sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, state)
 
             npc.follow_closest_lane(True, 10)
@@ -349,10 +349,10 @@ def on_lane_change(agent):
     def test_lane_change_left(self):
         with SimConnection(40) as sim:
             state = lgsvl.AgentState()
-            state.transform = sim.map_point_on_lane(lgsvl.Vector(40.85, -1.57, -4.49))
+            state.transform = sim.map_point_on_lane(lgsvl.Vector(4.49, -1.57, 40.85))
             npc = sim.add_agent("SUV", lgsvl.AgentType.NPC, state)
 
-            state.transform = sim.map_point_on_lane(lgsvl.Vector(42.02, -1.79, -9.82))
+            state.transform = sim.map_point_on_lane(lgsvl.Vector(9.82, -1.79, 42.02))
             sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, state)
             forward = lgsvl.utils.transform_to_forward(state.transform)
             target = state.position + 31 * forward
@@ -374,7 +374,7 @@ def on_lane_change(agent):
     def test_lane_change_left_opposing_traffic(self):
         with SimConnection(40) as sim:
             state = lgsvl.AgentState()
-            state.transform = sim.map_point_on_lane(lgsvl.Vector(-40.292, -3.363, -78.962))
+            state.transform = sim.map_point_on_lane(lgsvl.Vector(78.962, -3.363, -40.292))
             npc = sim.add_agent("SUV", lgsvl.AgentType.NPC, state)
             forward = lgsvl.utils.transform_to_forward(state.transform)
             target = state.position + 42.75 * forward
@@ -400,13 +400,13 @@ def on_lane_change(agent):
     def test_multiple_lane_changes(self):
         with SimConnection(120) as sim:
             state = lgsvl.AgentState()
-            state.transform.position = lgsvl.Vector(239,10,180)
-            state.transform.rotation = lgsvl.Vector(0,180,0)
+            state.transform.position = lgsvl.Vector(-180,10,239)
+            state.transform.rotation = lgsvl.Vector(0,90,0)
             sim.add_agent("XE_Rigged-apollo", lgsvl.AgentType.EGO, state)
 
             state = lgsvl.AgentState()
-            state.transform.position = lgsvl.Vector(234.5, 10, 175)
-            state.transform.rotation = lgsvl.Vector(0.016, 180, 0)
+            state.transform.position = lgsvl.Vector(-175, 10, 234.5)
+            state.transform.rotation = lgsvl.Vector(0, 90, 0.016)
             npc = sim.add_agent("Sedan", lgsvl.AgentType.NPC, state)
 
             npc.follow_closest_lane(True, 10)
@@ -455,9 +455,9 @@ def test_npc_control_exceptions(self):
                 npc.apply_control(control)
 
     def test_e_stop(self):
-        with SimConnection() as sim:
+        with SimConnection(60) as sim:
             state = lgsvl.AgentState()
-            state.transform = sim.map_point_on_lane(lgsvl.Vector(-40.292, -3.363, -78.962))
+            state.transform = sim.map_point_on_lane(lgsvl.Vector(78.962, -3.363, -40.292))
             sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, state)
             forward = lgsvl.utils.transform_to_forward(state.transform)
             state.transform.position = state.position + 10 * forward
@@ -478,7 +478,7 @@ def test_e_stop(self):
     def test_waypoint_speed(self):
         with SimConnection(60) as sim:
             state = lgsvl.AgentState()
-            state.transform = sim.map_point_on_lane(lgsvl.Vector(-40.292, -3.363, -78.962))
+            state.transform = sim.map_point_on_lane(lgsvl.Vector(78.962, -3.363, -40.292))
             sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, state)
             forward = lgsvl.utils.transform_to_forward(state.transform)
             up = lgsvl.utils.transform_to_up(state.transform)

tests/test_peds.py

@@ -19,13 +19,13 @@ def test_ped_creation(self): # Check if the different types of Peds can be creat
             state.transform.position = state.position - 4 * forward
             sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, state)
             for name in ["Bob", "EntrepreneurFemale", "Howard", "Johny", \
-                "Pamela", "Presley", "Red", "Robin", "Stephen", "Zoe"]:
+                "Pamela", "Presley", "Robin", "Stephen", "Zoe"]:
                 agent = self.create_ped(sim, name, spawnState(sim))
                 cmEqual(self, agent.state.position, sim.get_spawn()[0].position, name)
                 self.assertEqual(agent.name, name)
 
     def test_ped_random_walk(self): # Check if pedestrians can walk randomly
-        with SimConnection() as sim:
+        with SimConnection(40) as sim:
             state = spawnState(sim)
             forward = lgsvl.utils.transform_to_forward(state.transform)
             state.transform.position = state.position - 4 * forward

tests/test_simulator.py

@@ -74,12 +74,12 @@ def test_raycast(self): # Check if raycasting works
             # Right
             hit = sim.raycast(p, right, layer_mask)
             self.assertTrue(hit)
-            self.assertAlmostEqual(hit.distance, 15.0892992019653)
+            self.assertAlmostEqual(hit.distance, 15.089282989502, places=3)
 
             #Left
             hit = sim.raycast(p, -right, layer_mask)
             self.assertTrue(hit)
-            self.assertAlmostEqual(hit.distance, 19.72922706604)
+            self.assertAlmostEqual(hit.distance, 19.7292556762695, places=3)
 
             #Back
             hit = sim.raycast(p, -forward, layer_mask)
@@ -96,7 +96,7 @@ def test_raycast(self): # Check if raycasting works
             # Down
             hit = sim.raycast(p, -up, layer_mask)
             self.assertTrue(hit)
-            self.assertAlmostEqual(hit.distance, 1.08376359939575)
+            self.assertAlmostEqual(hit.distance, 1.0837641954422, places=3)
 
     def test_weather(self): # Check that the weather state can be read properly and changed
         with SimConnection() as sim:
@@ -217,7 +217,7 @@ def test_get_gps(self): # Checks that GPS reports the correct values
             self.assertAlmostEqual(gps.northing, 4141627.34000015)
             self.assertAlmostEqual(gps.easting, 587060.970000267)
             self.assertAlmostEqual(gps.altitude, -1.03600001335144)
-            self.assertAlmostEqual(gps.orientation, -194.823394775391)
+            self.assertAlmostEqual(gps.orientation, -104.823394775391)
 
     def test_from_northing(self): # Check that position vectors are correctly generated given northing and easting
         with SimConnection() as sim:
@@ -236,7 +236,7 @@ def test_from_latlong(self): # Check that position vectors are correctly generat
     def test_from_alt_orient(self): # Check that position vectors are correctly generated with altitude and orientation
         with SimConnection() as sim:
             spawn = sim.get_spawn()[0]
-            location = sim.map_from_gps(northing=4141627.34000015, easting=587060.970000267, altitude=-1.03600001335144, orientation=-194.823394775391)
+            location = sim.map_from_gps(northing=4141627.34000015, easting=587060.970000267, altitude=-1.03600001335144, orientation=-104.823371887207)
             self.assertAlmostEqual(spawn.position.y, location.position.y, places=1)
             self.assertAlmostEqual(spawn.rotation.y, location.rotation.y, places=1)