Refactor whl-can to make the buttons more concise.#159
Refactor whl-can to make the buttons more concise.#159
Conversation
There was a problem hiding this comment.
Pull request overview
This PR refactors the whl-can keyboard controller to provide a more intuitive and ergonomic control mapping for Apollo autonomous vehicle control. The refactoring changes button mappings to be more consistent with gaming conventions (WASD for driving), adds a comprehensive UI with real-time visual feedback, introduces multiple control modes (SPEED/THROTTLE/ACCEL), and reorganizes the code architecture to make the controller more self-contained.
Changes:
- Remapped keyboard controls to be more intuitive (e.g., V for brake decrease instead of B, Q/E for turn signals, L to cycle lights)
- Added rich terminal UI with colored status display, progress bars, and visual steering indicator
- Implemented three control modes (SPEED, THROTTLE, ACCEL) switchable via 1/2/3 keys
- Refactored architecture so KeyboardController creates its own Cyber RT node and writer
- Added help screen toggle and improved user feedback with timed message display
| ord("="): lambda: self.set_turn_signal(3, "HAZARD"), | ||
| ord("o"): self.toggle_horn, | ||
| ord("l"): self.cycle_lights, | ||
| ord("x"): self.toggle_emergency_light, |
There was a problem hiding this comment.
The key 'x' is mapped to toggle_emergency_light, but according to the PR description in the control key mapping table, 'X' should be used to "Turn off all light signals" (Cancel). However, this implementation toggles the emergency light instead. There's a discrepancy between the PR description which states 'X' should cancel all light signals and the actual implementation. Consider either updating the key mapping to match the description or clarifying the intended behavior.
| c1 = 2 | ||
| scr.addstr( | ||
| 3, c1, f"GEAR: [{self.gear_names[self.gear_index]}]", curses.A_BOLD | ||
| ) | ||
| scr.addstr(4, c1, f"EPB: [{'ON' if self.epb else ' '}]") | ||
| scr.addstr(5, c1, f"SIGNAL: {self._signal_text()}") | ||
| scr.addstr(6, c1, f"THRESH: {self.turn_signal_threshold:.1f}") | ||
|
|
||
| c2 = 20 | ||
| bar_speed = self._draw_bar(abs(self.speed), SPEED_MAX, 12) | ||
| scr.addstr(3, c2, f"SPD: {self.speed:5.2f} {bar_speed}") | ||
| bar_throttle = self._draw_bar(self.throttle, THROTTLE_MAX, 12) | ||
| scr.addstr(4, c2, f"THR: {self.throttle:5.2f} {bar_throttle}") | ||
| bar_accel = self._draw_bar(abs(self.acceleration), ACCEL_MAX, 12) | ||
| scr.addstr(5, c2, f"ACC: {self.acceleration:5.2f} {bar_accel}") | ||
| bar_brake = self._draw_bar(self.brake, BRAKE_MAX, 12) | ||
| scr.addstr(6, c2, f"BRK: {self.brake:5.2f} {bar_brake}", style_brake) | ||
|
|
||
| c3 = 55 | ||
| steer_visual = "." * 10 + "^" + "." * 10 | ||
| idx = int((self.steering + 100) / 10) | ||
| idx = max(0, min(20, idx)) | ||
| s_list = list(steer_visual) | ||
| if 0 <= idx < 21: | ||
| s_list[idx] = "O" | ||
| scr.addstr(3, c3, f"STR: {''.join(s_list)} {self.steering:.1f}") | ||
| bar_steer = self._draw_bar(abs(self.steering), STEERING_MAX, 12) | ||
| scr.addstr(4, c3, f"RATE: {self.steering_rate:.2f} {bar_steer}") | ||
| scr.addstr(5, c3, f"LOW: {'ON' if self.low_beam else 'OFF'}") | ||
| scr.addstr( | ||
| 6, | ||
| c3, | ||
| f"HIGH:{'ON' if self.high_beam else 'OFF'} EMG:{'ON' if self.emergency_light else 'OFF'}", | ||
| ) | ||
|
|
There was a problem hiding this comment.
The UI assumes a minimum width for column positions (c1=2, c2=20, c3=55) without verifying terminal width. If the terminal width is less than approximately 80 characters, the UI elements at c3=55 will either not render properly or cause rendering issues. Consider adding a minimum width check similar to the height check, or make the layout responsive to terminal width.
| self.node = cyber.Node("can_easy") | ||
| self.writer = self.node.create_writer( | ||
| CONTROL_TOPIC, control_cmd_pb2.ControlCommand | ||
| ) |
There was a problem hiding this comment.
The architecture has changed significantly - the KeyboardController now creates its own cyber.Node and writer in init, rather than receiving them as parameters or having them managed externally. This means the controller is tightly coupled to the Cyber RT infrastructure. In the old code, the node and writer were created in main() and the controller just used get_control_cmd(). This new design reduces testability and makes the controller harder to mock or unit test. Consider using dependency injection to pass the writer as a parameter.
| def _render_ui(self): | ||
| scr = self.screen | ||
| h, w = scr.getmaxyx() | ||
| scr.erase() | ||
|
|
||
| style_header = curses.A_BOLD | ( | ||
| curses.color_pair(1) if self.engage else curses.color_pair(3) | ||
| ) | ||
| style_brake = curses.color_pair(2) if self.brake > 0 else 0 | ||
|
|
||
| status_text = " AUTONOMOUS " if self.engage else " MANUAL " | ||
| scr.addstr(0, 0, "=" * w) | ||
| scr.addstr(1, 2, f"CONTROL MODE: {status_text}", style_header) | ||
| scr.addstr(1, w - 20, datetime.datetime.now().strftime("%H:%M:%S")) | ||
| scr.addstr(2, 0, "-" * w) | ||
|
|
||
| c1 = 2 | ||
| scr.addstr( | ||
| 3, c1, f"GEAR: [{self.gear_names[self.gear_index]}]", curses.A_BOLD | ||
| ) | ||
| scr.addstr(4, c1, f"EPB: [{'ON' if self.epb else ' '}]") | ||
| scr.addstr(5, c1, f"SIGNAL: {self._signal_text()}") | ||
| scr.addstr(6, c1, f"THRESH: {self.turn_signal_threshold:.1f}") | ||
|
|
||
| c2 = 20 | ||
| bar_speed = self._draw_bar(abs(self.speed), SPEED_MAX, 12) | ||
| scr.addstr(3, c2, f"SPD: {self.speed:5.2f} {bar_speed}") | ||
| bar_throttle = self._draw_bar(self.throttle, THROTTLE_MAX, 12) | ||
| scr.addstr(4, c2, f"THR: {self.throttle:5.2f} {bar_throttle}") | ||
| bar_accel = self._draw_bar(abs(self.acceleration), ACCEL_MAX, 12) | ||
| scr.addstr(5, c2, f"ACC: {self.acceleration:5.2f} {bar_accel}") | ||
| bar_brake = self._draw_bar(self.brake, BRAKE_MAX, 12) | ||
| scr.addstr(6, c2, f"BRK: {self.brake:5.2f} {bar_brake}", style_brake) | ||
|
|
||
| c3 = 55 | ||
| steer_visual = "." * 10 + "^" + "." * 10 | ||
| idx = int((self.steering + 100) / 10) | ||
| idx = max(0, min(20, idx)) | ||
| s_list = list(steer_visual) | ||
| if 0 <= idx < 21: | ||
| s_list[idx] = "O" | ||
| scr.addstr(3, c3, f"STR: {''.join(s_list)} {self.steering:.1f}") | ||
| bar_steer = self._draw_bar(abs(self.steering), STEERING_MAX, 12) | ||
| scr.addstr(4, c3, f"RATE: {self.steering_rate:.2f} {bar_steer}") | ||
| scr.addstr(5, c3, f"LOW: {'ON' if self.low_beam else 'OFF'}") | ||
| scr.addstr( | ||
| 6, | ||
| c3, | ||
| f"HIGH:{'ON' if self.high_beam else 'OFF'} EMG:{'ON' if self.emergency_light else 'OFF'}", | ||
| ) | ||
|
|
||
| scr.addstr(8, 0, "-" * w) | ||
| if time.time() - self.msg_time < 3.0: | ||
| scr.addstr(9, 2, f">> {self.msg_log}", curses.A_BOLD) | ||
| else: | ||
| scr.addstr(9, 2, ">>") | ||
|
|
||
| scr.addstr(11, 0, "=" * w) | ||
| if not self.show_help: | ||
| scr.addstr(12, 2, "[H] Help | [Space] E-Stop | [Enter] Auto | [Esc] Quit") | ||
| else: | ||
| scr.addstr( | ||
| 12, 2, "W/S: Drive | A/D: Steer | B/V: Brake +/- | Space: E-Stop" | ||
| ) | ||
| scr.addstr(13, 2, "M: Gear | P: EPB | 1/2/3: Mode | Enter: Auto-Drive") | ||
| scr.addstr(14, 2, "q/e/f/c: Left/Right/Hazard/Cancel | o: Horn") | ||
| scr.addstr(15, 2, "l: Lights (Off/Low/High) | x: Emergency") | ||
|
|
||
| scr.refresh() |
There was a problem hiding this comment.
The _render_ui method performs multiple scr.addstr() calls without any error handling. If any of these calls fail (e.g., due to terminal resize making positions invalid, or other curses errors), the entire UI rendering will fail silently, leaving the user with a potentially frozen or corrupted display. Consider wrapping the rendering logic in a try-except block to handle curses.error gracefully.
| ord("q"): lambda: self.set_turn_signal(1, "LEFT"), | ||
| ord("e"): lambda: self.set_turn_signal(2, "RIGHT"), | ||
| ord("f"): lambda: self.set_turn_signal(3, "HAZARD"), | ||
| ord("c"): lambda: self.set_turn_signal(0, "NONE"), |
There was a problem hiding this comment.
The key 'c' is mapped to set turn signal to NONE, but according to the PR description, 'X' should be the cancel key. The description states 'X' should "Turn off all light signals", while 'c' is not mentioned in the control key mapping table at all. This creates inconsistency between documentation and implementation.
| current_val = ( | ||
| self.speed if self.control_mode == CONTROL_MODE_SPEED else self.throttle | ||
| ) |
There was a problem hiding this comment.
The control_mode is checked to select between speed and throttle for gear shift validation, but when control_mode is ACCEL, the code falls back to using throttle. This is inconsistent - the code should check acceleration when in ACCEL mode. The current implementation may allow gear shifts when acceleration is high but throttle is low.
| current_val = ( | |
| self.speed if self.control_mode == CONTROL_MODE_SPEED else self.throttle | |
| ) | |
| if self.control_mode == CONTROL_MODE_SPEED: | |
| current_val = self.speed | |
| elif self.control_mode == CONTROL_MODE_THROTTLE: | |
| current_val = self.throttle | |
| else: | |
| current_val = self.acceleration |
| 12, 2, "W/S: Drive | A/D: Steer | B/V: Brake +/- | Space: E-Stop" | ||
| ) | ||
| scr.addstr(13, 2, "M: Gear | P: EPB | 1/2/3: Mode | Enter: Auto-Drive") | ||
| scr.addstr(14, 2, "q/e/f/c: Left/Right/Hazard/Cancel | o: Horn") |
There was a problem hiding this comment.
The help display in _render_ui shows that 'q/e/f/c' are for Left/Right/Hazard/Cancel turn signals. However, this contradicts the PR description which states that 'Q' and 'E' should be used (capital letters are mentioned in the table). While the actual implementation uses lowercase 'q' and 'e', the PR description suggests case-insensitive behavior for some keys but uses uppercase notation. This inconsistency could confuse users.
| self.control_map = { | ||
| ord("w"): self.move_forward, | ||
| ord("s"): self.move_backward, | ||
| ord("a"): self.turn_left, | ||
| ord("d"): self.turn_right, | ||
| ord("m"): self.loop_gear, | ||
| ord("b"): self.brake_inc, | ||
| ord("B"): self.brake_dec, | ||
| ord("v"): self.brake_dec, | ||
| ord(" "): self.emergency_stop, | ||
| 10: self.toggle_engage, | ||
| 27: self.quit_program, | ||
| ord("h"): self.toggle_help, |
There was a problem hiding this comment.
The PR description mentions that keys should be case-insensitive for 'B' (brake) and 'H' (help), but the implementation only maps lowercase 'b', 'v', and 'h'. The old code had ord("B") mapped separately for brake decrease. To match the PR description's promise of case-insensitive operation, consider adding mappings for both uppercase and lowercase versions of these keys.
| self.sequence_num += 1 | ||
| cmd.header.sequence_num = self.sequence_num | ||
| cmd.pad_msg.driving_mode = 1 if self.engage else 0 | ||
| cmd.pad_msg.action = 1 if self.engage else 0 | ||
| self.pad_only_pending = False | ||
| self.writer.write(cmd) | ||
| return | ||
|
|
||
| cmd = self.control_cmd_msg | ||
| cmd.header.timestamp_sec = datetime.datetime.now().timestamp() | ||
| cmd.header.module_name = "can_easy" | ||
| self.sequence_num += 1 |
There was a problem hiding this comment.
The sequence_num is incremented without any bounds checking or wraparound handling. In a long-running application, this could eventually overflow (though Python integers can grow arbitrarily large, the protobuf sequence_num field might have size limits). Consider implementing wraparound logic or resetting at a reasonable maximum value.
| cmd.ClearField("pad_msg") | ||
| cmd.ClearField("speed") | ||
| cmd.ClearField("throttle") | ||
| cmd.ClearField("acceleration") | ||
|
|
||
| if self.brake > 0: | ||
| cmd.speed = 0.0 | ||
| cmd.throttle = 0.0 | ||
| cmd.acceleration = 0.0 | ||
| elif self.control_mode == CONTROL_MODE_SPEED: | ||
| cmd.speed = self.speed | ||
| elif self.control_mode == CONTROL_MODE_THROTTLE: | ||
| cmd.throttle = self.throttle | ||
| else: | ||
| self.control_cmd_msg.pad_msg.driving_mode = 0 | ||
| self.control_cmd_msg.pad_msg.action = 0 | ||
| # TODO(All): set control command via control mode | ||
| self.control_cmd_msg.throttle = self.throttle | ||
| self.control_cmd_msg.speed = self.speed | ||
| self.control_cmd_msg.steering_target = self.steering | ||
| self.control_cmd_msg.steering_rate = self.steering_rate | ||
| self.control_cmd_msg.gear_location = self.gear | ||
| self.control_cmd_msg.brake = self.brake | ||
| if self.epb == 1: | ||
| self.control_cmd_msg.parking_brake = True | ||
| else: | ||
| self.control_cmd_msg.parking_brake = False | ||
| # TODO(All): set signal via keyboard input | ||
| self.control_cmd_msg.signal.horn = self.horn | ||
| self.control_cmd_msg.signal.high_beam = self.high_beam | ||
| self.control_cmd_msg.signal.low_beam = self.low_beam | ||
| self.control_cmd_msg.signal.emergency_light = self.emergency_light | ||
| self.control_cmd_msg.signal.turn_signal = self.turn_signal | ||
| if self.turn_signal in [1, 2, 3]: # Manual signal is active | ||
| self.control_cmd_msg.signal.turn_signal = self.turn_signal | ||
| cmd.acceleration = self.acceleration |
There was a problem hiding this comment.
The code clears pad_msg, speed, throttle, and acceleration fields on every publish (lines 180-183), but then immediately sets one of them based on control_mode. This clearing seems unnecessary since the fields are being set right after. However, this might be intentional to ensure clean state. If this is a protobuf behavior requirement (to avoid stale field values), consider adding a comment explaining why these fields need to be cleared before being set.
Control Key Mapping