class Car4W:
def __init__(self,
tyres,
sensors=[],
cameras=[],
timeframe=0.1,
car_speed=50):
logger.debug("Car4W.__init__()")
self.action_id = 0
self.t = None
self.camera = None
self.timeframe = timeframe
self.car_speed = car_speed
if len(tyres) != 4:
raise EnvironmentError("Car4W required four tyres")
self.state = 0
self.frontRight = tyres[0][1]
self.frontLeft = tyres[1][1]
self.backRight = tyres[2][1]
self.backLeft = tyres[3][1]
self.stop() # Stop the car, reset pins.
self.collision = Collision(sensors)
self.collision.start()
logger.debug("Waiting to collision sensor to get ready")
while not self.collision.ready:
sleep(0.5)
logger.info("Collision sensors are ready.")
logger.debug("Waiting to camera to get ready")
self.cameras = cameras
for camera in self.cameras:
camera[1].start()
logger.debug("Camera start called")
for camera in self.cameras:
while not camera[1].more():
sleep(0.5)
logger.debug(camera[0] + " is ready.")
logger.info("Cameras are ready.")
def speed(self, speedup=1.5):
percent = self.car_speed * speedup
if percent > 100:
percent = 100
self.frontRight.speed(percent)
self.frontLeft.speed(percent)
self.backRight.speed(percent)
self.backLeft.speed(percent)
def set_timeframe(self, timeframe):
self.timeframe = timeframe
def is_idle(self):
return self.state in (State.IDLE, State.STOPPED)
def take_action(self, method_name):
logger.debug("Taking action, " + method_name)
#self.action_id += 1 # This function is called from Websocket
method = getattr(self, method_name)
method()
def get_state(self):
return self.state
def get_state_vector(
self,
latest=False,
for_network=None
): # for_network convert numpy to list because it is faster for pickle
start_time = time.time()
state = {}
# ADDING: Sensors details
sensors = self.collision.get(latest)
state['sensors'] = sensors
# ADDING: Camera data
fps = []
for camera in self.cameras:
name = camera[0]
fps.append([camera[1].fps, camera[1].Q.qsize()])
frame = camera[1].get_frame(latest)
if for_network == False: # It is for Desktop GUI (Python)
frame = [
str(frame.dtype),
base64.b64encode(frame).decode("utf-8"), frame.shape
]
elif for_network == True:
stream = cStringIO.StringIO()
frame = frame[:, :, ::-1]
#frame = np.roll(frame, 1, axis=-1) # BGR to RGB
frame = Image.fromarray(frame,
'RGB') # Takes 150 ms - Slow RPS
frame.save(stream, format="JPEG")
frame = base64.b64encode(stream.getvalue()).decode("utf-8")
state[name] = frame
logger.debug("All Camera Info [FPS, QSize]: {}".format(fps))
logger.debug("Received State in {} seconds".format(time.time() -
start_time))
return state # converted to pickle in State.py
def forward(self):
self.speed(1)
self.state = State.FORWARD
self.frontLeft.forward()
self.frontRight.forward()
self.backLeft.forward()
self.backRight.forward()
def backward(self):
self.speed(1)
self.state = State.BACKWARD
self.frontLeft.backward()
self.frontRight.backward()
self.backLeft.backward()
self.backRight.backward()
def idle(self):
self.state = State.IDLE
self.stop()
def stop(self):
self.speed(1)
self.state = State.STOPPED
self.frontLeft.stop()
self.frontRight.stop()
self.backLeft.stop()
self.backRight.stop()
def forwardRight(self):
self.speed(1.5)
self.frontLeft.forward()
self.backLeft.forward()
self.frontRight.backward()
self.backRight.backward()
self.state = State.FORWARD_RIGHT
def forwardLeft(self):
self.speed(1.5)
self.frontRight.forward()
self.backRight.forward()
self.frontLeft.backward()
self.backLeft.backward()
self.state = State.FORWARD_LEFT
def backwardRight(self):
self.speed(1.5)
self.state = State.BACKWARD_RIGHT
self.frontLeft.backward()
self.backLeft.backward()
self.frontRight.forward()
self.backRight.forward()
def backwardLeft(self):
self.speed(1.5)
self.state = State.BACKWARD_LEFT
self.frontRight.backward()
self.backRight.backward()
self.frontLeft.forward()
self.backLeft.forward()
def close(self):
self.stop()
self.collision.stop()
for camera in self.cameras:
camera[1].stop()
def test(self):
from time import sleep
self.forward()
sleep(0.5)
self.stop()
sleep(1)
self.backward()
sleep(0.5)
self.stop()
sleep(1)
self.forwardRight()
sleep(0.5)
self.stop()
sleep(1)
self.forwardLeft()
sleep(0.5)
self.stop()
sleep(1)
self.backwardLeft()
sleep(1)
self.stop()
sleep(1)
self.backwardRight()
sleep(1)
self.stop()
def __del__(self):
GPIO.cleanup()
