diff --git a/modules/tools/whl-can/whl_can.py b/modules/tools/whl-can/whl_can.py index 235e2b85..399dde2f 100644 --- a/modules/tools/whl-can/whl_can.py +++ b/modules/tools/whl-can/whl_can.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # Copyright 2025 daohu527 # @@ -30,6 +30,7 @@ THROTTLE_MIN, THROTTLE_MAX = 0.0, 100.0 BRAKE_MIN, BRAKE_MAX = 0.0, 100.0 STEERING_RATE_MIN, STEERING_RATE_MAX = -6.28, 6.28 +ACCEL_MIN, ACCEL_MAX = -5.0, 5.0 SPEED_DELTA = 0.1 STEERING_DELTA = 1 @@ -37,343 +38,437 @@ STEERING_RATE_DELTA = 0.1 BRAKE_DELTA = 1 TURN_SIGNAL_THRESHOLD_DELTA = 1.0 +ACCEL_DELTA = 0.1 + +DEFAULT_STEERING_RATE = 0.0 +DEFAULT_TURN_SIGNAL_THRESHOLD = 0.0 + +CONTROL_MODE_SPEED = "SPEED" +CONTROL_MODE_THROTTLE = "THROTTLE" +CONTROL_MODE_ACCEL = "ACCEL" class KeyboardController: """ - Curses-based keyboard controller, no root privileges required, suitable for - Linux terminal environments. - - Listens for key presses in non-blocking mode: - - w: Forward (accelerate) - - s: Backward (decelerate) - - a: Turn left (increase steering angle) - - d: Turn right (decrease steering angle) - - m: Change gear (loop through P, R, N, D) - - b: Increase brake - - B: Decrease brake - - p: Toggle Electronic Parking Brake (EPB) - - q: Exit the program + Curses-based keyboard controller for Apollo Cyber RT. """ - def __init__( - self, - screen, - speed_delta=SPEED_DELTA, - steering_delta=STEERING_DELTA, - brake_delta=BRAKE_DELTA, - ): + def __init__(self, screen): self.screen = screen - self.running = True + self.logger = logging.getLogger("KeyboardController") + + self.node = cyber.Node("can_easy") + self.writer = self.node.create_writer( + CONTROL_TOPIC, control_cmd_pb2.ControlCommand + ) self.control_cmd_msg = control_cmd_pb2.ControlCommand() - self.speed = 0 - self.throttle = 0 + self.sequence_num = 0 + + self.running = True + self.engage = False + self.pad_only_pending = True + + self.control_mode = CONTROL_MODE_THROTTLE + self.speed = 0.0 + self.throttle = 0.0 + self.acceleration = 0.0 + self.brake = 0.0 self.steering = 0.0 - self.steering_rate = 0.0 - self.turn_signal_threshold = 0.0 - self.turn_signal = 0 # 0:None, 1:Left, 2:Right, 3:Hazard - self.horn = False - self.high_beam = False - self.low_beam = False - self.emergency_light = False - self.speed_delta = speed_delta - self.steering_delta = steering_delta - # check defination of in modules/common_msgs/chassis_msgs/chassis.proto + self.steering_rate = DEFAULT_STEERING_RATE + self.turn_signal_threshold = DEFAULT_TURN_SIGNAL_THRESHOLD + + self.speed_delta = SPEED_DELTA + self.steering_delta = STEERING_DELTA + self.brake_delta = BRAKE_DELTA + self.gear_list = [3, 2, 0, 1] - self.gear_str = ["P", "R", "N", "D"] + self.gear_names = ["P", "R", "N", "D"] self.gear_index = 0 self.gear = 3 - self.brake = 0 - self.brake_delta = brake_delta self.epb = 0 - self.engage = True + + self.turn_signal = 0 + self.horn = False + self.high_beam = False + self.low_beam = False + self.emergency_light = False + + self.show_help = False + self.msg_log = "System Ready." + self.msg_time = time.time() self.lock = threading.Lock() - self.logger = logging.getLogger(__name__) - - self.help_text_lines = [ - "Key instructions:", - " E: Toggle auto-drive mode A/D: Increase/Decrease steering rate", - " w/s: Increase/Decrease speed a/d: Turn left/right", - " m: Change gear b/B: Brake +/-", - " p: Toggle Electronic Parking Brake (EPB) o/O: Turn signal threshold +/-", - " h/l/k/e: Toggle Horn/Low Beam/High Beam/Emergency Light", - " [/]/\\/=: Turn Signal Left/Right/Off/Hazard", - " Space: Emergency stop q: Quit program ", - ] - - # Key mapping: map keys using ASCII codes - # TODO(All): add control mode, throttle, speed, acceleration, etc. + 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, + ord("m"): self.loop_gear, ord("p"): self.toggle_epb, - ord("o"): self.turn_signal_threshold_inc, - ord("O"): self.turn_signal_threshold_dec, - ord("A"): self.steering_rate_inc, - ord("D"): self.steering_rate_dec, - ord("E"): self.toggle_engage, - ord(" "): self.emergency_stop, # space key - ord("h"): self.toggle_horn, - ord("l"): self.toggle_low_beam, - ord("k"): self.toggle_high_beam, - ord("e"): self.toggle_emergency_light, - ord("["): lambda: self.set_turn_signal(1, "LEFT"), - ord("]"): lambda: self.set_turn_signal(2, "RIGHT"), - ord("\\"): lambda: self.set_turn_signal(0, "NONE"), - ord("="): lambda: self.set_turn_signal(3, "HAZARD"), + ord("o"): self.toggle_horn, + ord("l"): self.cycle_lights, + ord("x"): self.toggle_emergency_light, + 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"), + ord("1"): self.set_mode_speed, + ord("2"): self.set_mode_throttle, + ord("3"): self.set_mode_accel, } - def get_control_cmd(self): - """Returns the latest control command message.""" - with self.lock: - return self.control_cmd_msg - def start(self): - """Starts keyboard listening, sets curses to non-blocking mode and starts the listening thread.""" - self.screen.nodelay(True) # Set non-blocking input + self.screen.nodelay(True) self.screen.keypad(True) - self.screen.addstr(0, 0, - "Keyboard control started, press 'q' to exit. ") - for idx, line in enumerate(self.help_text_lines): - self.screen.addstr(16 + idx, 0, line) - self.thread = threading.Thread(target=self._listen_keyboard, - daemon=True) + curses.curs_set(0) + if curses.has_colors(): + curses.start_color() + curses.init_pair(1, curses.COLOR_GREEN, curses.COLOR_BLACK) + curses.init_pair(2, curses.COLOR_RED, curses.COLOR_BLACK) + curses.init_pair(3, curses.COLOR_YELLOW, curses.COLOR_BLACK) + curses.init_pair(4, curses.COLOR_CYAN, curses.COLOR_BLACK) + + self.thread = threading.Thread(target=self._loop, daemon=True) self.thread.start() def stop(self): - """Stops keyboard listening.""" with self.lock: self.running = False - self.screen.addstr(1, 0, - "Keyboard control stopped. ") - def _listen_keyboard(self): - """Loop reads keyboard input and calls the corresponding control method based on the key pressed.""" + def _loop(self): while self.running: - try: - key = self.screen.getch() # Non-blocking call - except Exception as e: - print(f"Error reading keyboard input: {e}") - key = -1 - - if key != -1: - if key == ord("q"): - self.stop() - break - elif key in self.control_map: - with self.lock: - self.control_map[key]() - self.fill_control_cmd() - self.fill_control_cmd_header() + self._handle_input() + self._publish_command() + self._render_ui() time.sleep(0.05) - def fill_control_cmd_header(self): - """Fills the header of the control command message.""" - with self.lock: - self.control_cmd_msg.header.timestamp_sec = ( - datetime.datetime.now().timestamp()) - self.control_cmd_msg.header.module_name = "can_easy" - self.control_cmd_msg.header.sequence_num += 1 + def _handle_input(self): + try: + key = self.screen.getch() + if key != -1 and key in self.control_map: + with self.lock: + self.control_map[key]() + except Exception as e: + self.log_msg(f"Input Error: {e}") - def fill_control_cmd(self): - """Updates the current speed and steering to the protobuf message.""" + def _publish_command(self): with self.lock: - # TODO(All): start auto-drive via keyboard input - if self.engage: - self.control_cmd_msg.pad_msg.driving_mode = 1 - self.control_cmd_msg.pad_msg.action = 1 + if self.pad_only_pending: + cmd = control_cmd_pb2.ControlCommand() + cmd.header.timestamp_sec = datetime.datetime.now().timestamp() + cmd.header.module_name = "can_easy" + 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 + cmd.header.sequence_num = self.sequence_num + + 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 + + cmd.steering_target = self.steering + cmd.steering_rate = self.steering_rate + cmd.gear_location = self.gear + cmd.brake = self.brake + cmd.parking_brake = bool(self.epb) + + cmd.signal.horn = self.horn + cmd.signal.high_beam = self.high_beam + cmd.signal.low_beam = self.low_beam + cmd.signal.emergency_light = self.emergency_light + + if self.turn_signal in [1, 2, 3]: + cmd.signal.turn_signal = self.turn_signal elif self.turn_signal_threshold <= 0: - # disable turn signal overwrite, use the vehicle logic - self.control_cmd_msg.signal.ClearField("turn_signal") + cmd.signal.ClearField("turn_signal") else: if self.steering > self.turn_signal_threshold: - self.control_cmd_msg.signal.turn_signal = 1 + cmd.signal.turn_signal = 1 elif self.steering < -self.turn_signal_threshold: - self.control_cmd_msg.signal.turn_signal = 2 + cmd.signal.turn_signal = 2 else: - self.control_cmd_msg.signal.turn_signal = 0 + cmd.signal.turn_signal = 0 + + self.writer.write(cmd) + + def log_msg(self, msg): + self.msg_log = msg + self.msg_time = time.time() + + def quit_program(self): + self.running = False + + def toggle_help(self): + self.show_help = not self.show_help def move_forward(self): - self.speed = min(self.speed + self.speed_delta, SPEED_MAX) - self.throttle = min(self.throttle + THROTTLE_DELTA, THROTTLE_MAX) - self.screen.addstr( - 2, 0, f"speed: {self.speed:.2f} [{SPEED_MIN}, {SPEED_MAX}], " - f"throttle: {self.throttle:.2f} [0, 100] ") + if self.control_mode == CONTROL_MODE_SPEED: + self.speed = min(self.speed + self.speed_delta, SPEED_MAX) + elif self.control_mode == CONTROL_MODE_THROTTLE: + self.throttle = min(self.throttle + THROTTLE_DELTA, THROTTLE_MAX) + else: + self.acceleration = min(self.acceleration + ACCEL_DELTA, ACCEL_MAX) + self.update_longitudinal_status() def move_backward(self): - self.speed = max(self.speed - self.speed_delta, SPEED_MIN) - self.throttle = max(self.throttle - THROTTLE_DELTA, THROTTLE_MIN) - self.screen.addstr( - 2, 0, f"speed: {self.speed:.2f} [{SPEED_MIN}, {SPEED_MAX}], " - f"throttle: {self.throttle:.2f} [0, 100] ") + if self.control_mode == CONTROL_MODE_SPEED: + self.speed = max(self.speed - self.speed_delta, SPEED_MIN) + elif self.control_mode == CONTROL_MODE_THROTTLE: + self.throttle = max(self.throttle - THROTTLE_DELTA, THROTTLE_MIN) + else: + self.acceleration = max(self.acceleration - ACCEL_DELTA, ACCEL_MIN) + self.update_longitudinal_status() def turn_left(self): self.steering = min(self.steering + self.steering_delta, STEERING_MAX) - self.screen.addstr( - 3, 0, - f"steer: {self.steering:.2f} [{STEERING_MIN}, {STEERING_MAX}] ") - def turn_right(self): self.steering = max(self.steering - self.steering_delta, STEERING_MIN) - self.screen.addstr( - 3, 0, - f"steer: {self.steering:.2f} [{STEERING_MIN}, {STEERING_MAX}] ") def brake_inc(self): self.brake = min(self.brake + self.brake_delta, BRAKE_MAX) - self.screen.addstr( - 5, 0, f"brake: {self.brake:.2f} [{BRAKE_MIN}, {BRAKE_MAX}] ") + if self.brake > 0: + self.throttle = 0.0 + self.acceleration = 0.0 + if self.control_mode == CONTROL_MODE_SPEED: + self.speed = 0.0 def brake_dec(self): self.brake = max(self.brake - self.brake_delta, BRAKE_MIN) - self.screen.addstr( - 5, 0, f"brake: {self.brake:.2f} [{BRAKE_MIN}, {BRAKE_MAX}] ") def loop_gear(self): + current_val = ( + self.speed if self.control_mode == CONTROL_MODE_SPEED else self.throttle + ) + if abs(current_val) > 0.1 and self.brake < 10: + self.log_msg("Speed too high to shift!") + return self.gear_index = (self.gear_index + 1) % len(self.gear_list) self.gear = self.gear_list[self.gear_index] - self.screen.addstr(4, 0, f"gear: {self.gear_str[self.gear_index]}") + self.log_msg(f"Gear switched to: {self.gear_names[self.gear_index]}") def toggle_epb(self): - """Toggle Electronic Parking Brake (EPB) state.""" - if self.epb == 0: - self.epb = 1 - else: - self.epb = 0 - self.screen.addstr(6, 0, f"epb: {self.epb}") + self.epb = 0 if self.epb == 1 else 1 + self.log_msg(f"EPB {'ON' if self.epb else 'OFF'}") def turn_signal_threshold_inc(self): - """Increase turn signal threshold""" self.turn_signal_threshold = min( - self.turn_signal_threshold + TURN_SIGNAL_THRESHOLD_DELTA, - STEERING_MAX) - self.screen.addstr( - 7, 0, - f"turn signal threshold: {self.turn_signal_threshold:.2f} ") + self.turn_signal_threshold + TURN_SIGNAL_THRESHOLD_DELTA, STEERING_MAX + ) + self.log_msg(f"Turn signal threshold: {self.turn_signal_threshold:.2f}") def turn_signal_threshold_dec(self): - """Decrease turn signal threshold""" self.turn_signal_threshold = max( - self.turn_signal_threshold - TURN_SIGNAL_THRESHOLD_DELTA, 0.0) - self.screen.addstr( - 7, 0, - f"turn signal threshold: {self.turn_signal_threshold:.2f} ") + self.turn_signal_threshold - TURN_SIGNAL_THRESHOLD_DELTA, 0.0 + ) + self.log_msg(f"Turn signal threshold: {self.turn_signal_threshold:.2f}") def toggle_horn(self): self.horn = not self.horn - self.screen.addstr(8, 0, - f"Horn: {'ON' if self.horn else 'OFF'} ") + self.log_msg(f"Horn {'ON' if self.horn else 'OFF'}") def toggle_low_beam(self): self.low_beam = not self.low_beam - self.screen.addstr( - 9, 0, f"Low Beam: {'ON' if self.low_beam else 'OFF'} ") + self.log_msg(f"Low Beam {'ON' if self.low_beam else 'OFF'}") def toggle_high_beam(self): self.high_beam = not self.high_beam - self.screen.addstr( - 10, 0, f"High Beam: {'ON' if self.high_beam else 'OFF'} ") + self.log_msg(f"High Beam {'ON' if self.high_beam else 'OFF'}") def toggle_emergency_light(self): self.emergency_light = not self.emergency_light - self.screen.addstr( - 11, 0, - f"Emergency Light: {'ON' if self.emergency_light else 'OFF'} ") + self.log_msg(f"Emergency Light {'ON' if self.emergency_light else 'OFF'}") + + def cycle_lights(self): + if self.high_beam: + self.high_beam = False + self.low_beam = False + self.log_msg("Lights: OFF") + elif self.low_beam: + self.low_beam = False + self.high_beam = True + self.log_msg("Lights: HIGH") + else: + self.low_beam = True + self.high_beam = False + self.log_msg("Lights: LOW") def set_turn_signal(self, signal_val, signal_str): self.turn_signal = signal_val - self.screen.addstr(12, 0, f"Turn Signal: {signal_str} ") + self.log_msg(f"Turn Signal: {signal_str}") def steering_rate_inc(self): - self.steering_rate = min(self.steering_rate + STEERING_RATE_DELTA, - STEERING_RATE_MAX) - self.screen.addstr(13, 0, - f"steering rate: {self.steering_rate:.2f} ") + self.steering_rate = min( + self.steering_rate + STEERING_RATE_DELTA, STEERING_RATE_MAX + ) + self.log_msg(f"Steering rate: {self.steering_rate:.2f}") def steering_rate_dec(self): - self.steering_rate = max(self.steering_rate - STEERING_RATE_DELTA, - STEERING_RATE_MIN) - self.screen.addstr(13, 0, - f"steering rate: {self.steering_rate:.2f} ") + self.steering_rate = max( + self.steering_rate - STEERING_RATE_DELTA, STEERING_RATE_MIN + ) + self.log_msg(f"Steering rate: {self.steering_rate:.2f}") def toggle_engage(self): - """Toggle auto-drive state.""" self.engage = not self.engage - - self.screen.addstr(14, 0, f"Auto-drive: {self.engage} ") + self.pad_only_pending = True + state = "ENGAGED" if self.engage else "DISENGAGED" + self.log_msg(f"Auto-drive {state}") def emergency_stop(self): self.speed = 0 self.throttle = 0 + self.acceleration = 0 self.brake = BRAKE_MAX - self.screen.addstr(15, 0, "Emergency Stop activated! ") + self.log_msg("!!! EMERGENCY STOP ACTIVATED !!!") + + def set_mode_speed(self): + self.control_mode = CONTROL_MODE_SPEED + self.update_longitudinal_status() + self.log_msg("Mode set to: SPEED") + + def set_mode_throttle(self): + self.control_mode = CONTROL_MODE_THROTTLE + self.update_longitudinal_status() + self.log_msg("Mode set to: THROTTLE") + + def set_mode_accel(self): + self.control_mode = CONTROL_MODE_ACCEL + self.update_longitudinal_status() + self.log_msg("Mode set to: ACCEL") + + def update_longitudinal_status(self): + pass + + def _draw_bar(self, val, max_val, width=12): + percent = max(0.0, min(1.0, val / max_val)) if max_val != 0 else 0 + filled = int(width * percent) + return "[" + "|" * filled + "." * (width - filled) + "]" + + 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() + + def _signal_text(self): + if self.turn_signal == 1: + return "LEFT" + if self.turn_signal == 2: + return "RIGHT" + if self.turn_signal == 3: + return "HAZARD" + return "NONE" def main(screen): - # Configure logging at the program entry point logging.basicConfig(level=logging.INFO) cyber.init() - node = cyber.Node("can_easy") - writer = node.create_writer(CONTROL_TOPIC, control_cmd_pb2.ControlCommand) - # Check if the terminal window is large enough max_y, max_x = screen.getmaxyx() - if max_y < 20: + if max_y < 24: screen.addstr(0, 0, "Error: Terminal window is too small.") screen.addstr( - 1, 0, - "Please resize your terminal to at least 20 rows and try again.") + 1, 0, "Please resize your terminal to at least 24 rows and try again." + ) screen.refresh() time.sleep(3) return - # Pre-print the fixed format lines - screen.addstr(2, 0, "speed: 0.00, throttle: 0.0 ") - screen.addstr(3, 0, "steer_percentage: 0.00 ") - screen.addstr(4, 0, "gear: P") - screen.addstr(5, 0, "brake: 0.00 ") - screen.addstr(6, 0, "epb: 0 ") - screen.addstr(7, 0, "turn signal threshold: 0") - screen.addstr(8, 0, "Horn: OFF") - screen.addstr(9, 0, "Low Beam: OFF") - screen.addstr(10, 0, "High Beam: OFF") - screen.addstr(11, 0, "Emergency Light: OFF") - screen.addstr(12, 0, "Turn Signal: NONE") - screen.addstr(13, 0, "steering rate: 0.00 ") - screen.addstr(14, 0, "Auto-drive: True ") controller = KeyboardController(screen) controller.start() try: while controller.running: - cmd = controller.get_control_cmd() - writer.write(cmd) time.sleep(0.1) except KeyboardInterrupt: controller.stop() @@ -381,15 +476,11 @@ def main(screen): controller.stop() cyber.shutdown() - screen.addstr(6, 0, "Program exited. ") - if __name__ == "__main__": - # Use curses.wrapper to ensure proper initialization and cleanup of the curses environment try: curses.wrapper(main) except curses.error as e: - # This can happen if the terminal is too small print(f"Error initializing curses: {e}") print( "Please ensure your terminal is large enough (e.g., 80x25) and try again."