BWSI-RACECAR-Team-7-Presentable-Repo/drive_and_foto.py at main · Vodiboi/BWSI-RACECAR-Team-7-Presentable-Repo · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
"""
MIT BWSI Autonomous RACECAR
MIT License

File Name: drive_and_foto.py

Title: Photography Code

Author: Team 7

Purpose: Code for taking photos for training object detection models.

Expected Outcome: When right trigger is pressed, photo is taken (refresh rate of 2 Hz). Use joysticks to drive the car manually.
"""
########################################################################################
# Imports
########################################################################################

import sys

# If this file is nested inside a folder in the labs folder, the relative path should
# be [1, ../../library] instead.
sys.path.insert(1, '../../library')
import racecar_core
from PIL import Image
import os
import cv2

########################################################################################
# Global variables
########################################################################################

rc = racecar_core.create_racecar()

# Declare any global variables here
global speed
global angle
global speed_offset
global angle_offset

########################################################################################
# Functions
########################################################################################

# [FUNCTION] The start function is run once every time the start button is pressed
def start():
    global speed
    global angle
    global speed_offset
    global angle_offset

    speed = 0.0 # The initial speed is at 1.0
    angle = 0.0 # The initial turning angle away from the center is at 0.0

    # This tells the car to begin at a standstill
    rc.drive.stop()

diff = 0
def update():
    global speed
    global angle
    global speed_offset
    global angle_offset
    global diff

    (x, y) = rc.controller.get_joystick(rc.controller.Joystick.LEFT)
    (x2, y2) = rc.controller.get_joystick(rc.controller.Joystick.RIGHT)
    angle = x2
    speed = y
    diff -= rc.get_delta_time()
    if rc.controller.get_trigger(rc.controller.Trigger.RIGHT) and diff < 0:
        print("Taking Foto")
        img = rc.camera.get_color_image()
        rgb_image = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
        pil_img = Image.fromarray(rgb_image)
        num_files = len(os.listdir("images/"))
        pil_img.save(f"images/photo_new_sign_{num_files}.jpg")
        diff = 0.5

    # Send the speed and angle values to the RACECAR
    rc.drive.set_speed_angle(speed, angle)

########################################################################################
# DO NOT MODIFY: Register start and update and begin execution
########################################################################################

if __name__ == "__main__":
    rc.set_start_update(start, update)
    rc.go()