QCar 2

../_images/QCar2_Banner.png

Description

QCars are considered “actors” in Quanser Interactive Labs. The QCar library can be used to acquire sensor data from the virtual environment and controls the motion of the vehicles.

See the QCar 2 Tutorial to get a better understanding of using QCars in Quanser Interactive Labs.

Library

class qvl.qcar2.QLabsQCar2(qlabs, verbose=False)[source]

This class is for spawning QCars.

Constants

QLabsQCar2.ID_QCAR = 161

Class ID

Note

CSI camera resolution is 820x410.

QLabsQCar2.CAMERA_CSI_RIGHT = 0
QLabsQCar2.CAMERA_CSI_BACK = 1
QLabsQCar2.CAMERA_CSI_LEFT = 2
QLabsQCar2.CAMERA_CSI_FRONT = 3

Note

RGB and depth resolution is 640x480.

QLabsQCar2.CAMERA_RGB = 4
QLabsQCar2.CAMERA_DEPTH = 5

Note

Overhead and trailing cameras support user zoom using the mouse wheel.

QLabsQCar2.CAMERA_OVERHEAD = 6
QLabsQCar2.CAMERA_TRAILING = 7

Member Variables

QLabsQCar2.actorNumber = None

The current actor number of this class to be addressed. This will be set by spawn methods and cleared by destroy methods. It will not be modified by the destroy all actors. This can be manually altered at any time to use one object to address multiple actors.

Methods

QLabsQCar2.set_transform_and_request_state(location, rotation, enableDynamics, headlights, leftTurnSignal, rightTurnSignal, brakeSignal, reverseSignal, waitForConfirmation=True)[source]

Sets the location, rotation, and other car properties. Note that setting the location ignores collisions so ensure that the location is free of obstacles that may trap the actor if it is subsequently used in a dynamic mode. This transform can also be used to “playback” previously recorded position data without the need for a full dynamic model.

Parameters

  • location (float array[3]) – An array of floats for x, y and z coordinates in full-scale units. Multiply physical QCar locations by 10 to get full scale locations.

  • rotation (float array[3]) – An array of floats for the roll, pitch, and yaw in radians

  • enableDynamics (boolean/int) – Enables or disables gravity for set transform requests.

  • headlights (boolean/int) – If the type is a boolean set all the headlights. If the type is an int, it will be treated as a bit mask for individual light control (b0: left outside, b1: left middle, b2: left inside, b3: right outside, b4: right middle, b5: right inside).

  • leftTurnSignal (boolean/int) – If the type is a boolean set all the left turn signals. If the type is an int, it will be treated as a bit mask for individual light control (b0: front, b1: rear).

  • rightTurnSignal (boolean/int) – If the type is a boolean set all the right turn signals. If the type is an int, it will be treated as a bit mask for individual light control (b0: front, b1: rear).

  • brakeSignal (boolean/int) – This does not affect the motion of the vehicle. If the type is a boolean set all the brake lights. If the type is an int, it will be treated as a bit mask for individual light control (b0: left outside, b1: left inside, b2: right outside, b3: right inside).

  • reverseSignal (boolean/int) – If the type is a boolean set all the reverse lights. If the type is an int, it will be treated as a bit mask for individual light control (b0: left, b1: right).

  • waitForConfirmation (boolean) – (Optional) Wait for confirmation before proceeding. This makes the method a blocking operation. NOTE: Return data will only be valid if waitForConfirmation is True.

Returns

  • status - True if successful or False otherwise

  • location - in full scale

  • rotation - in radians

  • forward vector - unit scale vector

  • up vector - unit scale vector

  • 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

Return type

boolean, float array[3], float array[3], float array[3], float array[3], boolean, boolean

QLabsQCar2.set_transform_and_request_state_degrees(location, rotation, enableDynamics, headlights, leftTurnSignal, rightTurnSignal, brakeSignal, reverseSignal, waitForConfirmation=True)[source]

Sets the location, rotation, and other car properties. Note that setting the location ignores collisions so ensure that the location is free of obstacles that may trap the actor if it is subsequently used in a dynamic mode. This transform can also be used to “playback” previously recorded position data without the need for a full dynamic model.

