From 880095f2b898295b2e2de0f3754a41f2829a9700 Mon Sep 17 00:00:00 2001 From: michaelwestern1 Date: Thu, 27 Nov 2025 22:00:44 +0000 Subject: [PATCH] Implement reset functionality for control_interface --- control_interface/control_ui.py | 59 ++++++++++++------------ control_interface/node.py | 82 +++++++++++++++++++++++++-------- resource/ap1_control_interface | 1 + 3 files changed, 93 insertions(+), 49 deletions(-) create mode 100644 resource/ap1_control_interface diff --git a/control_interface/control_ui.py b/control_interface/control_ui.py index d604b4d..3d01771 100644 --- a/control_interface/control_ui.py +++ b/control_interface/control_ui.py @@ -1,5 +1,4 @@ import threading - import rclpy from textual.app import App, ComposeResult @@ -11,6 +10,7 @@ from diagnostics_display import DiagnosticsDisplay from visual_path import PathCanvas + def print_help(self): self.command_output.add_line('=' * 40) self.command_output.add_line('Welcome to AP1 Debug UI') @@ -18,14 +18,13 @@ def print_help(self): self.command_output.add_line('\tspeed - Set target speed (m/s)') self.command_output.add_line('\tlocation - Set target location (m)') self.command_output.add_line('\tget_speed_profile - Return planned speed profile (m/s)') - self.command_output.add_line('\treset - Reset system and simulation (if applicable)') + self.command_output.add_line('\treset - Reset system + control + target speed') self.command_output.add_line('\tclear - Clear screen') self.command_output.add_line('\thelp - Print this screen') self.command_output.add_line('=' * 40) - + + class AP1DebugUI(App): - # me n my homies hate writing css - # god bless chat: CSS_PATH = 'style.css' def __init__(self, node: AP1SystemInterfaceNode): @@ -46,40 +45,28 @@ def compose(self) -> ComposeResult: with Container(id='cli_container', classes='pane'): yield Label('COMMAND LINE INTERFACE', classes='header') with VerticalScroll(id='output_scroll'): - yield self.command_output + yield self.command_output yield Input(placeholder='Command...', id='command_input') yield Footer() def on_mount(self): - # start ros spin threading.Thread(target=rclpy.spin, args=(self.ros_node,), daemon=True).start() - - # welcome msg print_help(self) - - # focus the input self.query_one('#command_input', Input).focus() def on_input_submitted(self, event: Input.Submitted): cmd = event.value.strip() - - # skip if nothing if not cmd: - return + return - # show command in output self.command_output.add_line(f'> {cmd}') - - # clear input event.input.value = '' - - # execute command self.execute_command(cmd) def execute_command(self, cmd: str): try: - parts = cmd.split() + parts = cmd.split() command = parts[0].lower() if command == 'help': @@ -88,13 +75,13 @@ def execute_command(self, cmd: str): elif command == 'clear': self.command_output.history.clear() self.command_output.update('') - self.command_output.add_line('History cleared') # may remove if annoying + self.command_output.add_line('History cleared') elif command == 'speed': if len(parts) < 2: self.command_output.add_line('Error! Usage: speed ') else: - speed = float(parts[1]) # grab the value + speed = float(parts[1]) self.ros_node.set_target_speed(speed) self.command_output.add_line(f"✓ Target speed set to {speed:.2f} m/s") @@ -108,18 +95,30 @@ def execute_command(self, cmd: str): elif command == 'get_speed_profile': speed_profile = self.ros_node.speed_profile - - out = '' - for spd in speed_profile: - out += spd.__str__() + ' m/s, ' - out = out[:-2] + out = ', '.join(str(s) for s in speed_profile) self.command_output.add_line('{ ' + out + ' }') - elif command == 'reset': - self.command_output.add_line("Not yet implemented.") + elif command == "reset": + # Try reset sim + try: + self.ros_node.call_trigger_service("/pnc_sim/reset") + self.command_output.add_line("✓ Simulation reset triggered.") + except Exception: + pass + + # Try reset control + try: + self.ros_node.call_trigger_service("/control/reset") + self.command_output.add_line("✓ Control reset triggered.") + except Exception: + pass + + # ★ NEW: reset target speed to 0 + self.ros_node.set_target_speed(0.0) + self.command_output.add_line("✓ Target speed reset to 0.0") else: self.command_output.add_line('Command not found.') + except Exception as e: self.command_output.add_line(f"Error: {str(e)}") - diff --git a/control_interface/node.py b/control_interface/node.py index 3a9771a..216ab41 100644 --- a/control_interface/node.py +++ b/control_interface/node.py @@ -1,6 +1,16 @@ +import rclpy from rclpy.node import Node -from geometry_msgs.msg import Point -from ap1_msgs.msg import VehicleSpeedStamped, TurnAngleStamped, MotorPowerStamped, TargetPathStamped, SpeedProfileStamped +from geometry_msgs.msg import Point +from std_srvs.srv import Trigger + +from ap1_msgs.msg import ( + VehicleSpeedStamped, + TurnAngleStamped, + MotorPowerStamped, + TargetPathStamped, + SpeedProfileStamped, +) + class AP1SystemInterfaceNode(Node): def __init__(self): @@ -15,15 +25,24 @@ def __init__(self): self.turn_angle_sub = self.create_subscription(TurnAngleStamped, '/ap1/actuation/turn_angle_actual', self.turn_angle_callback, 10) self.current_motor_power_sub = self.create_subscription(MotorPowerStamped, '/ap1/control/motor_power', self.motor_power_callback, 10) self.path_sub = self.create_subscription(TargetPathStamped, '/ap1/planning/target_path', self.target_path_callback, 10) - self.speed_profile = self.create_subscription(SpeedProfileStamped, '/ap1/planning/speed_profile', self.speed_profile_callback, 10) + self.speed_profile_sub = self.create_subscription(SpeedProfileStamped, '/ap1/planning/speed_profile', self.speed_profile_callback, 10) + + # state + self.current_speed = 0.0 + self.target_speed = 0.0 + self.motor_power = 0.0 + self.target_location = (0.0, 0.0) + self.current_turn_angle = 0.0 + self.target_path = [] + self.speed_profile = [] + + # reset service clients + self.sim_reset_client = self.create_client(Trigger, '/pnc_sim/reset') + self.system_reset_client = self.create_client(Trigger, '/ap1/system/reset') - self.current_speed = 0.0 # m - self.target_speed = 0.0 # m/s - self.motor_power = 0.0 # [-1, 1] - self.target_location = (0.0, 0.0) # m - self.current_turn_angle = 0.0 # rads - self.target_path = [] # waypoints - self.speed_profile = [] # m/s + # --------------------- + # Callbacks + # --------------------- def speed_callback(self, msg: VehicleSpeedStamped): self.current_speed = msg.speed @@ -33,22 +52,24 @@ def speed_profile_callback(self, msg: SpeedProfileStamped): def turn_angle_callback(self, msg: TurnAngleStamped): self.current_turn_angle = msg.angle - + def motor_power_callback(self, msg: MotorPowerStamped): self.motor_power = msg.power def target_path_callback(self, msg: TargetPathStamped): self.target_path = msg.path - + + # --------------------- + # Commands + # --------------------- + def set_target_speed(self, speed: float): self.target_speed = speed - # assemble msg msg = VehicleSpeedStamped() - msg.speed = speed + msg.speed = speed msg.header.stamp = self.get_clock().now().to_msg() - # send out msg self.speed_pub.publish(msg) self.get_logger().info(f'Sent target speed: {speed}') @@ -56,12 +77,37 @@ def set_target_location(self, x: float, y: float): self.target_location = (x, y) msg = Point() msg.x = x - msg.y = y + msg.y = y msg.z = 0.0 + self.target_location_pub.publish(msg) self.get_logger().info(f'Sent target location: ({x}, {y})') -# TEMPORARY + # --------------------- + # Reset logic + # --------------------- + + def reset_system(self): + req = Trigger.Request() + + # Try simulation reset service first + if self.sim_reset_client.wait_for_service(timeout_sec=0.3): + future = self.sim_reset_client.call_async(req) + rclpy.spin_until_future_complete(self, future) + if future.result(): + return future.result().success, future.result().message + + # Try system reset + if self.system_reset_client.wait_for_service(timeout_sec=0.3): + future = self.system_reset_client.call_async(req) + rclpy.spin_until_future_complete(self, future) + if future.result(): + return future.result().success, future.result().message + + return False, "No reset service available." + + +# TEMPORARY local testing if __name__ == '__main__': import rclpy @@ -71,7 +117,5 @@ def set_target_location(self, x: float, y: float): try: node.set_target_speed(2) finally: - # teardown node.destroy_node() rclpy.shutdown() - \ No newline at end of file diff --git a/resource/ap1_control_interface b/resource/ap1_control_interface new file mode 100644 index 0000000..1dc067c --- /dev/null +++ b/resource/ap1_control_interface @@ -0,0 +1 @@ +ap1_control_interface