Skip to content

Commit

Permalink
1005 fix recording yet again (autorope#1009)
Browse files Browse the repository at this point in the history 
* fix JoystickController so it correctly latches recording state
- needed to set latch when auto-record-on-throttle changed state
- needed to set latch when toggling manual recording state
- needed to apply latched state in run_threaded

* Update logging in controller to be less verbose
- also use real logging rather then print statements.

* version=4.3.11
  • Loading branch information
@Ezward
Ezward authored Apr 21, 2022
1 parent 6c9ecb9 commit bfb4f03
Show file tree
Hide file tree
Showing 2 changed files with 51 additions and 36 deletions.
85 changes: 50 additions & 35 deletions donkeycar/parts/controller.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@
from donkeycar.parts.web_controller.web import LocalWebController
from donkeycar.parts.web_controller.web import WebFpv

logger = logging.getLogger(__name__)

class Joystick(object):
'''
Expand Down Expand Up @@ -41,25 +42,25 @@ def init(self):
except ModuleNotFoundError:
self.num_axes = 0
self.num_buttons = 0
print("no support for fnctl module. joystick not enabled.")
logger.warn("no support for fnctl module. joystick not enabled.")
return False

if not os.path.exists(self.dev_fn):
print(self.dev_fn, "is missing")
logger.warn(f"{self.dev_fn} is missing")
return False

'''
call once to setup connection to device and map buttons
'''
# Open the joystick device.
print('Opening %s...' % self.dev_fn)
logger.info(f'Opening %s... {self.dev_fn}')
self.jsdev = open(self.dev_fn, 'rb')

# Get the device name.
buf = array.array('B', [0] * 64)
ioctl(self.jsdev, 0x80006a13 + (0x10000 * len(buf)), buf) # JSIOCGNAME(len)
self.js_name = buf.tobytes().decode('utf-8')
print('Device name: %s' % self.js_name)
logger.info('Device name: %s' % self.js_name)

# Get number of axes and buttons.
buf = array.array('B', [0])
Expand Down Expand Up @@ -131,15 +132,15 @@ def poll(self):
if button:
self.button_states[button] = value
button_state = value
logging.info("button: %s state: %d" % (button, value))
logger.info("button: %s state: %d" % (button, value))

if typev & 0x02:
axis = self.axis_map[number]
if axis:
fvalue = value / 32767.0
self.axis_states[axis] = fvalue
axis_val = fvalue
logging.debug("axis: %s val: %f" % (axis, fvalue))
logger.debug("axis: %s val: %f" % (axis, fvalue))

return button, button_state, axis, axis_val

Expand All @@ -164,7 +165,7 @@ def __init__( self,
self.joystick = pygame.joystick.Joystick(which_js)
self.joystick.init()
name = self.joystick.get_name()
print("detected joystick device:", name)
logger.info(f"detected joystick device: {name}")

self.axis_states = [ 0.0 for i in range(self.joystick.get_numaxes())]
self.button_states = [ 0 for i in range(self.joystick.get_numbuttons() + self.joystick.get_numhats() * 4)]
Expand Down Expand Up @@ -203,7 +204,7 @@ def poll(self):
state = self.joystick.get_button( i )
if self.button_states[i] != state:
if not i in self.button_names:
print('button:', i)
logger.info(f'button: {i}')
continue
button = self.button_names[i]
button_state = state
Expand All @@ -220,7 +221,7 @@ def poll(self):
state = int(state)
if self.button_states[iBtn] != state:
if not iBtn in self.button_names:
print("button:", iBtn)
logger.info(f"button: {iBtn}")
continue
button = self.button_names[iBtn]
button_state = state
Expand Down Expand Up @@ -271,7 +272,7 @@ def __init__(self, cfg, debug=False):
self.pi.set_mode(channel.pin, pigpio.INPUT)
self.cbs.append(self.pi.callback(channel.pin, pigpio.EITHER_EDGE, self.cbf))
if self.debug:
print(f'RCReceiver gpio {channel.pin} created')
logger.info(f'RCReceiver gpio {channel.pin} created')


def cbf(self, gpio, level, tick):
Expand Down Expand Up @@ -318,7 +319,7 @@ def run(self, mode=None, recording=None):
self.signals[i] += self.MIN_OUT
i += 1
if self.debug:
print('RC CH1 signal:', round(self.signals[0], 3), 'RC CH2 signal:', round(self.signals[1], 3), 'RC CH3 signal:', round(self.signals[2], 3))
logger.info(f'RC CH1 signal:{round(self.signals[0], 3)}, RC CH2 signal:{round(self.signals[1], 3)}, RC CH3 signal:{round(self.signals[2], 3)}')

# check mode channel if present
if (self.signals[2] - self.jitter) > 0:
Expand Down Expand Up @@ -852,6 +853,7 @@ def __init__(self, poll_delay=0.0,
self.steering_scale = steering_scale
self.throttle_dir = throttle_dir
self.recording = False
self.recording_latch = None
self.constant_throttle = False
self.auto_record_on_throttle = auto_record_on_throttle
self.dev_fn = dev_fn
Expand Down Expand Up @@ -942,24 +944,28 @@ def erase_last_N_records(self):
if self.tub is not None:
try:
self.tub.delete_last_n_records(self.num_records_to_erase)
print('deleted last %d records.' % self.num_records_to_erase)
logger.info('deleted last %d records.' % self.num_records_to_erase)
except:
print('failed to erase')
logger.info('failed to erase')


def on_throttle_changes(self):
'''
turn on recording when non zero throttle in the user mode.
'''
if self.auto_record_on_throttle:
self.recording = (abs(self.throttle) > self.dead_zone and self.mode == 'user')
recording = (abs(self.throttle) > self.dead_zone and self.mode == 'user')
if recording != self.recording:
self.recording = recording
self.recording_latch = self.recording
logger.debug(f"JoystickController::on_throttle_changes() setting recording = {self.recording}")


def emergency_stop(self):
'''
initiate a series of steps to try to stop the vehicle as quickly as possible
'''
print('E-Stop!!!')
logger.warn('E-Stop!!!')
self.mode = "user"
self.recording = False
self.constant_throttle = False
Expand Down Expand Up @@ -1025,13 +1031,17 @@ def toggle_manual_recording(self):
toggle recording on/off
'''
if self.auto_record_on_throttle:
print('auto record on throttle is enabled.')
logger.info('auto record on throttle is enabled; ignoring toggle of manual mode.')
elif self.recording:
self.recording = False
self.recording_latch = self.recording
logger.debug(f"JoystickController::toggle_manual_recording() setting recording and recording_latch = {self.recording}")
else:
self.recording = True
self.recording_latch = self.recording
logger.debug(f"JoystickController::toggle_manual_recording() setting recording and recording_latch = {self.recording}")

print('recording:', self.recording)
logger.info(f'recording: {self.recording}')


def increase_max_throttle(self):
Expand All @@ -1045,7 +1055,7 @@ def increase_max_throttle(self):
else:
self.throttle = (self.throttle_dir * self.last_throttle_axis_val * self.throttle_scale)

print('throttle_scale:', self.throttle_scale)
logger.info(f'throttle_scale: {self.throttle_scale}')


def decrease_max_throttle(self):
Expand All @@ -1059,7 +1069,7 @@ def decrease_max_throttle(self):
else:
self.throttle = (self.throttle_dir * self.last_throttle_axis_val * self.throttle_scale)

print('throttle_scale:', self.throttle_scale)
logger.info(f'throttle_scale: {self.throttle_scale}')


def toggle_constant_throttle(self):
Expand All @@ -1074,7 +1084,7 @@ def toggle_constant_throttle(self):
self.constant_throttle = True
self.throttle = self.throttle_scale
self.on_throttle_changes()
print('constant_throttle:', self.constant_throttle)
logger.info(f'constant_throttle: {self.constant_throttle}')


def toggle_mode(self):
Expand All @@ -1091,7 +1101,7 @@ def toggle_mode(self):
else:
self.mode = 'user'
self.mode_latch = self.mode
print('new mode:', self.mode)
logger.info(f'new mode: {self.mode}')


def chaos_monkey_on_left(self):
Expand Down Expand Up @@ -1122,8 +1132,13 @@ def run_threaded(self, img_arr=None, mode=None, recording=None):
if self.mode_latch is not None:
self.mode = self.mode_latch
self.mode_latch = None
if recording is not None:
if recording is not None and recording != self.recording:
logger.debug(f"JoystickController::run_threaded() setting recording from default = {recording}")
self.recording = recording
if self.recording_latch is not None:
logger.debug(f"JoystickController::run_threaded() setting recording from latch = {self.recording_latch}")
self.recording = self.recording_latch
self.recording_latch = None

'''
process E-Stop state machine
Expand Down Expand Up @@ -1181,7 +1196,7 @@ def init_js(self):
if not self.js.init():
self.js = None
except FileNotFoundError:
print(self.dev_fn, "not found.")
logger.error(f"{self.dev_fn} not found.")
self.js = None

return self.js is not None
Expand Down Expand Up @@ -1211,7 +1226,7 @@ def init_js(self):
if not self.js.init():
self.js = None
except FileNotFoundError:
print(self.dev_fn, "not found.")
logger.error(f"{self.dev_fn} not found.")
self.js = None
return self.js is not None

Expand Down Expand Up @@ -1257,7 +1272,7 @@ def init_js(self):
if not self.js.init():
self.js = None
except FileNotFoundError:
print(self.dev_fn, "not found.")
logger.error(f"{self.dev_fn} not found.")
self.js = None
return self.js is not None

Expand Down Expand Up @@ -1289,7 +1304,7 @@ def init_js(self):
if not self.js.init():
self.js = None
except FileNotFoundError:
print(self.dev_fn, "not found.")
logger.error(f"{self.dev_fn} not found.")
self.js = None
return self.js is not None

Expand Down Expand Up @@ -1331,7 +1346,7 @@ def init_js(self):
try:
self.js = PyGamePS4Joystick(which_js=self.which_js)
except Exception as e:
print(e)
logger.error(e)
self.js = None
return self.js is not None

Expand All @@ -1355,7 +1370,7 @@ def init_js(self):
self.js = XboxOneJoystick(self.dev_fn)
self.js.init()
except FileNotFoundError:
print(self.dev_fn, "not found.")
logger.error(f"{self.dev_fn} not found.")
self.js = None
return self.js is not None

Expand Down Expand Up @@ -1439,7 +1454,7 @@ def init_js(self):
self.js = LogitechJoystick(self.dev_fn)
self.js.init()
except FileNotFoundError:
print(self.dev_fn, "not found.")
logger.error(f"{self.dev_fn} not found.")
self.js = None
return self.js is not None

Expand Down Expand Up @@ -1490,10 +1505,10 @@ def on_dpad_down(self):
self.decrease_max_throttle()

def on_dpad_left(self):
print("dpad left un-mapped")
logger.error("dpad left un-mapped")

def on_dpad_right(self):
print("dpad right un-mapped")
logger.error("dpad right un-mapped")


class NimbusController(JoystickController):
Expand All @@ -1508,7 +1523,7 @@ def init_js(self):
self.js = Nimbus(self.dev_fn)
self.js.init()
except FileNotFoundError:
print(self.dev_fn, "not found.")
logger.error(f"{self.dev_fn} not found.")
self.js = None
return self.js is not None

Expand Down Expand Up @@ -1540,7 +1555,7 @@ def init_js(self):
self.js = WiiU(self.dev_fn)
self.js.init()
except FileNotFoundError:
print(self.dev_fn, "not found.")
logger.error(f"{self.dev_fn} not found.")
self.js = None
return self.js is not None

Expand Down Expand Up @@ -1573,7 +1588,7 @@ def init_js(self):
self.js = RC3ChanJoystick(self.dev_fn)
self.js.init()
except FileNotFoundError:
print(self.dev_fn, "not found.")
logger.error(f"{self.dev_fn} not found.")
self.js = None
return self.js is not None

Expand Down Expand Up @@ -1637,7 +1652,7 @@ def run(self):
axis_val = 0
message_data = (button, button_state, axis, axis_val)
self.socket.send_string( "%s %d %s %f" % message_data)
print("SENT", message_data)
logger.info(f"SENT {message_data}")


class JoyStickSub(object):
Expand Down
2 changes: 1 addition & 1 deletion setup.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,7 @@ def package_files(directory, strip_leading):
long_description = fh.read()

setup(name='donkeycar',
version="4.3.10",
version="4.3.11",
long_description=long_description,
description='Self driving library for python.',
url='https://github.com/autorope/donkeycar',
Expand Down

0 comments on commit bfb4f03

Please sign in to comment.