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
