STU3-Sim/RoverControl_Keyboard.py at main · SimDynamX/STU3-Sim · 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
90
91
# THIS COMMENT LINE SHOULD BE THE FIRST LINE OF THE FILE
# DON'T CHANGE ANY OF THE BELOW; NECESSARY FOR JOINING SIMULATION
import os, sys, time, datetime, traceback
import spaceteams as st
def custom_exception_handler(exctype, value, tb):
    error_message = "".join(traceback.format_exception(exctype, value, tb))
    st.logger_fatal(error_message)
    exit(1)
sys.excepthook = custom_exception_handler
st.connect_to_sim(sys.argv)
import numpy as np
# DON'T CHANGE ANY OF THE ABOVE; NECESSARY FOR JOINING SIMULATION
################################################################

st.OnScreenLogMessage("RoverControl_Keyboard started properly.", "RoverControl_Keyboard", st.Severity.Info)

from pynput import keyboard


def clamp(val, minimum, maximum):
    return min(maximum, max(val, minimum))


controlled_entity: st.Entity = st.GetThisSystem().GetParam(st.VarType.entityRef, "ControlledEntity")


def on_press(key : keyboard.Key | keyboard.KeyCode):
    try:
        if key == keyboard.Key.up:
            controlled_entity.SetParam(st.VarType.double, ["ControlCmd", "Accelerator"], 1.0)

        if key == keyboard.Key.down:
            controlled_entity.SetParam(st.VarType.double, ["ControlCmd", "Brake"], 1.0)

        if key == keyboard.Key.left:
            controlled_entity.SetParam(st.VarType.double, ["ControlCmd", "SteerRight"], -1.0)

        if key == keyboard.Key.right:
            controlled_entity.SetParam(st.VarType.double, ["ControlCmd", "SteerRight"], 1.0)

        if key == keyboard.Key.shift_r:
            controlled_entity.SetParam(st.VarType.bool, ["ControlCmd", "Handbrake"], True)

        if hasattr(key, 'char'):
            if key.char == 'f' or key.char == 'F':
                controlled_entity.SetParam(st.VarType.bool, ["ControlCmd", "Flip"], True)

        #     if key.char == 'p' or key.char == 'P':
        #         beginTimeWarpPayload: st.ParamMap = st.ParamMap()
        #         beginTimeWarpPayload.AddParam(st.VarType.entityRef, "Rover", controlled_entity)
        #         st.SimGlobals.DispatchEvent("BeginTimeWarp", beginTimeWarpPayload)

    except AttributeError:
        st.logger_info('special key {0} pressed'.format(key))


def on_release(key):
    try:
        if key == keyboard.Key.up:
            controlled_entity.SetParam(st.VarType.double, ["ControlCmd", "Accelerator"], 0.0)

        if key == keyboard.Key.down:
            controlled_entity.SetParam(st.VarType.double, ["ControlCmd", "Brake"], 0.0)

        if key == keyboard.Key.left:
            controlled_entity.SetParam(st.VarType.double, ["ControlCmd", "SteerRight"], 0.0)

        if key == keyboard.Key.right:
            controlled_entity.SetParam(st.VarType.double, ["ControlCmd", "SteerRight"], 0.0)

        if key == keyboard.Key.shift_r:
            controlled_entity.SetParam(st.VarType.bool, ["ControlCmd", "Handbrake"], False)

        if key.char == 'f' or key.char == 'F':
            controlled_entity.SetParam(st.VarType.bool, ["ControlCmd", "Flip"], False)

    except AttributeError:
        st.logger_info('special key {0} pressed'.format(key))

    # if key == keyboard.Key.esc:
    #     # Stop listener
    #     st.logger_info("Program ended")
    #     return False


with keyboard.Listener(
        on_press=on_press,
        on_release=on_release) as listener:
    listener.join()

st.leave_sim()