def test_vector_multiply(self): # Check that vector_multiply calculates the right values inputMatrix = lgsvl.utils.transform_to_matrix(lgsvl.Transform(lgsvl.Vector(1,2,3), lgsvl.Vector(4,5,6))) inputVector = lgsvl.Vector(10,20,30) expectedVector = lgsvl.Vector(11.560345883724906, 20.792029959836434, 33.58330761714148) vector = lgsvl.utils.vector_multiply(inputVector, inputMatrix) self.assertAlmostEqual(vector.x, expectedVector.x) self.assertAlmostEqual(vector.y, expectedVector.y) self.assertAlmostEqual(vector.z, expectedVector.z)
def test_matrix_inverse(self): # Check that matrix_inverse calculates the right values inputMatrix = lgsvl.utils.transform_to_matrix(lgsvl.Transform(lgsvl.Vector(1,2,3), lgsvl.Vector(4,5,6))) expectedMatrix = [[0.9913729386253347, -0.0980843287345578, 0.08694343573875718, 0.0], \ [0.10427383718471564, 0.9920992900156518, -0.0697564737441253, 0.0], \ [-0.07941450396586013, 0.07822060602635744, 0.9937680178757644, 0.0], \ [-0.9616771010971856, -2.120776069375818, -2.9287345418778, 1.0]] matrix = lgsvl.utils.matrix_inverse(inputMatrix) for i in range(4): for j in range(4): self.assertAlmostEqual(matrix[i][j], expectedMatrix[i][j])
def test_matrix_multiply(self): # Check that matrix_multiply calculates the right values inputMatrix = lgsvl.utils.transform_to_matrix(lgsvl.Transform(lgsvl.Vector(1,2,3), lgsvl.Vector(4,5,6))) expectedMatrix = [[0.9656881042915112, 0.21236393599051254, -0.1494926216255657, 0.0], \ [-0.18774677387638924, 0.9685769782741936, 0.1631250626244768, 0.0], \ [0.1794369920860106, -0.12946117505974142, 0.9752139098799174, 0.0], \ [2.0560345883724906, 3.8792029959836434, 6.058330761714148, 1.0]] matrix = lgsvl.utils.matrix_multiply(inputMatrix, inputMatrix) for i in range(4): for j in range(4): self.assertAlmostEqual(matrix[i][j], expectedMatrix[i][j])
def test_transform_to_matrix(self): transform = lgsvl.Transform(lgsvl.Vector(1,2,3), lgsvl.Vector(4,5,6)) expectedMatrix = [[0.9913729386253347, 0.10427383718471564, -0.07941450396586013, 0.0], \ [-0.0980843287345578, 0.9920992900156518, 0.07822060602635744, 0.0], \ [0.08694343573875718, -0.0697564737441253, 0.9937680178757644, 0.0], \ [1, 2, 3, 1.0]] matrix = lgsvl.utils.transform_to_matrix(transform) for i in range(4): for j in range(4): self.assertAlmostEqual(matrix[i][j], expectedMatrix[i][j])
def get_ego_random_transform(self): origin = lgsvl.Transform() sx = origin.position.x sy = origin.position.y sz = origin.position.z mindist = 0.0 maxdist = 700.0 angle = random.uniform(0.0, 2 * math.pi) dist = random.uniform(mindist, maxdist) point = lgsvl.Vector(sx + dist * math.cos(angle), sy, sz + dist * math.sin(angle)) transform = self.sim.map_point_on_lane(point) return transform
print("Python API Quickstart #34: Setting the fixed camera position") env = Env() sim = lgsvl.Simulator( env.str("LGSVL__SIMULATOR_HOST", lgsvl.wise.SimulatorSettings.simulator_host), env.int("LGSVL__SIMULATOR_PORT", lgsvl.wise.SimulatorSettings.simulator_port)) if sim.current_scene == lgsvl.wise.DefaultAssets.map_borregasave: sim.reset() else: sim.load(lgsvl.wise.DefaultAssets.map_borregasave) # This creates a transform in Unity world coordinates tr = lgsvl.Transform(lgsvl.Vector(10, 50, 0), lgsvl.Vector(90, 0, 0)) spawns = sim.get_spawn() state = lgsvl.AgentState() state.transform = spawns[0] forward = lgsvl.utils.transform_to_forward(spawns[0]) state.velocity = 20 * forward ego = sim.add_agent( env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5), lgsvl.AgentType.EGO, state) # This function sets the camera to free state and applies the transform to the camera rig sim.set_sim_camera(tr) print("Set transform: {}".format(tr))
def set_destination(self, x_long_east, z_lat_north, y=0, coord_type=CoordType.Unity): """ This function can accept a variety of Coordinate systems If using Unity World Coordinate System: x_long_east = x z_lat_north = z y = y If using Latitude/Longitude: x_long_east = Longitude z_lat_north = Latitude If using Easting/Northing: x_long_east = Easting z_lat_north = Northing """ current_pos = self.ego.state.transform current_gps = self.sim.map_to_gps(current_pos) heading = math.radians(current_gps.orientation) # Start position should be the position of the GPS # Unity returns the position of the center of the vehicle so adjustment is required northing_adjustment = ( math.sin(heading) * self.gps_offset.z - math.cos(heading) * self.gps_offset.x ) easting_adjustment = ( math.cos(heading) * self.gps_offset.z + math.sin(heading) * self.gps_offset.x ) if coord_type == CoordType.Unity: transform = lgsvl.Transform( lgsvl.Vector(x_long_east, y, z_lat_north), lgsvl.Vector(0, 0, 0) ) gps = self.sim.map_to_gps(transform) dest_x = gps.easting dest_y = gps.northing elif coord_type == CoordType.Northing: dest_x = x_long_east dest_y = z_lat_north elif coord_type == CoordType.Latitude: transform = self.sim.map_from_gps(longitude=x_long_east, latitude=z_lat_north) gps = self.sim.map_to_gps(transform) dest_x = gps.easting dest_y = gps.northing else: log.error( "Please input a valid Coordinate Type: Unity position, Easting/Northing, or Longitude/Latitude" ) return self.ws.send( json.dumps( { "type": "SendRoutingRequest", "start": { "x": current_gps.easting + easting_adjustment, "y": current_gps.northing + northing_adjustment, "z": 0, "heading": heading, }, "end": {"x": dest_x, "y": dest_y, "z": 0}, "waypoint": "[]", } ) ) return