from qvl.qlabs import CommModularContainer
from qvl.actor import QLabsActor
import math
import struct
import cv2
import numpy as np
######################### MODULAR CONTAINER CLASS #########################
[docs]class QLabsQCar(QLabsActor):
"""This class is for spawning QCars."""
ID_QCAR = 160
""" Class ID """
FCN_QCAR_SET_VELOCITY_AND_REQUEST_STATE = 10
FCN_QCAR_VELOCITY_STATE_RESPONSE = 11
FCN_QCAR_SET_TRANSFORM_AND_REQUEST_STATE = 12
FCN_QCAR_TRANSFORM_STATE_RESPONSE = 13
FCN_QCAR_POSSESS = 20
FCN_QCAR_POSSESS_ACK = 21
FCN_QCAR_GHOST_MODE = 22
FCN_QCAR_GHOST_MODE_ACK = 23
FCN_QCAR_CAMERA_DATA_REQUEST = 100
FCN_QCAR_CAMERA_DATA_RESPONSE = 101
FCN_QCAR_LIDAR_DATA_REQUEST = 110
FCN_QCAR_LIDAR_DATA_RESPONSE = 111
CAMERA_CSI_RIGHT = 0
#Image capture resolution: 820x410
CAMERA_CSI_BACK = 1
#Image capture resolution: 820x410
CAMERA_CSI_LEFT = 2
#Image capture resolution: 820x410
CAMERA_CSI_FRONT = 3
#Image capture resolution: 820x410
CAMERA_RGB = 4
#Image capture resolution: 640x480
CAMERA_DEPTH = 5
#Image capture resolution: 640x480
CAMERA_OVERHEAD = 6
CAMERA_TRAILING = 7
#Note: The mouse scroll wheel can be used to zoom in and out in this mode. """
_sensor_scaling = 1
[docs] def __init__(self, qlabs, verbose=False):
""" Constructor Method
:param qlabs: A QuanserInteractiveLabs object
:param verbose: (Optional) Print error information to the console.
:type qlabs: object
:type verbose: boolean
"""
self._qlabs = qlabs
self._verbose = verbose
self.classID = self.ID_QCAR
return
[docs] def spawn_id(self, actorNumber, location=[0,0,0], rotation=[0,0,0], scale=[1,1,1], configuration=0, waitForConfirmation=True):
"""Spawns a new QCar actor.
:param actorNumber: User defined unique identifier for the class actor in QLabs
:param location: (Optional) An array of floats for x, y and z coordinates
:param rotation: (Optional) An array of floats for the roll, pitch, and yaw in radians
:param scale: (Optional) An array of floats for the scale in the x, y, and z directions. Scale values of 0.0 should not be used and only uniform scaling is recommended. Sensor scaling will be based on scale[0].
:param configuration: (Optional) Spawn configuration. See class library for configuration options.
:param waitForConfirmation: (Optional) Make this operation blocking until confirmation of the spawn has occurred.
:type actorNumber: uint32
:type location: float array[3]
:type rotation: float array[3]
:type scale: float array[3]
:type configuration: uint32
:type waitForConfirmation: boolean
:return:
- **status** - 0 if successful, 1 class not available, 2 actor number not available or already in use, 3 unknown error, -1 communications error
:rtype: int32
"""
self._sensor_scaling = scale[0]
return super().spawn_id(actorNumber, location, rotation, scale, configuration, waitForConfirmation)
[docs] def spawn_id_degrees(self, actorNumber, location=[0,0,0], rotation=[0,0,0], scale=[1,1,1], configuration=0, waitForConfirmation=True):
"""Spawns a new QCar actor.
:param actorNumber: User defined unique identifier for the class actor in QLabs
:param location: (Optional) An array of floats for x, y and z coordinates
:param rotation: (Optional) An array of floats for the roll, pitch, and yaw in radians
:param scale: (Optional) An array of floats for the scale in the x, y, and z directions. Scale values of 0.0 should not be used and only uniform scaling is recommended. Sensor scaling will be based on scale[0].
:param configuration: (Optional) Spawn configuration. See class library for configuration options.
:param waitForConfirmation: (Optional) Make this operation blocking until confirmation of the spawn has occurred.
:type actorNumber: uint32
:type location: float array[3]
:type rotation: float array[3]
:type scale: float array[3]
:type configuration: uint32
:type waitForConfirmation: boolean
:return:
- **status** - 0 if successful, 1 class not available, 2 actor number not available or already in use, 3 unknown error, -1 communications error
:rtype: int32
"""
self._sensor_scaling = scale[0]
return super().spawn_id_degrees(actorNumber, location, rotation, scale, configuration, waitForConfirmation)
[docs] def spawn(self, location=[0,0,0], rotation=[0,0,0], scale=[1,1,1], configuration=0, waitForConfirmation=True):
"""Spawns a new QCar actor with the next available actor number within this class.
:param location: (Optional) An array of floats for x, y and z coordinates
:param rotation: (Optional) An array of floats for the roll, pitch, and yaw in radians
:param scale: (Optional) An array of floats for the scale in the x, y, and z directions. Scale values of 0.0 should not be used and only uniform scaling is recommended. Sensor scaling will be based on scale[0].
:param configuration: (Optional) Spawn configuration. See class library for configuration options.
:param waitForConfirmation: (Optional) Make this operation blocking until confirmation of the spawn has occurred. Note that if this is False, the returned actor number will be invalid.
:type location: float array[3]
:type rotation: float array[3]
:type scale: float array[3]
:type configuration: uint32
:type waitForConfirmation: boolean
:return:
- **status** - 0 if successful, 1 class not available, 3 unknown error, -1 communications error.
- **actorNumber** - An actor number to use for future references.
:rtype: int32, int32
"""
self._sensor_scaling = scale[0]
return super().spawn(location, rotation, scale, configuration, waitForConfirmation)
[docs] def spawn_degrees(self, location=[0,0,0], rotation=[0,0,0], scale=[1,1,1], configuration=0, waitForConfirmation=True):
"""Spawns a new QCar actor with the next available actor number within this class.
:param location: (Optional) An array of floats for x, y and z coordinates
:param rotation: (Optional) An array of floats for the roll, pitch, and yaw in degrees
:param scale: (Optional) An array of floats for the scale in the x, y, and z directions. Scale values of 0.0 should not be used and only uniform scaling is recommended. Sensor scaling will be based on scale[0].
:param configuration: (Optional) Spawn configuration. See class library for configuration options.
:param waitForConfirmation: (Optional) Make this operation blocking until confirmation of the spawn has occurred. Note that if this is False, the returned actor number will be invalid.
:type location: float array[3]
:type rotation: float array[3]
:type scale: float array[3]
:type configuration: uint32
:type waitForConfirmation: boolean
:return:
- **status** - 0 if successful, 1 class not available, 3 unknown error, -1 communications error.
- **actorNumber** - An actor number to use for future references.
:rtype: int32, int32
"""
self._sensor_scaling = scale[0]
return super().spawn_degrees(location, rotation, scale, configuration, waitForConfirmation)
def spawn_id_and_parent_with_relative_transform(self, actorNumber, location=[0,0,0], rotation=[0,0,0], scale=[1,1,1], configuration=0, parentClassID=0, parentActorNumber=0, parentComponent=0, waitForConfirmation=True):
"""Spawns a new QCar actor relative to an existing actor and creates a kinematic relationship.
:param actorNumber: User defined unique identifier for the class actor in QLabs
:param location: (Optional) An array of floats for x, y and z coordinates
:param rotation: (Optional) An array of floats for the roll, pitch, and yaw in radians
:param scale: (Optional) An array of floats for the scale in the x, y, and z directions. Scale values of 0.0 should not be used and only uniform scaling is recommended. Sensor scaling will be based on scale[0].
:param configuration: (Optional) Spawn configuration. See class library for configuration options.
:param parentClassID: See the ID variables in the respective library classes for the class identifier
:param parentActorNumber: User defined unique identifier for the class actor in QLabs
:param parentComponent: `0` for the origin of the parent actor, see the parent class for additional reference frame options
:param waitForConfirmation: (Optional) Make this operation blocking until confirmation of the spawn has occurred.
:type actorNumber: uint32
:type location: float array[3]
:type rotation: float array[3]
:type scale: float array[3]
:type configuration: uint32
:type parentClassID: uint32
:type parentActorNumber: uint32
:type parentComponent: uint32
:type waitForConfirmation: boolean
:return:
- **status** - 0 if successful, 1 class not available, 2 actor number not available or already in use, 3 cannot find the parent actor, 4 unknown error, -1 communications error
:rtype: int32
"""
self._sensor_scaling = scale[0]
return super().spawn_id_and_parent_with_relative_transform(actorNumber, location, rotation, scale, configuration, parentClassID, parentActorNumber, parentComponent, waitForConfirmation)
def spawn_id_and_parent_with_relative_transform_degrees(self, actorNumber, location=[0,0,0], rotation=[0,0,0], scale=[1,1,1], configuration=0, parentClassID=0, parentActorNumber=0, parentComponent=0, waitForConfirmation=True):
"""Spawns a new QCar actor relative to an existing actor and creates a kinematic relationship.
:param actorNumber: User defined unique identifier for the class actor in QLabs
:param location: (Optional) An array of floats for x, y and z coordinates
:param rotation: (Optional) An array of floats for the roll, pitch, and yaw in degrees
:param scale: (Optional) An array of floats for the scale in the x, y, and z directions. Scale values of 0.0 should not be used and only uniform scaling is recommended. Sensor scaling will be based on scale[0].
:param configuration: (Optional) Spawn configuration. See class library for configuration options.
:param parentClassID: See the ID variables in the respective library classes for the class identifier
:param parentActorNumber: User defined unique identifier for the class actor in QLabs
:param parentComponent: `0` for the origin of the parent actor, see the parent class for additional reference frame options
:param waitForConfirmation: (Optional) Make this operation blocking until confirmation of the spawn has occurred.
:type actorNumber: uint32
:type location: float array[3]
:type rotation: float array[3]
:type scale: float array[3]
:type configuration: uint32
:type parentClassID: uint32
:type parentActorNumber: uint32
:type parentComponent: uint32
:type waitForConfirmation: boolean
:return:
- **status** - 0 if successful, 1 class not available, 2 actor number not available or already in use, 3 cannot find the parent actor, 4 unknown error, -1 communications error
:rtype: int32
"""
self._sensor_scaling = scale[0]
return super().spawn_id_and_parent_with_relative_transform_degrees(actorNumber, location, rotation, scale, configuration, parentClassID, parentActorNumber, parentComponent, waitForConfirmation)
[docs] def set_velocity_and_request_state(self, forward, turn, headlights, leftTurnSignal, rightTurnSignal, brakeSignal, reverseSignal):
"""Sets the velocity, turn angle in radians, and other car properties.
:param forward: Speed in m/s of a full-scale car. Multiply physical QCar speeds by 10 to get full scale speeds.
:param turn: Turn angle in radians. Positive values turn right.
:param headlights: Enable the headlights.
:param leftTurnSignal: Enable the left turn signal.
:param rightTurnSignal: Enable the right turn signal.
:param brakeSignal: Enable the brake lights (does not affect the motion of the vehicle).
:param reverseSignal: Enable the reverse lights.
:type forward: float
:type turn: float
:type headlights: boolean
:type leftTurnSignal: boolean
:type rightTurnSignal: boolean
:type brakeSignal: boolean
:type reverseSignal: boolean
:return:
- **status** - True if successful, False otherwise
- **location**
- **rotation** - in radians
- **front bumper hit** - True if in contact with a collision object, False otherwise
- **rear bumper hit** - True if in contact with a collision object, False otherwise
:rtype: boolean, float array[3], float array[3], boolean, boolean
"""
if (not self._is_actor_number_valid()):
return False, [0,0,0], [0,0,0], False, False
c = CommModularContainer()
c.classID = self.ID_QCAR
c.actorNumber = self.actorNumber
c.actorFunction = self.FCN_QCAR_SET_VELOCITY_AND_REQUEST_STATE
c.payload = bytearray(struct.pack(">ffBBBBB", forward, turn, headlights, leftTurnSignal, rightTurnSignal, brakeSignal, reverseSignal))
c.containerSize = c.BASE_CONTAINER_SIZE + len(c.payload)
location = [0,0,0]
rotation = [0,0,0]
frontHit = False
rearHit = False
self._qlabs.flush_receive()
if (self._qlabs.send_container(c)):
c = self._qlabs.wait_for_container(self.ID_QCAR, self.actorNumber, self.FCN_QCAR_VELOCITY_STATE_RESPONSE)
if (c == None):
return False, location, rotation, frontHit, rearHit
if len(c.payload) == 26:
location[0], location[1], location[2], rotation[0], rotation[1], rotation[2], frontHit, rearHit, = struct.unpack(">ffffff??", c.payload[0:26])
return True, location, rotation, frontHit, rearHit
else:
return False, location, rotation, frontHit, rearHit
else:
return False, location, rotation, frontHit, rearHit
[docs] def set_velocity_and_request_state_degrees(self, forward, turn, headlights, leftTurnSignal, rightTurnSignal, brakeSignal, reverseSignal):
"""Sets the velocity, turn angle in degrees, and other car properties.
:param forward: Speed in m/s of a full-scale car. Multiply physical QCar speeds by 10 to get full scale speeds.
:param turn: Turn angle in degrees. Positive values turn right.
:param headlights: Enable the headlights.
:param leftTurnSignal: Enable the left turn signal.
:param rightTurnSignal: Enable the right turn signal.
:param brakeSignal: Enable the brake lights (does not affect the motion of the vehicle).
:param reverseSignal: Enable the reverse lights.
:type forward: float
:type turn: float
:type headlights: boolean
:type leftTurnSignal: boolean
:type rightTurnSignal: boolean
:type brakeSignal: boolean
:type reverseSignal: boolean
:return:
- **status** - `True` if successful, `False` otherwise
- **location**
- **rotation** - in radians
- **front bumper hit** - `True` if in contact with a collision object, `False` otherwise
- **rear bumper hit** - `True` if in contact with a collision object, `False` otherwise
:rtype: boolean, float array[3], float array[3], boolean, boolean
"""
success, location, rotation, frontHit, rearHit = self.set_velocity_and_request_state(forward, turn/180*math.pi, headlights, leftTurnSignal, rightTurnSignal, brakeSignal, reverseSignal)
rotation_deg = [rotation[0]/math.pi*180, rotation[1]/math.pi*180, rotation[2]/math.pi*180]
return success, location, rotation_deg, frontHit, rearHit
[docs] def possess(self, camera=CAMERA_TRAILING):
"""
Possess (take control of) a QCar in QLabs with the selected camera.
:param camera: Pre-defined camera constant. See CAMERA constants for available options. Default is the trailing camera.
:type camera: uint32
:return:
- **status** - `True` if possessing the camera was successful, `False` otherwise
:rtype: boolean
"""
if (not self._is_actor_number_valid()):
return False
c = CommModularContainer()
c.classID = self.ID_QCAR
c.actorNumber = self.actorNumber
c.actorFunction = self.FCN_QCAR_POSSESS
c.payload = bytearray(struct.pack(">B", camera))
c.containerSize = c.BASE_CONTAINER_SIZE + len(c.payload)
self._qlabs.flush_receive()
if (self._qlabs.send_container(c)):
c = self._qlabs.wait_for_container(self.ID_QCAR, self.actorNumber, self.FCN_QCAR_POSSESS_ACK)
if (c == None):
return False
else:
return True
else:
return False
[docs] def ghost_mode(self, enable=True, color=[0,1,0]):
"""
Ghost mode changes the selected QCar actor into a transparent colored version. This can be useful as a reference actor or indicating a change in state.
:param enable: Set the QCar to the defined transparent color, otherwise revert to the solid color scheme.
:param color: Red, Green, Blue components of the RGB color on a 0.0 to 1.0 scale.
:type camera: uint32
:type enable: boolean
:type color: float array[3]
:return:
- **status** - `True` if successful, `False` otherwise
:rtype: boolean
"""
if (not self._is_actor_number_valid()):
return False
c = CommModularContainer()
c.classID = self.ID_QCAR
c.actorNumber = self.actorNumber
c.actorFunction = self.FCN_QCAR_GHOST_MODE
c.payload = bytearray(struct.pack(">Bfff", enable, color[0], color[1], color[2]))
c.containerSize = c.BASE_CONTAINER_SIZE + len(c.payload)
self._qlabs.flush_receive()
if (self._qlabs.send_container(c)):
c = self._qlabs.wait_for_container(self.ID_QCAR, self.actorNumber, self.FCN_QCAR_GHOST_MODE_ACK)
if (c == None):
return False
else:
return True
else:
return False
[docs] def get_image(self, camera):
"""
Request a JPG image from one of the QCar cameras.
:param camera: Pre-defined camera constant. See CAMERA constants for available options. Trailing and Overhead cameras cannot be selected.
:type camera: uint32
:return:
- **status** - `True` and image data if successful, `False` and empty otherwise
- **imageData** - Image in a JPG format
:rtype: boolean, byte array with jpg data
"""
if (not self._is_actor_number_valid()):
return False, None
c = CommModularContainer()
c.classID = self.ID_QCAR
c.actorNumber = self.actorNumber
c.actorFunction = self.FCN_QCAR_CAMERA_DATA_REQUEST
c.payload = bytearray(struct.pack(">I", camera))
c.containerSize = c.BASE_CONTAINER_SIZE + len(c.payload)
self._qlabs.flush_receive()
if (self._qlabs.send_container(c)):
c = self._qlabs.wait_for_container(self.ID_QCAR, self.actorNumber, self.FCN_QCAR_CAMERA_DATA_RESPONSE)
if (c == None):
return False, None
jpg_buffer = cv2.imdecode(np.frombuffer(bytearray(c.payload[8:len(c.payload)]), dtype=np.uint8, count=-1, offset=0), 1)
return True, jpg_buffer
else:
return False, None
[docs] def get_lidar(self, samplePoints=400):
"""
Request LIDAR data from a QCar.
:param samplePoints: (Optional) Change the number of points per revolution of the LIDAR.
:type samplePoints: uint32
:return: `True`, angles in radians, and distances in m if successful, `False`, none, and none otherwise
:rtype: boolean, float array, float array
"""
if (not self._is_actor_number_valid()):
return False, None, None
LIDAR_SAMPLES = 4096
LIDAR_RANGE = 80*self._sensor_scaling
# The LIDAR is simulated by using 4 orthogonal virtual cameras that are 1 pixel high. The
# lens distortion of these cameras must be removed to accurately calculate the XY position
# of the depth samples.
quarter_angle = np.linspace(0, 45, int(LIDAR_SAMPLES/8))
lens_curve = -0.0077*quarter_angle*quarter_angle + 1.3506*quarter_angle
lens_curve_rad = lens_curve/180*np.pi
angles = np.concatenate((np.pi*4/2-1*np.flip(lens_curve_rad), \
lens_curve_rad, \
(np.pi/2 - 1*np.flip(lens_curve_rad)), \
(np.pi/2 + lens_curve_rad), \
(np.pi - 1*np.flip(lens_curve_rad)), \
(np.pi + lens_curve_rad), \
(np.pi*3/2 - 1*np.flip(lens_curve_rad)), \
(np.pi*3/2 + lens_curve_rad)))
c = CommModularContainer()
c.classID = self.ID_QCAR
c.actorNumber = self.actorNumber
c.actorFunction = self.FCN_QCAR_LIDAR_DATA_REQUEST
c.payload = bytearray()
c.containerSize = c.BASE_CONTAINER_SIZE + len(c.payload)
self._qlabs.flush_receive()
if (self._qlabs.send_container(c)):
c = self._qlabs.wait_for_container(self.ID_QCAR, self.actorNumber, self.FCN_QCAR_LIDAR_DATA_RESPONSE)
if (c == None):
return False, None, None
if ((len(c.payload)-4)/2 != LIDAR_SAMPLES):
#print("Received {} bytes, expected {}".format(len(c.payload), LIDAR_SAMPLES*2))
return False, None, None
distance = np.linspace(0,0,LIDAR_SAMPLES)
for count in range(LIDAR_SAMPLES-1):
# clamp any value at 65535 to 0
raw_value = ((c.payload[4+count*2] * 256 + c.payload[5+count*2] )) %65535
# scale to LIDAR range
distance[count] = (raw_value/65535)*LIDAR_RANGE
# Resample the data using a linear radial distribution to the desired number of points
# and realign the first index to be 0 (forward)
sampled_angles = np.linspace(0,2*np.pi, num=samplePoints, endpoint=False)
sampled_distance = np.linspace(0,0, samplePoints)
index_raw = 512
for count in range(samplePoints):
while (angles[index_raw] < sampled_angles[count]):
index_raw = (index_raw + 1) % 4096
if index_raw != 0:
if (angles[index_raw]-angles[index_raw-1]) == 0:
sampled_distance[count] = distance[index_raw]
else:
sampled_distance[count] = (distance[index_raw]-distance[index_raw-1])*(sampled_angles[count]-angles[index_raw-1])/(angles[index_raw]-angles[index_raw-1]) + distance[index_raw-1]
else:
sampled_distance[count] = distance[index_raw]
return True, sampled_angles, sampled_distance
else:
return False, None, None