Implement reset functionality for control_interface

control_interface/control_ui.py

@@ -1,5 +1,4 @@
 import threading 
-
 import rclpy
 
 from textual.app import App, ComposeResult
@@ -11,21 +10,21 @@
 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')
     self.command_output.add_line('Commands:')
     self.command_output.add_line('\tspeed <value>     - Set target speed (m/s)')
     self.command_output.add_line('\tlocation <x> <y>  - 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 <value>')
                 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)}")
-

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,35 +52,62 @@ 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}')
 
     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()
-

resource/ap1_control_interface

@@ -0,0 +1 @@
+ap1_control_interface