Parameters

  • location (float array[3]) – An array of floats for x, y and z coordinates in full-scale units. Multiply physical QCar locations by 10 to get full scale locations.

  • rotation (float array[3]) – An array of floats for the roll, pitch, and yaw in degrees

  • enableDynamics (boolean) – (default True) Enables or disables gravity for set transform requests.

  • headlights (boolean/int) – If the type is a boolean set all the headlights. If the type is an int, it will be treated as a bit mask for individual light control (b0: left outside, b1: left middle, b2: left inside, b3: right outside, b4: right middle, b5: right inside).

  • leftTurnSignal (boolean/int) – If the type is a boolean set all the left turn signals. If the type is an int, it will be treated as a bit mask for individual light control (b0: front, b1: rear).

  • rightTurnSignal (boolean/int) – If the type is a boolean set all the right turn signals. If the type is an int, it will be treated as a bit mask for individual light control (b0: front, b1: rear).

  • brakeSignal (boolean/int) – This does not affect the motion of the vehicle. If the type is a boolean set all the brake lights. If the type is an int, it will be treated as a bit mask for individual light control (b0: left outside, b1: left inside, b2: right outside, b3: right inside).

  • reverseSignal (boolean/int) – If the type is a boolean set all the reverse lights. If the type is an int, it will be treated as a bit mask for individual light control (b0: left, b1: right).

  • waitForConfirmation (boolean) – (Optional) Wait for confirmation before proceeding. This makes the method a blocking operation. NOTE: Return data will only be valid if waitForConfirmation is True.

Returns

  • status - True if successful or False otherwise

  • location - in full scale

  • rotation - in radians

  • forward vector - unit scale vector

  • up vector - unit scale vector

  • 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

Return type

boolean, float array[3], float array[3], float array[3], float array[3], boolean, boolean

QLabsQCar2.set_velocity_and_request_state(forward, turn, headlights, leftTurnSignal, rightTurnSignal, brakeSignal, reverseSignal)[source]

Sets the velocity, turn angle in radians, and other car properties.

Parameters

  • forward (float) – Speed in m/s of a full-scale car. Multiply physical QCar speeds by 10 to get full scale speeds.

  • turn (float) – Turn angle in radians. Positive values turn right.

  • headlights (boolean/int) – If the type is a boolean set all the headlights. If the type is an int, it will be treated as a bit mask for individual light control (b0: left outside, b1: left middle, b2: left inside, b3: right outside, b4: right middle, b5: right inside).

  • leftTurnSignal (boolean/int) – If the type is a boolean set all the left turn signals. If the type is an int, it will be treated as a bit mask for individual light control (b0: front, b1: rear).

  • rightTurnSignal (boolean/int) – If the type is a boolean set all the right turn signals. If the type is an int, it will be treated as a bit mask for individual light control (b0: front, b1: rear).

  • brakeSignal (boolean/int) – This does not affect the motion of the vehicle. If the type is a boolean set all the brake lights. If the type is an int, it will be treated as a bit mask for individual light control (b0: left outside, b1: left inside, b2: right outside, b3: right inside).

  • reverseSignal (boolean/int) – If the type is a boolean set all the reverse lights. If the type is an int, it will be treated as a bit mask for individual light control (b0: left, b1: right).

Returns

  • 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

Return type

boolean, float array[3], float array[3], boolean, boolean

QLabsQCar2.set_velocity_and_request_state_degrees(forward, turn, headlights, leftTurnSignal, rightTurnSignal, brakeSignal, reverseSignal)[source]

Sets the velocity, turn angle in degrees, and other car properties.

Parameters

  • forward – Speed in m/s of a full-scale car. Multiply physical QCar speeds by 10 to get full scale speeds.

  • turn (float) – Turn angle in degrees. Positive values turn right.

  • headlights (boolean) – Enable the headlights

  • leftTurnSignal (boolean) – Enable the left turn signal

  • rightTurnSignal (boolean) – Enable the right turn signal

  • brakeSignal (boolean) – Enable the brake lights (does not affect the motion of the vehicle)

  • reverseSignal (boolean) – Play a honking sound

Returns

  • 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

Return type

boolean, float array[3], float array[3], boolean, boolean

QLabsQCar2.possess(camera=CAMERA_TRAILING)[source]

Possess (take control of) a QCar in QLabs with the selected camera.

Parameters

camera (uint32) – Pre-defined camera constant. See CAMERA constants for available options. Default is the trailing camera.

Returns

  • status - True if possessing the camera was successful, False otherwise

Return type

boolean

QLabsQCar2.ghost_mode(enable=True, color=[0, 1, 0])[source]

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.

Parameters

  • enable (boolean) – Set the QCar to the defined transparent color, otherwise revert to the solid color scheme.

  • color (float array[3]) – Red, Green, Blue components of the RGB color on a 0.0 to 1.0 scale.

Returns

  • status - True if successful, False otherwise

Return type

boolean

QLabsQCar2.set_led_strip_uniform(color=[0, 0, 0], waitForConfirmation=True)[source]

Sets the entire LED color strip to the RGB value specified.

Parameters

color (float array[3]) – Red, Green, Blue components of the RGB color on a 0.0 to 1.0 scale. Values greater than 1 can be used to enhance the glow or bloom effect (try 50).

Returns

  • status - True if successful, False otherwise

Return type

boolean

QLabsQCar2.set_led_strip_individual(color, waitForConfirmation=True)[source]

Sets the 33 LEDs in the color strip individually. Note that specifying individual LED’s has a slight impact on performance versus set_led_strip_uniform.

Parameters

color (float array[3][33]) – A 2D array Red, Green, Blue components of the RGB color on a 0.0 to 1.0 scale by 33 rows. Values greater than 1 can be used to enhance the glow or bloom effect (try 50).

Returns

  • status - True if successful, False otherwise

Return type

boolean

QLabsQCar2.get_image(camera)[source]

Request a JPG image from one of the QCar cameras.

Parameters

camera (uint32) – Pre-defined camera constant. See CAMERA constants for available options. Trailing and Overhead cameras cannot be selected.

Returns

  • status - True and image data if successful, False and empty otherwise

  • imageData - Image in a JPG format

Return type

boolean, byte array with jpg data

QLabsQCar2.get_lidar(samplePoints=400)[source]

Request LIDAR data from a QCar.

Parameters

samplePoints (uint32) – (Optional) Change the number of points per revolution of the LIDAR.

Returns

True, angles in radians, and distances in m if successful, False, none, and none otherwise

Return type

boolean, float array, float array

Parent Class (actor.py) Methods

QLabsQCar2.__init__(qlabs, verbose=False)[source]

Constructor Method

Parameters

  • qlabs (object) – A QuanserInteractiveLabs object

  • verbose (boolean) – (Optional) Print error information to the console.

QLabsQCar2.spawn(location=[0, 0, 0], rotation=[0, 0, 0], scale=[1, 1, 1], configuration=0, waitForConfirmation=True)[source]

Spawns a new QCar actor with the next available actor number within this class.

Parameters

  • location (float array[3]) – (Optional) An array of floats for x, y and z coordinates

  • rotation (float array[3]) – (Optional) An array of floats for the roll, pitch, and yaw in radians

  • scale (float array[3]) – (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].

  • configuration (uint32) – (Optional) Spawn configuration. See class library for configuration options.

  • waitForConfirmation (boolean) – (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.

Returns

  • status - 0 if successful, 1 class not available, 3 unknown error, -1 communications error.

  • actorNumber - An actor number to use for future references.

Return type

int32, int32

QLabsQCar2.spawn_degrees(location=[0, 0, 0], rotation=[0, 0, 0], scale=[1, 1, 1], configuration=0, waitForConfirmation=True)[source]

Spawns a new QCar actor with the next available actor number within this class.

Parameters

  • location (float array[3]) – (Optional) An array of floats for x, y and z coordinates

  • rotation (float array[3]) – (Optional) An array of floats for the roll, pitch, and yaw in degrees

  • scale (float array[3]) – (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].

  • configuration (uint32) – (Optional) Spawn configuration. See class library for configuration options.

  • waitForConfirmation (boolean) – (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.

Returns

  • status - 0 if successful, 1 class not available, 3 unknown error, -1 communications error.

  • actorNumber - An actor number to use for future references.

Return type

int32, int32

QLabsQCar2.spawn_id(actorNumber, location=[0, 0, 0], rotation=[0, 0, 0], scale=[1, 1, 1], configuration=0, waitForConfirmation=True)[source]

Spawns a new QCar actor.

Parameters

  • actorNumber (uint32) – User defined unique identifier for the class actor in QLabs

  • location (float array[3]) – (Optional) An array of floats for x, y and z coordinates

  • rotation (float array[3]) – (Optional) An array of floats for the roll, pitch, and yaw in radians

  • scale (float array[3]) – (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].

  • configuration (uint32) – (Optional) Spawn configuration. See class library for configuration options.

  • waitForConfirmation (boolean) – (Optional) Make this operation blocking until confirmation of the spawn has occurred.

Returns

  • status - 0 if successful, 1 class not available, 2 actor number not available or already in use, 3 unknown error, -1 communications error

Return type

int32

QLabsQCar2.spawn_id_degrees(actorNumber, location=[0, 0, 0], rotation=[0, 0, 0], scale=[1, 1, 1], configuration=0, waitForConfirmation=True)[source]

Spawns a new QCar actor.

Parameters

  • actorNumber (uint32) – User defined unique identifier for the class actor in QLabs

  • location (float array[3]) – (Optional) An array of floats for x, y and z coordinates

  • rotation (float array[3]) – (Optional) An array of floats for the roll, pitch, and yaw in radians

  • scale (float array[3]) – (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].

  • configuration (uint32) – (Optional) Spawn configuration. See class library for configuration options.

  • waitForConfirmation (boolean) – (Optional) Make this operation blocking until confirmation of the spawn has occurred.

Returns

  • status - 0 if successful, 1 class not available, 2 actor number not available or already in use, 3 unknown error, -1 communications error

Return type

int32

QLabsQCar2.destroy()

Find and destroy a specific actor. This is a blocking operation.

Returns

  • numActorsDestroyed - The number of actors destroyed. -1 if failed.

Return type

int32

QLabsQCar2.destroy_all_actors_of_class()

Find and destroy all actors of this class. This is a blocking operation.

Returns

  • numActorsDestroyed - The number of actors destroyed. -1 if failed.

Return type

int32

QLabsQCar2.ping()

Checks if the actor is still present in the environment. Note that if you did not spawn the actor with one of the spawn functions, you may need to manually set the actorNumber member variable.

Returns

  • status - True if successful, False otherwise

Return type

boolean

QLabsQCar2.get_world_transform()

Get the location, rotation, and scale in world coordinates of the actor.

Returns

  • status - True if successful, False otherwise

  • location

  • rotation

  • scale

Return type

boolean, float array[3], float array[3], float array[3]

QLabsQCar2.get_world_transform_degrees()

Get the location, rotation, and scale in world coordinates of the actor.

Returns

  • status - True if successful, False otherwise

  • location

  • rotation

  • scale

Return type

boolean, float array[3], float array[3], float array[3]

Configurations

There is only one configuration of the QCar actor.

Connection Points

Image (Click for enlarged view)

Reference Frame Number

Parent Frame

Relative Transform to Parent (Location, Rotation)

Description

conn0

0

The base frame is located at ground level, centered between the two rear wheels.

conn1

1

0

[ 1.300,0,0] [0,0,0]

Centered between the front and rear axles on the ground.

conn2

2

0

[ 1.207,0,1.880] [0,0,0]

Top center of the LIDAR

Component Extrinsics

“Extrinsics” refer to the external relationship of an object with respect to a specific frame of reference (in this case the body center of the QCar). Sometimes it’s important to know specific distances and orientation of extrinsic components, for instance, this can be use for obstacle detection and camera calibration. You will find a list of the important extrinsics below.

Distances From Body Frame

The body frame is located between the front and rear axles on the ground plane. Distances of the QCar 2 in its virtual environment are 10 times larger than on the physical system so a QCar 2 spawned at a scale of 1 is equivalent size to a full-scale automobile. A QCar spawned at a scale of 0.1 will be equivalent to the size of a physical QCar.

The RealSense camera distance is considered from the front face of the camera at the location of the RGB sensor.

../_images/qcar2_bodyframe.png

Component

x (m)

y (m)

z (m)

CG

-0.11

0.029

0.814

Front axle

1.30

0

0.31

Rear axle

-1.30

0

0.31

CSI front

1.83

0

1.10

CSI left

0.12

0.33

1.10

CSI rear

-1.52

0

1.10

CSI right

0.12

-0.53

1.10

IMU

0.11

0

0.89

RealSense

0.95

0.32

1.72

RPLIDAR

-0.12

-0

1.93

Transformation Matrices

All transformation matrices are built off of the body frame and sensor frames for the QCar 2.

To read more about this check out our documentation here by clicking on resources button and looking inside the zip folder for src/user_manuals/qcar2/user_manual_system_hardware.pdf

../_images/extrinsics_qc2.png

QCar 2 Tutorial

Python Tutorial

Raw to download this tutorial: QCar 2 Tutorial (.py).

  1"""
  2QCar 2 Library Example
  3----------------------
  4This example will show you how to spawn cars, and use the qvl library commands
  5to control the car and its related functions.
  6
  7.. note::
  8
  9    Make sure you have Quanser Interactive Labs open before running this
 10    example.  This example is designed to best be run in QCar Cityscape.
 11
 12"""
 13
 14import sys
 15
 16from qvl.qlabs import QuanserInteractiveLabs
 17from qvl.free_camera import QLabsFreeCamera
 18from qvl.basic_shape import QLabsBasicShape
 19from qvl.qcar2 import QLabsQCar2
 20from qvl.environment_outdoors import QLabsEnvironmentOutdoors
 21
 22import time
 23import math
 24import numpy as np
 25import cv2
 26import os
 27
 28import pyqtgraph as pg
 29from pyqtgraph.Qt import QtWidgets
 30
 31from qvl.system import QLabsSystem
 32
 33
 34
 35def main():
 36    os.system('cls')
 37
 38    #Communications with qlabs
 39
 40    qlabs = QuanserInteractiveLabs()
 41    cv2.startWindowThread()
 42
 43    print("Connecting to QLabs...")
 44    if (not qlabs.open("localhost")):
 45        print("Unable to connect to QLabs")
 46        return    
 47
 48    print("Connected")
 49
 50    qlabs.destroy_all_spawned_actors()
 51
 52    # Use hSystem to set the tutorial title in the upper left of the qlabs window 
 53    hSystem = QLabsSystem(qlabs)
 54    hSystem.set_title_string('QCar Tutorial')
 55
 56    ### QCar
 57
 58    hCameraQCars = QLabsFreeCamera(qlabs)
 59    hCameraQCars.spawn_id(actorNumber=1, location=[-15.075, 26.703, 6.074], rotation=[0, 0.564, -1.586])
 60    hCameraQCars.possess()
 61
 62    print("\n\n---QCar---")
 63
 64    #spawning the QCar with radians
 65    hQCar0 = QLabsQCar2(qlabs)
 66    hQCar0.spawn_id(actorNumber=0, location=[-8.700, 14.643, 0.005], rotation=[0,0,math.pi/2], waitForConfirmation=True)
 67
 68    #Spawn and destroy the existing QCar
 69    hQCar1 = QLabsQCar2(qlabs)
 70    hQCar1.spawn_id(actorNumber=1, location=[-15.075, 26.703, 6.074], rotation=[0,0,math.pi/2], waitForConfirmation=True)
 71    hQCar1.destroy()
 72
 73    #spawn a QCar with degrees
 74    hQCar2 = QLabsQCar2(qlabs)
 75    x = hQCar2.spawn_id_degrees(actorNumber=2, location=[-11.048, 14.643, 0.005], rotation=[0,0,90], waitForConfirmation=True)
 76
 77    #pinging the QCar
 78    hQCar2.ping()
 79
 80    time.sleep(0.5)
 81
 82    LEDCam = QLabsFreeCamera(qlabs)
 83    LEDCam.spawn(location=[-12.705, 19.139, 1.748], rotation=[0, 0.357, -0.803])
 84    LEDCam.possess()
 85    time.sleep(1)
 86    # sets LEDS of the QCar to blue on different brightness and sets as green at the end.
 87    hQCar2.set_led_strip_uniform(color=[0,0,.2])
 88    time.sleep(1)
 89    hQCar2.set_led_strip_uniform(color=[0,0,1], waitForConfirmation=True)
 90    time.sleep(1)
 91    hQCar2.set_led_strip_uniform(color=[0,0,30])
 92    time.sleep(1)
 93    hQCar2.set_led_strip_uniform(color=[0,1,0])
 94    time.sleep(1)
 95
 96    hCameraQCars.possess()
 97
 98    # Set the velocity and direction of the QCar in radians while also turning on the headlights and right turn signal
 99    hQCar2.set_velocity_and_request_state(forward=1, turn = -math.pi/6, headlights=True, leftTurnSignal=False, rightTurnSignal=True, brakeSignal=False, reverseSignal=False)
100    time.sleep(1)
101    # Set the velocity to 0 and direction of the QCar in radians while keeping the headlights on and right turn signal on
102    hQCar2.set_velocity_and_request_state(forward=0.0, turn = -math.pi/6, headlights=True, leftTurnSignal=False, rightTurnSignal=True, brakeSignal=False, reverseSignal=False)
103    # Set the velocity to 1 and direction of the QCar in degrees while keeping the headlights on and turning on the left turn signal
104    hQCar2.set_velocity_and_request_state_degrees(forward=1, turn = 30, headlights=True, leftTurnSignal=True, rightTurnSignal=False, brakeSignal=False, reverseSignal=False)
105    time.sleep(1)
106    # Set the velocity to 0 and direction of the QCar in degrees while keeping the headlights on and left turn signal on
107    success, location, rotation, frontHit, rearHit = hQCar2.set_velocity_and_request_state_degrees(forward=0.0, turn = 30, headlights=True, leftTurnSignal=True, rightTurnSignal=False, brakeSignal=False, reverseSignal=False)
108
109    # Possess another QCar
110    x = hQCar2.possess()
111
112    time.sleep(0.1)
113    # Set the velocity to 1 of the QCar in radians while keeping the headlights, brakeSignal and reverseSignal on
114    hQCar2.set_velocity_and_request_state(forward=1, turn = 0, headlights=True, leftTurnSignal=True, rightTurnSignal=True, brakeSignal=True, reverseSignal=True)
115    time.sleep(1)
116    # Set the velocity to 0 while keeping the headlights, brakeSignal and reverseSignal on and turning on the left turn signal and right turn signal
117    hQCar2.set_velocity_and_request_state(forward=0.0, turn = 0, headlights=True, leftTurnSignal=True, rightTurnSignal=True, brakeSignal=True, reverseSignal=True)
118
119    # Turn all the lights off
120    hQCar2.set_velocity_and_request_state(forward=0, turn = 0, headlights=False, leftTurnSignal=False, rightTurnSignal=False, brakeSignal=False, reverseSignal=False)
121
122    # Car bumper test
123    hCameraQCars.possess()
124    # Change the camera view to see the bumper test
125    hCameraQCars.set_transform(location=[-17.045, 32.589, 6.042], rotation=[0, 0.594, -1.568])
126
127    # Spawn some shapes for our bumper test
128    hCubeQCarBlocks = QLabsBasicShape(qlabs)
129    hCubeQCarBlocks .spawn_id(100, [-11.919, 26.289, 0.5], [0,0,0], [1,1,1], configuration=hCubeQCarBlocks.SHAPE_CUBE, waitForConfirmation=True)
130    hCubeQCarBlocks .spawn_id(101, [-19.919, 26.289, 0.5], [0,0,0], [1,1,1], configuration=hCubeQCarBlocks.SHAPE_CUBE, waitForConfirmation=True)
131
132    # Create another QCar
133    hQCar3 = QLabsQCar2(qlabs)
134    hQCar3.spawn_id(actorNumber=3, location=[-13.424, 26.299, 0.005], rotation=[0,0,math.pi])
135
136    # Have the QCar drive forward to hit the front block
137    for count in range(10):
138        x, location, rotation, frontHit, rearHit  = hQCar3.set_velocity_and_request_state(forward=2, turn = 0, headlights=False, leftTurnSignal=False, rightTurnSignal=False, brakeSignal=False, reverseSignal=False)
139        time.sleep(0.25)
140
141    # Put the QCar in ghost mode
142    hQCar3.ghost_mode()
143
144    # Have the QCar drive backwards to hit the back bumper
145    for count in range(10):
146        x, location, rotation, frontHit, rearHit  = hQCar3.set_velocity_and_request_state(forward=-2, turn = 0, headlights=False, leftTurnSignal=False, rightTurnSignal=False, brakeSignal=False, reverseSignal=False)
147        time.sleep(0.25)
148
149    # Change the color of ghost mode to red
150    hQCar3.ghost_mode(enable=True, color=[1,0,0])
151    time.sleep(0.50)
152
153    # Set the velocity to 0 and turn all lights off
154    hQCar3.set_velocity_and_request_state(forward=0, turn = 0, headlights=False, leftTurnSignal=False, rightTurnSignal=False, brakeSignal=False, reverseSignal=False)
155
156    # Set the location of the QCar and request the state of the car.  If x== True and frontHit==True then the front bumper hit the block correctly.
157    x, location, rotation, forward_vector, up_vector, frontHit, rearHit = hQCar3.set_transform_and_request_state(location=[-16.1, 26.299, 0.005], rotation=[0,0,math.pi-0.01], enableDynamics=True, headlights=False, leftTurnSignal=False, rightTurnSignal=False, brakeSignal=False, reverseSignal=False)
158    time.sleep(0.50)
159
160    # Getting and saving the world transform of the QCar in Quanser Interactive Labs
161    x, loc, rot, scale = hQCar3.get_world_transform()
162
163    # Set the location of the QCar and request the state of the car.  If x== True and rearHit==True then the back bumper hit the block correctly.
164    x, location, rotation, forward_vector, up_vector, frontHit, rearHit = hQCar3.set_transform_and_request_state_degrees(location=[-13.1, 26.299, 0.005], rotation=[0,0,179], enableDynamics=True, headlights=False, leftTurnSignal=False, rightTurnSignal=False, brakeSignal=False, reverseSignal=False)
165    time.sleep(0.50)
166
167    #Turning off ghost mode for the QCar
168    hQCar3.ghost_mode(enable=False, color=[1,0,0])
169
170    # Possessing the overhead camera on the QCar
171    hQCar2.possess(hQCar2.CAMERA_OVERHEAD)
172    time.sleep(0.5)
173    
174    # Possessing the trailing camera on the QCar
175    hQCar2.possess(hQCar2.CAMERA_TRAILING)
176    time.sleep(0.5)
177    
178    #Possessing the front CSI camera on the QCar
179    hQCar2.possess(hQCar2.CAMERA_CSI_FRONT)
180    time.sleep(0.5)
181
182    # Possessing the right CSI camera on the QCar
183    hQCar2.possess(hQCar2.CAMERA_CSI_RIGHT)
184    time.sleep(0.5)
185
186    # Possessing the back CSI camera on the QCar
187    hQCar2.possess(hQCar2.CAMERA_CSI_BACK)
188    time.sleep(0.5)
189
190    # Possessing the left CSI camera on the QCar
191    hQCar2.possess(hQCar2.CAMERA_CSI_LEFT)
192    time.sleep(0.5)
193
194    # Possessing the front RealSense RGB camera on the QCar
195    hQCar2.possess(hQCar2.CAMERA_RGB)
196    time.sleep(0.5)
197
198    # Possessing the RealSense depth camera on the QCar
199    hQCar2.possess(hQCar2.CAMERA_DEPTH)
200    time.sleep(0.5)
201
202    # Getting images from the different cameras 
203    x, camera_image = hQCar2.get_image(camera=hQCar2.CAMERA_CSI_FRONT)
204    x, camera_image = hQCar2.get_image(camera=hQCar2.CAMERA_CSI_RIGHT)
205    x, camera_image = hQCar2.get_image(camera=hQCar2.CAMERA_CSI_BACK)
206    x, camera_image = hQCar2.get_image(camera=hQCar2.CAMERA_CSI_LEFT)
207    x, camera_image = hQCar2.get_image(camera=hQCar2.CAMERA_RGB)
208    x, camera_image = hQCar2.get_image(camera=hQCar2.CAMERA_DEPTH)
209
210
211    #LIDAR
212
213    hQCar3.possess(hQCar3.CAMERA_OVERHEAD)
214
215    # Creating a plot to plot the LIDAR data 
216    lidarPlot = pg.plot(title="LIDAR")
217    squareSize = 100
218    lidarPlot.setXRange(-squareSize, squareSize)
219    lidarPlot.setYRange(-squareSize, squareSize)
220    lidarData = lidarPlot.plot([], [], pen=None, symbol='o', symbolBrush='r', symbolPen=None, symbolSize=2)
221
222    time.sleep(1)
223
224    print("Reading from LIDAR... if QLabs crashes or output isn't great, make sure FPS > 100")
225    
226    # Have the QCar drive forward to hit the front block to show the live lidar.
227    # Speed can be changed by increasing or decreasing the value in the first
228    # parameter "forward"
229    hQCar3.set_velocity_and_request_state(1, 0, False, False, False, False, False)
230    lidar_rate = 0.05
231
232    # Obtaining and plotting lidar data for 0.2s
233    for count in range(25): 
234        
235        success, angle, distance = hQCar3.get_lidar(samplePoints=400)
236
237        x = np.sin(angle)*distance
238        y = np.cos(angle)*distance
239    
240        lidarData.setData(x,y)
241        QtWidgets.QApplication.instance().processEvents()
242        time.sleep(lidar_rate) #lidar_rate is set at the top of this example
243
244
245    time.sleep(5)
246
247    # Closing qlabs
248    qlabs.close()
249    print("Done!")
250
251
252main()