diff --git a/camera_ros/scripts/camera_publisher.py b/camera_ros/scripts/camera_publisher.py index cb973d1..ddf92b6 100644 --- a/camera_ros/scripts/camera_publisher.py +++ b/camera_ros/scripts/camera_publisher.py @@ -29,11 +29,10 @@ class CameraPublisher(Node): """ Zero-copy MJPEG camera publisher using GStreamer pipeline. - Publishes compressed JPEG images from camera with zero CPU transcoding - overhead. Supports service-based streaming control - (start_streaming/stop_streaming). Automatically activates when clients - are detected and pauses when no clients are present. Listens to - /client_count topic for automatic activation/deactivation. + Publishes compressed JPEG images from camera with zero CPU transcoding overhead. + Supports service-based streaming control (start_streaming/stop_streaming). + Automatically activates when clients are detected and pauses when no clients are present. + Listens to /lucy/client_count topic for automatic activation/deactivation. """ def __init__(self): @@ -97,7 +96,7 @@ def __init__(self): self.create_service(SetBool, 'start_streaming', self.start_streaming_callback) self.create_service(SetBool, 'stop_streaming', self.stop_streaming_callback) # Subscribe to client count for automatic control - self.create_subscription(Int32, '/client_count', self.client_count_callback, 10) + self.create_subscription(Int32, '/lucy/client_count', self.client_count_callback, 10) self.get_logger().info( f"Camera publisher node started using device {self.camera_device} " diff --git a/camera_ros/scripts/camera_stream_controller.py b/camera_ros/scripts/camera_stream_controller.py index 5c51728..f87ca3e 100644 --- a/camera_ros/scripts/camera_stream_controller.py +++ b/camera_ros/scripts/camera_stream_controller.py @@ -24,7 +24,7 @@ class CameraStreamController(Node): """ Controller node that manages camera streaming based on client count. - Subscribes to /client_count topic and uses services to start/stop + Subscribes to /lucy/client_count topic and uses services to start/stop camera streaming when clients connect/disconnect. """ @@ -33,7 +33,7 @@ def __init__(self): # Subscribe to client count self.create_subscription( - Int32, '/client_count', self.client_count_callback, 10 + Int32, '/lucy/client_count', self.client_count_callback, 10 ) # Service clients for streaming control diff --git a/lucy_cli/developer.md b/lucy_cli/developer.md index b40a690..77de875 100644 --- a/lucy_cli/developer.md +++ b/lucy_cli/developer.md @@ -10,44 +10,46 @@ The recommended way to interface with Lucy is to use the `lucy_cli.ros_interface ## 1. Core Concepts -### Control Arbitration (Taking and Releasing Control) +### Presence and Control Arbitration (the client registry) -To prevent multiple users from sending conflicting commands simultaneously, Lucy uses a simple control arbitration system. - -- **Topic:** `/control_panel_active_client` -- **Message Type:** `std_msgs/msg/String` -- **QoS:** Latching (`TRANSIENT_LOCAL` durability) - -**Protocol:** -1. **To Take Control:** Publish your unique client ID to this topic. -2. **To Release Control:** Publish an empty string (`""`) to this topic. -3. **To Monitor Control:** Subscribe to this topic. If you receive a message containing a client ID that is *not* your own, your application should enter a "read-only" mode and disable its own command publishing features. +Presence (the connected-client count) and control arbitration are owned by a single transport-agnostic node, `lucy_client_registry`. +Every interface — the web control panel over rosbridge, the native CLI directly over DDS, your own script — participates in exactly the same way, and **nothing here depends on a rosbridge connection**. +The registry is the only writer of the count and the active controller; clients register by heartbeat and change control through a service. A good client ID is one that is both unique and descriptive, for example: `my_app_1678886400_a3f7`. -```python -# From lucy_cli/ros_interface.py - -import time -import random +**Registering presence (heartbeat).** Pulse your client ID on `/lucy/client_heartbeat` (`std_msgs/msg/String`, volatile) roughly once per second. +The registry counts you as connected while your heartbeats keep arriving and prunes you automatically about three seconds after they stop (three missed beats at the 1 Hz cadence) — so a crash or a closed tab is handled for free, with no explicit "disconnect" step. +```python # A unique identifier for this specific client instance. CLIENT_ID = f"my_app_{int(time.time())}_{random.randint(1000, 9999)}" # ... inside your ROS Node class ... -# self.control_publisher = self.create_publisher(String, '/control_panel_active_client', qos_profile=LATCHING_QOS) +# self.heartbeat_publisher = self.create_publisher(String, '/lucy/client_heartbeat', qos_profile=VOLATILE_QOS) +# self.create_timer(1.0, lambda: self.heartbeat_publisher.publish(String(data=CLIENT_ID))) +``` +**Taking and releasing control.** Call the `/lucy/control` service (`lucy_msgs/srv/ClientControl`) with your client ID and `acquire=True` to take control, or `acquire=False` to release it. +Control is preemptive (the latest acquirer wins); if the controlling client's heartbeat lapses, the registry releases control automatically so the robot is never stuck owned by a dead client. + +**Monitoring control.** Subscribe to `/lucy/active_client` (`std_msgs/msg/String`, latching). +If you receive a non-empty client ID that is *not* your own, enter a "read-only" mode and disable your command publishing. + +```python def take_control(self): - """Publishes this client's ID to request control of the robot.""" - self.get_logger().info("Requesting control...") - msg = String(data=CLIENT_ID) - self.control_publisher.publish(msg) + self._send_control_request(acquire=True) def release_control(self): - """Publishes an empty string to release control of the robot.""" - self.get_logger().info("Releasing control...") - msg = String(data="") - self.control_publisher.publish(msg) + self._send_control_request(acquire=False) + +def _send_control_request(self, acquire: bool): + """The resulting controller comes back on /lucy/active_client.""" + if not self._control_client.wait_for_service(timeout_sec=2.0): + self.get_logger().warn("'/lucy/control' service not available.") + return + req = ClientControl.Request(client_id=CLIENT_ID, acquire=acquire) + self._control_client.call_async(req) # fire-and-forget ``` ### Dynamic Hardware Configuration @@ -141,24 +143,25 @@ def publish_joint_trajectory(self, controller_topic: str, joint_names: list[str] ### Monitoring Connected Clients -You can monitor how many clients are connected to the control system. This is useful for diagnostics and understanding system load. +You can monitor how many clients are connected to the control system. +This is the universal count maintained by the registry from live heartbeats — it includes *every* interface (web and native alike), not just rosbridge WebSocket clients. -- **Topic:** `/client_count` +- **Topic:** `/lucy/client_count` - **Message Type:** `std_msgs/msg/Int32` - **QoS:** Latching (`TRANSIENT_LOCAL` durability) -- **Service:** `/get_client_count` +- **Service:** `/lucy/get_client_count` - **Service Type:** `lucy_msgs/srv/GetInt` **Protocol:** -1. **On Startup:** Call the `/get_client_count` service to get the number of clients already connected. -2. **Continuously:** Subscribe to the `/client_count` topic to receive live updates whenever a client connects or disconnects. +1. **On Startup:** Call the `/lucy/get_client_count` service to get the number of clients already connected. +2. **Continuously:** Subscribe to the `/lucy/client_count` topic to receive live updates whenever a client connects or disconnects. ```python # From lucy_cli/ros_interface.py # In __init__: -# self.create_subscription(Int32, '/client_count', self._client_count_callback, qos_profile=LATCHING_QOS) +# self.create_subscription(Int32, '/lucy/client_count', self._client_count_callback, qos_profile=LATCHING_QOS) def _client_count_callback(self, msg: Int32): """Handles incoming messages on the client count topic.""" @@ -168,7 +171,7 @@ def _client_count_callback(self, msg: Int32): # --- In your startup logic --- def get_initial_client_count(self) -> int | None: """ - Synchronously calls the /get_client_count service. + Synchronously calls the /lucy/get_client_count service. """ # ... (implementation similar to get_hardware_config_yaml) ... ``` @@ -181,10 +184,12 @@ def get_initial_client_count(self) -> int | None: | Endpoint | Type | Message/Service Type | QoS Durability | Purpose | |---|---|---|---|---| -| `/control_panel_active_client` | Topic | `std_msgs/msg/String` | `TRANSIENT_LOCAL` | Take, release, and monitor control. | -| `/client_count` | Topic | `std_msgs/msg/Int32` | `TRANSIENT_LOCAL` | Get live updates on the number of connected clients. | +| `/lucy/client_heartbeat` | Topic | `std_msgs/msg/String` | `VOLATILE` | Pulse your client ID ~1 Hz to register as connected. | +| `/lucy/active_client` | Topic | `std_msgs/msg/String` | `TRANSIENT_LOCAL` | Monitor which client currently holds control. | +| `/lucy/client_count` | Topic | `std_msgs/msg/Int32` | `TRANSIENT_LOCAL` | Get live updates on the number of connected clients. | +| `/lucy/control` | Service | `lucy_msgs/srv/ClientControl` | N/A | Take or release control (`acquire` flag). | +| `/lucy/get_client_count` | Service | `lucy_msgs/srv/GetInt` | N/A | Get the initial number of connected clients on startup. | | `/config/get` | Service | `lucy_msgs/srv/GetConfig` | N/A | Fetch the active robot hardware configuration (YAML). | -| `/get_client_count` | Service | `lucy_msgs/srv/GetInt` | N/A | Get the initial number of connected clients on startup. | | `/{controller_name}/joint_trajectory` | Topic | `trajectory_msgs/msg/JointTrajectory` | `VOLATILE` | Send joint position commands to a specific controller. | | `/joint_states` | Topic | `sensor_msgs/msg/JointState` | `VOLATILE` | Read the actual, measured state of all robot joints. | diff --git a/lucy_cli/lucy_cli/ros_interface.py b/lucy_cli/lucy_cli/ros_interface.py index 12afeb8..8b0aa55 100644 --- a/lucy_cli/lucy_cli/ros_interface.py +++ b/lucy_cli/lucy_cli/ros_interface.py @@ -50,15 +50,30 @@ def on_control_change(controller_id): from rclpy.qos import QoSProfile, QoSHistoryPolicy, QoSReliabilityPolicy, QoSDurabilityPolicy from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint from std_msgs.msg import String, Int32 -from lucy_msgs.srv import GetConfig, GetInt +from sensor_msgs.msg import JointState +from lucy_msgs.srv import GetConfig, GetInt, ClientControl # --- Unique Client ID --- # A unique identifier for this specific client instance. CLIENT_ID = f"cli_{int(time.time())}_{random.randint(1000, 9999)}" -# --- Topic Names --- -CONTROL_MODE_TOPIC = '/control_panel_active_client' -CLIENT_COUNT_TOPIC = '/client_count' +# --- Topic & Service Names --- +# Presence and control go through the lucy_client_registry node; see lucy_cli/developer.md. +HEARTBEAT_TOPIC = '/lucy/client_heartbeat' +CLIENT_COUNT_TOPIC = '/lucy/client_count' +ACTIVE_CLIENT_TOPIC = '/lucy/active_client' +# Actual, measured joint positions published by ros2_control (URDF radians). +JOINT_STATES_TOPIC = '/joint_states' +GET_CLIENT_COUNT_SERVICE = '/lucy/get_client_count' +CONTROL_SERVICE = '/lucy/control' + +# How often we pulse our heartbeat so the registry keeps counting us as connected. +HEARTBEAT_PERIOD_S = 1.0 + +# Joint moves smaller than this (radians) don't count as a change. +JOINT_STATE_EPSILON_RAD = 0.001 +# Store /joint_states at full rate; drain changes to listeners at this period. +JOINT_STATE_DRAIN_PERIOD_S = 1.0 # --- QoS Profiles --- # For state topics that should be "latched" (i.e., the last published message @@ -100,26 +115,44 @@ def __init__(self): self._control_lock = threading.Lock() self._is_shutting_down = False + # Latest measured joint positions in URDF radians, keyed by joint name. + # Written at full rate by the subscription, read by the drain timer / UI. + self._joint_positions_rad = {} + # Positions at the last drain; changes are measured against this so slow, sub-epsilon-per-message motion still accumulates past the threshold. + self._notify_baseline_rad = {} + self._joint_state_lock = threading.Lock() + # --- Callbacks --- # These sets will hold functions to be called when state changes. self._on_control_change_callbacks = set() self._on_client_count_change_callbacks = set() + self._on_joint_state_change_callbacks = set() # --- Publishers --- - self.control_publisher = self.create_publisher( - String, CONTROL_MODE_TOPIC, qos_profile=LATCHING_QOS) + self.heartbeat_publisher = self.create_publisher( + String, HEARTBEAT_TOPIC, qos_profile=VOLATILE_QOS) # Joint publishers are created dynamically after config is loaded. self._joint_publishers = {} # --- Subscribers --- self.create_subscription( - String, CONTROL_MODE_TOPIC, self._control_callback, qos_profile=LATCHING_QOS) + String, ACTIVE_CLIENT_TOPIC, self._control_callback, qos_profile=LATCHING_QOS) self.create_subscription( Int32, CLIENT_COUNT_TOPIC, self._client_count_callback, qos_profile=LATCHING_QOS) + self.create_subscription( + JointState, JOINT_STATES_TOPIC, self._joint_state_callback, qos_profile=VOLATILE_QOS) # --- Service Clients --- self._get_config_client = self.create_client(GetConfig, '/config/get') - self._get_count_client = self.create_client(GetInt, '/get_client_count') + self._get_count_client = self.create_client(GetInt, GET_CLIENT_COUNT_SERVICE) + self._control_client = self.create_client(ClientControl, CONTROL_SERVICE) + + # --- Heartbeat --- + self._heartbeat_timer = self.create_timer(HEARTBEAT_PERIOD_S, self._publish_heartbeat) + + # --- Joint-state drain --- + self._joint_state_timer = self.create_timer( + JOINT_STATE_DRAIN_PERIOD_S, self._drain_joint_state_changes) # --- Executor --- # Spin the node in a background thread to handle callbacks automatically. @@ -129,8 +162,8 @@ def __init__(self): self._spin_thread.start() self.get_logger().info(f"LucyROSInterface started with Client ID: {CLIENT_ID}") - # Take control on startup - self.take_control() + # Register as connected. Control is opt-in (the user takes it from the TUI). + self._publish_heartbeat() # --- Public Methods: Callbacks --- @@ -153,8 +186,24 @@ def register_on_client_count_change(self, callback): """ self._on_client_count_change_callbacks.add(callback) + def register_on_joint_state_change(self, callback): + """ + Register a function to be called when measured joint positions change. + + The callback receives no arguments; call get_joint_positions_rad() to + read the latest state. Fired only when at least one joint moved beyond + JOINT_STATE_EPSILON_RAD, so an idle robot doesn't spam callbacks. + """ + self._on_joint_state_change_callbacks.add(callback) + # --- Public Methods: State & Commands --- + def get_joint_positions_rad(self) -> dict: + """Returns a snapshot of the latest measured joint positions (radians), + keyed by joint name.""" + with self._joint_state_lock: + return dict(self._joint_positions_rad) + @property def client_id(self) -> str: """Returns the unique ID for this client instance.""" @@ -165,25 +214,45 @@ def has_control(self) -> bool: with self._control_lock: return self._has_control + @property + def client_count(self) -> int: + """Returns the most recently known number of connected clients.""" + return self._client_count + + @property + def active_controller(self) -> str: + """Returns the id of the client currently in control, or '' if none.""" + return self._active_controller_id + def take_control(self): - """Publishes this client's ID to request control of the robot.""" + """Requests control of the robot from the client registry.""" if self._is_shutting_down: return self.get_logger().info("Requesting control...") - msg = String(data=CLIENT_ID) - try: - self.control_publisher.publish(msg) - except Exception: - pass # Ignore errors if already shutting down + self._send_control_request(acquire=True) def release_control(self): - """Publishes an empty string to release control of the robot.""" + """Releases control of the robot via the client registry.""" if self._is_shutting_down: return self.get_logger().info("Releasing control...") - msg = String(data="") + self._send_control_request(acquire=False) + + def _send_control_request(self, acquire: bool): + # The resulting controller comes back on ACTIVE_CLIENT_TOPIC. + try: + if not self._control_client.wait_for_service(timeout_sec=2.0): + self.get_logger().warn(f"'{CONTROL_SERVICE}' service not available.") + return + req = ClientControl.Request(client_id=CLIENT_ID, acquire=acquire) + self._control_client.call_async(req) + except Exception as e: + self.get_logger().error(f"Control request failed: {e}") + + def _publish_heartbeat(self): + if self._is_shutting_down: return try: - self.control_publisher.publish(msg) + self.heartbeat_publisher.publish(String(data=CLIENT_ID)) except Exception: - pass # Ignore errors if already shutting down + pass def publish_joint_trajectory(self, controller_topic: str, joint_names: list[str], positions_rad: list[float]): """ @@ -253,7 +322,7 @@ def get_hardware_config_yaml(self) -> str | None: def get_initial_client_count(self) -> int | None: """ - Synchronously calls the /get_client_count service. + Synchronously calls the registry's get-client-count service. This is a blocking call and should be used during application startup. @@ -262,7 +331,7 @@ def get_initial_client_count(self) -> int | None: """ try: if not self._get_count_client.wait_for_service(timeout_sec=3.0): - self.get_logger().warn("'/get_client_count' service not available.") + self.get_logger().warn(f"'{GET_CLIENT_COUNT_SERVICE}' service not available.") return None req = GetInt.Request() future = self._get_count_client.call_async(req) @@ -316,6 +385,33 @@ def _control_callback(self, msg: String): for cb in self._on_control_change_callbacks: cb(new_controller_id) + def _joint_state_callback(self, msg: JointState): + """Stores the latest measured joint positions at the full ROS rate.""" + with self._joint_state_lock: + for name, position in zip(msg.name, msg.position): + self._joint_positions_rad[name] = position + + def _drain_joint_state_changes(self): + """Notifies listeners at most once per drain, and only when a joint has + moved past the threshold since the last drain. Comparing against the + last-drained baseline (not the previous sample) lets slow motion + accumulate past it; an idle robot stays below and produces no redraws. + """ + if self._is_shutting_down: + return + changed = False + with self._joint_state_lock: + for name, position in self._joint_positions_rad.items(): + baseline = self._notify_baseline_rad.get(name) + if baseline is None or abs(baseline - position) > JOINT_STATE_EPSILON_RAD: + changed = True + break + if changed: + self._notify_baseline_rad.update(self._joint_positions_rad) + if changed: + for cb in self._on_joint_state_change_callbacks: + cb() + def _client_count_callback(self, msg: Int32): """Handles incoming messages on the client count topic.""" with self._control_lock: @@ -332,12 +428,13 @@ def _client_count_callback(self, msg: Int32): def shutdown(self): """Gracefully shuts down the node and its background thread.""" if self._is_shutting_down: return - self._is_shutting_down = True - + self.get_logger().info("Shutting down LucyROSInterface.") + # Release before flipping the guard, else release_control() short-circuits. self.release_control() - # Give a moment for the release message to be sent - time.sleep(0.1) + self._is_shutting_down = True + self._heartbeat_timer.cancel() + self._joint_state_timer.cancel() + time.sleep(0.1) # let the release request go out self._executor.shutdown() - # self._spin_thread.join() # This can hang, shutdown is enough - self.destroy_node() \ No newline at end of file + self.destroy_node() diff --git a/lucy_cli/lucy_cli/tui.py b/lucy_cli/lucy_cli/tui.py index 7e51536..efefa1c 100644 --- a/lucy_cli/lucy_cli/tui.py +++ b/lucy_cli/lucy_cli/tui.py @@ -20,6 +20,20 @@ # across screen refreshes in auto-refresh mode. INPUT_BUFFER = "" +# Self-pipe that lets a background thread wake get_user_input() from its select() to force a redraw. +# Only effective on POSIX, where select() watches it. +_wake_r, _wake_w = os.pipe() + +def notify_event(): + """Wake a blocked get_user_input() so the TUI can redraw on a state change. + + Safe to call from a background (ROS executor) thread. + """ + try: + os.write(_wake_w, b'x') + except OSError: + pass + def clear_screen(): """Clears the terminal screen using ANSI escape codes for a flicker-free update.""" if os.name == 'posix': @@ -60,7 +74,17 @@ def get_user_input(prompt: str, timeout: float = 1.0) -> str | None: old_settings = termios.tcgetattr(fd) try: tty.setcbreak(fd) - if select.select([sys.stdin], [], [], timeout)[0]: + ready = select.select([sys.stdin, _wake_r], [], [], timeout)[0] + if _wake_r in ready: + # A background state change asked for a redraw. Drain the pipe. + try: + os.read(_wake_r, 65536) + except OSError: + pass + # If the user wasn't also typing, redraw without touching the buffer. + if sys.stdin not in ready: + return None + if sys.stdin in ready: while True: char = sys.stdin.read(1) if char == '\n' or char == '\r': # Enter pressed @@ -102,13 +126,29 @@ def display_help_screen(): print(" 'q' - Quit the application.") print("\n[Main Menu]") print(" - Select an actuator group to view and edit its joints.") - print(" 't' - (When available) Forcibly take control from another client.") + print(" 'c' - Toggle control: take control of the robot, or release it if you already hold it.") print("\n[Joint Menu]") print(" - Select a joint to modify its angle.") print(" 'b' - Go back to the main menu.") print("\nPress Enter to return...") input() +def display_control_taken_popup(controller_id: str): + """Full-screen notice shown when another client takes control from us. + + Blocks until the user acknowledges, mirroring the front-end popup behaviour. + """ + global INPUT_BUFFER + INPUT_BUFFER = "" # Discard anything typed before the takeover. + clear_screen() + print("=" * 50) + print(" CONTROL TAKEN") + print("=" * 50) + print(f"\n '{controller_id}' has taken control of the robot.") + print(" You are now in read-only mode.\n") + print(" Press ENTER to continue...") + input() + def display_main_menu(state: dict): """ Renders the main menu screen. @@ -118,15 +158,21 @@ def display_main_menu(state: dict): 'client_count', 'autorefresh', 'has_control', 'active_controller', and 'actuator_groups'. """ - print("--- Robot Actuator TUI ---") - print(f"Control Panel Connection Count: {state.get('client_count', 'N/A')}") + print("--- Robot Actuator TUI ---\n") + print(f"Connected Clients: {state.get('client_count', 'N/A')}") if state.get('autorefresh'): print("[Auto-Refresh: ON]") - if not state.get('has_control') and state.get('active_controller'): - print(f"!! CONTROLLED BY: {state['active_controller']} !!\n") - print("Press 't' to take control.") - + if state.get('has_control'): + print(">> YOU ARE IN CONTROL of the robot <<") + print("Type 'c' to release control.") + else: + if state.get('active_controller'): + print(f"!! CONTROLLED BY: {state['active_controller']} !!") + else: + print("No client has control.") + print("Type 'c' to take control.") + print("\nSelect an actuator group:") for i, name in enumerate(state.get('actuator_groups', [])): print(f"{i+1}. {name}") @@ -141,7 +187,9 @@ def display_joint_menu(state: dict, group_name: str): group_name: The name of the actuator group being displayed. """ print(f"--- {group_name} ---") - if not state.get('has_control'): + if state.get('has_control'): + print(">> YOU ARE IN CONTROL of the robot <<\n") + else: print(f"!! READ-ONLY (Controlled by {state.get('active_controller')}) !!\n") joints = state.get('actuators', {}).get(group_name, {}).get('joints', []) @@ -173,4 +221,4 @@ def get_new_joint_value(joint_name: str) -> float | None: except (ValueError, TypeError): print("Invalid input. Please enter a number.") time.sleep(1) - return None \ No newline at end of file + return None diff --git a/lucy_cli/lucy_cli/tui_node.py b/lucy_cli/lucy_cli/tui_node.py index 1db203f..c5ef6a1 100644 --- a/lucy_cli/lucy_cli/tui_node.py +++ b/lucy_cli/lucy_cli/tui_node.py @@ -1,19 +1,119 @@ """ -The main entry point for the Lucy CLI TUI Actuator Node. -... +Entry point for the Lucy CLI TUI actuator node: connects the ROS layer +(ros_interface.py) to the terminal UI (tui.py) and runs the menu loops. """ import sys +import os import argparse import time import rclpy import math import yaml from .ros_interface import LucyROSInterface +from . import tui from .tui import ( clear_screen, get_user_input, display_help_screen, - display_main_menu, display_joint_menu, get_new_joint_value + display_main_menu, display_joint_menu, get_new_joint_value, + display_control_taken_popup, ) +# Shared between the on_control_change callback (background ROS thread) and the TUI loop. +# 'by' is the id of the client to announce in the takeover popup, or None when there is nothing to announce. +# 'was_ours' is whether we held control at the previous change. +_control_takeover = {'by': None, 'was_ours': False} + +def _consume_control_takeover() -> bool: + """Show the takeover popup if another client just grabbed control. + + Returns True if a popup was shown (the caller should restart its loop so the + menu redraws with the updated state). + """ + controller_id = _control_takeover['by'] + if controller_id is None: + return False + _control_takeover['by'] = None + display_control_taken_popup(controller_id) + return True + +def _actuator_deg_to_joint_rad(actuator_deg: float, mapping: dict) -> float: + """Servo degrees -> URDF joint radians. Mirrors LCP's actuatorDegToJointRad + so commands from the CLI land where LCP would put them.""" + joint_deg = (actuator_deg - mapping['offset_deg']) * mapping['direction'] * mapping['scale'] + return math.radians(joint_deg) + +def _joint_rad_to_actuator_deg(joint_rad: float, mapping: dict) -> float: + """URDF joint radians -> servo degrees. Mirrors LCP's jointRadToActuatorDeg + so /joint_states feedback reads in the same units the menu displays.""" + denom = mapping['direction'] * mapping['scale'] + if denom == 0: + return mapping['offset_deg'] + return math.degrees(joint_rad) / denom + mapping['offset_deg'] + +def _build_state(ros: LucyROSInterface, actuators: dict, autorefresh: bool) -> dict: + """Snapshots the current UI state passed to the rendering functions. + + Overlays each joint's displayed value with its actual measured position from + /joint_states (URDF radians), converted to servo degrees via the joint's + per-actuator calibration, so the menus show the robot's live state in the + same units LCP uses regardless of which client moved it. Joints without + feedback yet keep their config default. + """ + live_positions_rad = ros.get_joint_positions_rad() + for group in actuators.values(): + for joint in group['joints']: + position_rad = live_positions_rad.get(joint['name']) + if position_rad is not None: + joint['value'] = _joint_rad_to_actuator_deg(position_rad, joint['mapping']) + return { + 'autorefresh': autorefresh, + 'client_count': ros.client_count, + 'has_control': ros.has_control(), + 'active_controller': ros.active_controller, + 'actuator_groups': list(actuators.keys()), + 'actuators': actuators, + } + +def _prompt_choice(autorefresh: bool) -> str | None: + """Reads one menu choice, returning None when the user typed nothing. + + The short timeout under auto-refresh lets the screen repaint periodically; + otherwise we wait essentially indefinitely (a background event still wakes + the input via tui.notify_event()). + """ + timeout = 1.0 if autorefresh else 3600.0 + choice = get_user_input("> ", timeout=timeout) + if choice is None or choice == '': + return None + return choice.lower() + +def _handle_common_command(ros: LucyROSInterface, choice: str, autorefresh: bool, + has_control: bool) -> tuple[bool, bool, bool]: + """Handles keys shared by every menu (quit / help / auto-refresh / control). + + Returns (handled, autorefresh, quit): `handled` is False when `choice` is not + a common command and the caller should apply its menu-specific logic. + """ + if choice == 'q': + return True, autorefresh, True + if choice == 'h': + display_help_screen() + return True, autorefresh, False + if choice == 'a': + autorefresh = not autorefresh + print(f"Auto-refresh {'enabled' if autorefresh else 'disabled'}.") + time.sleep(0.7) + return True, autorefresh, False + if choice == 'c': + if has_control: + ros.release_control() + print("Releasing control...") + else: + ros.take_control() + print("Requesting control...") + time.sleep(0.5) + return True, autorefresh, False + return False, autorefresh, False + def run_tui(ros: LucyROSInterface, actuators: dict, autorefresh: bool): """ The main application loop. @@ -23,117 +123,104 @@ def run_tui(ros: LucyROSInterface, actuators: dict, autorefresh: bool): actuators: The dynamic actuator configuration dictionary. autorefresh: Boolean indicating if auto-refresh is enabled by default. """ - current_autorefresh = autorefresh - while rclpy.ok(): + if _consume_control_takeover(): + continue + clear_screen() - - state = { - 'autorefresh': current_autorefresh, - 'client_count': ros._client_count, - 'has_control': ros.has_control(), - 'active_controller': ros._active_controller_id, - 'actuator_groups': list(actuators.keys()), - 'actuators': actuators - } - + state = _build_state(ros, actuators, autorefresh) display_main_menu(state) - - timeout = 1.0 if state['autorefresh'] else 3600.0 - choice = get_user_input("> ", timeout=timeout) - - if choice is None or choice == '': + + choice = _prompt_choice(autorefresh) + if choice is None: continue - - choice = choice.lower() - if choice == 'q': + + handled, autorefresh, quit_app = _handle_common_command( + ros, choice, autorefresh, state['has_control']) + if quit_app: break - elif choice == 'h': - display_help_screen() - elif choice == 'a': - current_autorefresh = not current_autorefresh - print(f"Auto-refresh {'enabled' if current_autorefresh else 'disabled'}.") - time.sleep(0.7) - elif choice == 't' and not state['has_control']: - ros.take_control() - print("Requesting control...") - time.sleep(0.5) - else: - try: - idx = int(choice) - 1 - if 0 <= idx < len(state['actuator_groups']): - group_name = state['actuator_groups'][idx] - current_autorefresh = handle_group_menu(ros, actuators, group_name, current_autorefresh) - else: - raise ValueError - except ValueError: - print("Invalid input. Refreshing...") - time.sleep(1) - -def handle_group_menu(ros: LucyROSInterface, actuators: dict, group_name: str, autorefresh: bool) -> bool: - """Handles the sub-menu for a specific actuator group.""" - current_autorefresh = autorefresh - + if handled: + continue + + # Menu-specific: select an actuator group by number. + try: + idx = int(choice) - 1 + if not (0 <= idx < len(state['actuator_groups'])): + raise ValueError + except ValueError: + print("Invalid input. Refreshing...") + time.sleep(1) + continue + + group_name = state['actuator_groups'][idx] + autorefresh, quit_app = handle_group_menu(ros, actuators, group_name, autorefresh) + if quit_app: + break + +def handle_group_menu(ros: LucyROSInterface, actuators: dict, group_name: str, + autorefresh: bool) -> tuple[bool, bool]: + """Handles the sub-menu for a specific actuator group. + + Returns (autorefresh, quit): `quit` is True when the user asked to quit the + whole application, so the caller can unwind to main()'s cleanup. + """ while rclpy.ok(): + if _consume_control_takeover(): + continue + clear_screen() - - state = { - 'autorefresh': current_autorefresh, - 'client_count': ros._client_count, - 'has_control': ros.has_control(), - 'active_controller': ros._active_controller_id, - 'actuator_groups': list(actuators.keys()), - 'actuators': actuators - } - + state = _build_state(ros, actuators, autorefresh) display_joint_menu(state, group_name) - - timeout = 1.0 if state['autorefresh'] else 3600.0 - choice = get_user_input("> ", timeout=timeout) - - if choice is None or choice == '': + + choice = _prompt_choice(autorefresh) + if choice is None: continue - - choice = choice.lower() + if choice == 'b': - break - elif choice == 'q': - ros.shutdown() - rclpy.try_shutdown() - sys.exit(0) - elif choice == 'h': - display_help_screen() - elif choice == 'a': - current_autorefresh = not current_autorefresh - print(f"Auto-refresh {'enabled' if current_autorefresh else 'disabled'}.") - time.sleep(0.7) - elif state['has_control']: - try: - joints = state['actuators'][group_name]['joints'] - joint_idx = int(choice) - 1 - if 0 <= joint_idx < len(joints): - selected_joint = joints[joint_idx] - new_val = get_new_joint_value(selected_joint['name']) - if new_val is not None: - if selected_joint['min'] <= new_val <= selected_joint['max']: - selected_joint['value'] = new_val - - names = [j['name'] for j in joints] - positions = [math.radians(j['value']) for j in joints] - topic = state['actuators'][group_name]['topic'] - - ros.publish_joint_trajectory(topic, names, positions) - print("Value updated successfully. Refreshing...") - else: - print(f"Value out of range ({selected_joint['min']} - {selected_joint['max']}).") - time.sleep(1) - else: - raise ValueError - except ValueError: - print("Invalid input.") - time.sleep(1) - - return current_autorefresh + return autorefresh, False + + handled, autorefresh, quit_app = _handle_common_command( + ros, choice, autorefresh, state['has_control']) + if quit_app: + return autorefresh, True + if handled: + continue + + # Menu-specific: edit a joint by number (only when we hold control). + if state['has_control']: + _edit_joint(ros, state, group_name, choice) + + return autorefresh, False + +def _edit_joint(ros: LucyROSInterface, state: dict, group_name: str, choice: str): + """Prompts for and publishes a new value for the chosen joint, if valid.""" + joints = state['actuators'][group_name]['joints'] + try: + joint_idx = int(choice) - 1 + if not (0 <= joint_idx < len(joints)): + raise ValueError + except ValueError: + print("Invalid input.") + time.sleep(1) + return + + selected_joint = joints[joint_idx] + new_val = get_new_joint_value(selected_joint['name']) + if new_val is None: + return # get_new_joint_value already reported the error. + + if not (selected_joint['min'] <= new_val <= selected_joint['max']): + print(f"Value out of range ({selected_joint['min']} - {selected_joint['max']}).") + time.sleep(1) + return + + selected_joint['value'] = new_val + names = [j['name'] for j in joints] + positions = [_actuator_deg_to_joint_rad(j['value'], j['mapping']) for j in joints] + topic = state['actuators'][group_name]['topic'] + ros.publish_joint_trajectory(topic, names, positions) + print("Value updated successfully. Refreshing...") + time.sleep(1) def parse_config_yaml(yaml_string: str) -> dict: """Parses the hardware config YAML and builds the ACTUATORS dictionary.""" @@ -157,6 +244,12 @@ def parse_config_yaml(yaml_string: str) -> dict: "min": float(actuator.get('servo_min_deg', 0.0)), "max": float(actuator.get('servo_max_deg', 180.0)), "value": float(actuator.get('servo_default_deg', 90.0)), + # Servo<->URDF calibration (defaults match LCP: identity map). + "mapping": { + "offset_deg": float(actuator.get('offset_deg', 0.0)), + "direction": float(actuator.get('direction', 1.0)), + "scale": float(actuator.get('scale', 1.0)), + }, }) if joints: group_name = board_id.replace("rp2040_", "").replace("_", " ").title() @@ -166,6 +259,30 @@ def parse_config_yaml(yaml_string: str) -> dict: print(f"Error parsing YAML: {e}") return {} +# The file the launcher's readiness_check tests for: created only once the TUI is up, so the launcher reports LOADING during startup and RUNNING from the main menu on. +READY_MARKER = '/tmp/lucy_cli.ready' + +def _set_ready_marker(): + try: + with open(READY_MARKER, 'w') as f: + f.write(str(os.getpid())) + except OSError: + pass + +def _clear_ready_marker(): + try: + os.remove(READY_MARKER) + except OSError: + pass + +def _fail(ros: LucyROSInterface, message: str): + """Reports a startup failure, tears down ROS, and exits non-zero.""" + _clear_ready_marker() + print(message) + ros.shutdown() + rclpy.try_shutdown() + sys.exit(1) + def main(args=None): parser = argparse.ArgumentParser(description='Lucy TUI Actuator Node') parser.add_argument('-a', '--autorefresh', action='store_true', help='Enable auto-refresh for the TUI screen') @@ -173,32 +290,55 @@ def main(args=None): sys_args = sys.argv[1:] if args is None else args parsed_args, ros_args = parser.parse_known_args(sys_args) + # Drop any stale marker from a previous run before we start fetching, so the launcher shows LOADING until this run reaches the main menu. + _clear_ready_marker() + rclpy.init(args=ros_args) ros_interface = LucyROSInterface() + # Wake the (possibly blocked) TUI input loop on background state changes so the screen refreshes immediately instead of only after the next keypress. + def on_client_count_change(_count): + tui.notify_event() + + # The interface already drains joint-state changes at a fixed rate, so just wake the input loop to redraw. + def on_joint_state_change(): + tui.notify_event() + + def on_control_change(controller_id): + # Show the takeover popup only when we held control and another client now holds it. + # An empty controller_id means control was released. + now_ours = (controller_id == ros_interface.client_id) + taken_by_other = bool(controller_id) and not now_ours + if _control_takeover['was_ours'] and taken_by_other: + _control_takeover['by'] = controller_id + _control_takeover['was_ours'] = now_ours + tui.notify_event() + + ros_interface.register_on_client_count_change(on_client_count_change) + ros_interface.register_on_control_change(on_control_change) + ros_interface.register_on_joint_state_change(on_joint_state_change) + print("Fetching active hardware configuration...") config_yaml = ros_interface.get_hardware_config_yaml() if not config_yaml: - print("Could not retrieve hardware configuration. Is lucy_config_pipeline running?") - ros_interface.shutdown() - rclpy.shutdown() - sys.exit(1) + _fail(ros_interface, "Could not retrieve hardware configuration. Is lucy_config_pipeline running?") actuators = parse_config_yaml(config_yaml) if not actuators: - print("Failed to parse hardware configuration or no actuators found.") - ros_interface.shutdown() - rclpy.shutdown() - sys.exit(1) + _fail(ros_interface, "Failed to parse hardware configuration or no actuators found.") print("Fetching initial client count...") ros_interface.get_initial_client_count() + # Config and client count are in; the main menu is about to render. + _set_ready_marker() + try: run_tui(ros_interface, actuators, parsed_args.autorefresh) except KeyboardInterrupt: pass finally: + _clear_ready_marker() print("\nShutting down...") ros_interface.shutdown() rclpy.try_shutdown() diff --git a/lucy_config_pipeline/launch/config_pipeline.launch.py b/lucy_config_pipeline/launch/config_pipeline.launch.py index a70802d..313856b 100644 --- a/lucy_config_pipeline/launch/config_pipeline.launch.py +++ b/lucy_config_pipeline/launch/config_pipeline.launch.py @@ -20,4 +20,11 @@ def generate_launch_description() -> LaunchDescription: ], ) - return LaunchDescription([robot_pkg_arg, config_dir_arg, node]) + # Separate process so a registry fault can never take down config services. + registry_node = Node( + package="lucy_config_pipeline", + executable="client_registry_node", + output="screen", + ) + + return LaunchDescription([robot_pkg_arg, config_dir_arg, node, registry_node]) diff --git a/lucy_config_pipeline/setup.py b/lucy_config_pipeline/setup.py index ef9e464..35236a4 100644 --- a/lucy_config_pipeline/setup.py +++ b/lucy_config_pipeline/setup.py @@ -21,6 +21,7 @@ entry_points={ "console_scripts": [ "config_pipeline_node = src.main:main", + "client_registry_node = src.services.client_registry_node:main", ], }, ) diff --git a/lucy_config_pipeline/src/services/client_registry_node.py b/lucy_config_pipeline/src/services/client_registry_node.py new file mode 100644 index 0000000..67defd1 --- /dev/null +++ b/lucy_config_pipeline/src/services/client_registry_node.py @@ -0,0 +1,160 @@ +"""Transport-agnostic client presence and control arbitration. + +See lucy_cli/developer.md for the protocol. Clients register by heart-beating +their id; this node is the single writer of the connected-client count and the +active controller, so the web panel (rosbridge) and the CLI (DDS) behave alike. +""" + +from __future__ import annotations + +import threading +import time + +from lucy_msgs.srv import ClientControl, GetInt +import rclpy +from rclpy.node import Node +from rclpy.qos import ( + QoSDurabilityPolicy, + QoSHistoryPolicy, + QoSProfile, + QoSReliabilityPolicy, +) +from std_msgs.msg import Int32, String + +HEARTBEAT_TOPIC = "/lucy/client_heartbeat" +CLIENT_COUNT_TOPIC = "/lucy/client_count" +ACTIVE_CLIENT_TOPIC = "/lucy/active_client" +GET_CLIENT_COUNT_SERVICE = "/lucy/get_client_count" +CONTROL_SERVICE = "/lucy/control" + +TICK_PERIOD_S = 1.0 +CLIENT_TTL_S = 3.0 # ~3 missed beats at the 1 Hz client cadence + +_LATCHED_QOS = QoSProfile( + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + reliability=QoSReliabilityPolicy.RELIABLE, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, +) +# Volatile keeps us compatible with rosbridge's default publisher QoS. +_HEARTBEAT_QOS = QoSProfile( + history=QoSHistoryPolicy.KEEP_LAST, + depth=10, + reliability=QoSReliabilityPolicy.RELIABLE, + durability=QoSDurabilityPolicy.VOLATILE, +) + + +class ClientRegistryNode(Node): + """Single-writer registry for connected clients and the active controller.""" + + def __init__(self) -> None: + super().__init__("lucy_client_registry") + + # client_id -> last-seen monotonic time. + # Guarded by _state_lock: callbacks may run on different executor threads. + # (Not _clients — Node uses that.) + self._last_seen: dict[str, float] = {} + self._active_client: str = "" + self._state_lock = threading.Lock() + + self._count_pub = self.create_publisher(Int32, CLIENT_COUNT_TOPIC, _LATCHED_QOS) + self._active_pub = self.create_publisher(String, ACTIVE_CLIENT_TOPIC, _LATCHED_QOS) + + self.create_subscription(String, HEARTBEAT_TOPIC, self._on_heartbeat, _HEARTBEAT_QOS) + self.create_service(GetInt, GET_CLIENT_COUNT_SERVICE, self._on_get_client_count) + self.create_service(ClientControl, CONTROL_SERVICE, self._on_control) + + self._tick_timer = self.create_timer(TICK_PERIOD_S, self._on_tick) + + self._publish_count() + self._publish_active() + self.get_logger().info("Client registry started") + + def _on_heartbeat(self, msg: String) -> None: + client_id = msg.data + if not client_id: + return + with self._state_lock: + is_new = client_id not in self._last_seen + self._last_seen[client_id] = time.monotonic() + count = len(self._last_seen) + if is_new: + self.get_logger().info(f"Client registered: {client_id} ({count} connected)") + self._publish_count() + + def _on_tick(self) -> None: + deadline = time.monotonic() - CLIENT_TTL_S + with self._state_lock: + expired = [cid for cid, seen in self._last_seen.items() if seen < deadline] + for cid in expired: + del self._last_seen[cid] + controller_lost = bool(self._active_client) and self._active_client not in self._last_seen + if controller_lost: + self._active_client = "" + count = len(self._last_seen) + for cid in expired: + self.get_logger().info(f"Client expired: {cid} ({count} connected)") + if controller_lost: + self.get_logger().info("Active controller expired; control released") + # Republish so volatile subscribers (web via rosbridge) converge without a dedicated query; subscribers dedupe by value. + self._publish_count() + self._publish_active() + + def _on_get_client_count( + self, _req: GetInt.Request, res: GetInt.Response + ) -> GetInt.Response: + with self._state_lock: + res.value = len(self._last_seen) + return res + + def _on_control( + self, req: ClientControl.Request, res: ClientControl.Response + ) -> ClientControl.Response: + if not req.client_id: + with self._state_lock: + res.active_client = self._active_client + res.success = False + res.message = "empty client_id" + return res + with self._state_lock: + is_new = req.client_id not in self._last_seen # a request also proves liveness + self._last_seen[req.client_id] = time.monotonic() + if req.acquire: + self._active_client = req.client_id + elif self._active_client == req.client_id: + self._active_client = "" + res.active_client = self._active_client + if is_new: + self._publish_count() + self.get_logger().info( + f"Control {'granted to' if req.acquire else 'released by'} {req.client_id}" + ) + self._publish_active() + res.success = True + res.message = "ok" + return res + + def _publish_count(self) -> None: + with self._state_lock: + count = len(self._last_seen) + self._count_pub.publish(Int32(data=count)) + + def _publish_active(self) -> None: + with self._state_lock: + active = self._active_client + self._active_pub.publish(String(data=active)) + + +def main() -> None: # pragma: no cover + rclpy.init() + node = ClientRegistryNode() + try: + rclpy.spin(node) + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": # pragma: no cover + main() diff --git a/lucy_config_pipeline/src/services/config_services_node.py b/lucy_config_pipeline/src/services/config_services_node.py index fe2af79..ae5b5c0 100644 --- a/lucy_config_pipeline/src/services/config_services_node.py +++ b/lucy_config_pipeline/src/services/config_services_node.py @@ -6,14 +6,12 @@ ActivateConfig, DeleteConfig, GetConfig, - GetInt, GetMesh, ListConfigs, SaveConfig, ) import rclpy from rclpy.node import Node -from std_msgs.msg import Int32 from ..config_store import ConfigStore from ..error_format import format_error_lines @@ -44,19 +42,6 @@ def __init__( self.create_service(DeleteConfig, "config/delete", self._on_delete_config) self.create_service(GetMesh, "mesh/get", self._on_get_mesh) - self._client_count: int = 0 - self.create_subscription(Int32, "/client_count", self._on_client_count, 10) - self.create_service(GetInt, "/get_client_count", self._on_get_client_count) - - def _on_client_count(self, msg: Int32) -> None: - self._client_count = msg.data - - def _on_get_client_count( - self, _req: GetInt.Request, res: GetInt.Response - ) -> GetInt.Response: - res.value = self._client_count - return res - def _on_list_configs( self, _req: ListConfigs.Request, res: ListConfigs.Response ) -> ListConfigs.Response: diff --git a/lucy_config_pipeline/test/services/test_client_registry_node.py b/lucy_config_pipeline/test/services/test_client_registry_node.py new file mode 100644 index 0000000..9997a12 --- /dev/null +++ b/lucy_config_pipeline/test/services/test_client_registry_node.py @@ -0,0 +1,106 @@ +import time + +import pytest +import rclpy +from std_msgs.msg import String + +from src.services.client_registry_node import ClientRegistryNode + + +@pytest.fixture +def rclpy_init_shutdown(): + rclpy.init() + yield + rclpy.shutdown() + + +@pytest.fixture +def node(rclpy_init_shutdown): + n = ClientRegistryNode() + yield n + n.destroy_node() + + +class TestClientRegistryNode: + def test_heartbeat_registers_and_counts(self, node): + from lucy_msgs.srv import GetInt + + node._on_heartbeat(String(data="cli_1")) + node._on_heartbeat(String(data="cp_2")) + node._on_heartbeat(String(data="cli_1")) # duplicate refresh, not a new client + + res = node._on_get_client_count(GetInt.Request(), GetInt.Response()) + assert res.value == 2 + + def test_empty_heartbeat_ignored(self, node): + from lucy_msgs.srv import GetInt + + node._on_heartbeat(String(data="")) + + res = node._on_get_client_count(GetInt.Request(), GetInt.Response()) + assert res.value == 0 + + def test_take_then_preempt_then_release_control(self, node): + from lucy_msgs.srv import ClientControl + + def control(client_id, acquire): + return node._on_control( + ClientControl.Request(client_id=client_id, acquire=acquire), + ClientControl.Response(), + ) + + res = control("cli_1", True) + assert res.success is True + assert res.active_client == "cli_1" + + # Control is preemptive: the latest acquirer wins. + assert control("cp_2", True).active_client == "cp_2" + + # A non-owner releasing is a no-op. + assert control("cli_1", False).active_client == "cp_2" + + # The owner releasing frees control. + assert control("cp_2", False).active_client == "" + + def test_empty_client_id_rejected(self, node): + from lucy_msgs.srv import ClientControl + + res = node._on_control( + ClientControl.Request(client_id="", acquire=True), ClientControl.Response() + ) + assert res.success is False + assert res.active_client == "" + + def test_control_request_also_registers_client(self, node): + from lucy_msgs.srv import ClientControl, GetInt + + node._on_control( + ClientControl.Request(client_id="cli_1", acquire=True), ClientControl.Response() + ) + + res = node._on_get_client_count(GetInt.Request(), GetInt.Response()) + assert res.value == 1 + + def test_tick_prunes_stale_clients(self, node): + from lucy_msgs.srv import GetInt + + node._on_heartbeat(String(data="cli_1")) + # Backdate the heartbeat well past the TTL. + node._last_seen["cli_1"] = time.monotonic() - 999 + node._on_tick() + + res = node._on_get_client_count(GetInt.Request(), GetInt.Response()) + assert res.value == 0 + + def test_tick_releases_control_when_controller_expires(self, node): + from lucy_msgs.srv import ClientControl + + node._on_control( + ClientControl.Request(client_id="cli_1", acquire=True), ClientControl.Response() + ) + assert node._active_client == "cli_1" + + node._last_seen["cli_1"] = time.monotonic() - 999 + node._on_tick() + + assert node._active_client == "" diff --git a/lucy_config_pipeline/test/services/test_config_services_node.py b/lucy_config_pipeline/test/services/test_config_services_node.py index 69dd2ad..9dfb920 100644 --- a/lucy_config_pipeline/test/services/test_config_services_node.py +++ b/lucy_config_pipeline/test/services/test_config_services_node.py @@ -15,9 +15,9 @@ def rclpy_init_shutdown(): class TestConfigServicesNode: - def test_get_client_count_service(self, rclpy_init_shutdown, tmp_path: Path): - """Test /get_client_count service returns the current client count.""" - from lucy_msgs.srv import GetInt + def test_get_config_missing_returns_failure(self, rclpy_init_shutdown, tmp_path: Path): + """Unknown/active config on an empty store fails gracefully.""" + from lucy_msgs.srv import GetConfig node = ConfigServicesNode( robot_package="test_pkg", @@ -26,9 +26,9 @@ def test_get_client_count_service(self, rclpy_init_shutdown, tmp_path: Path): base_path=tmp_path, controller_config=Path("/dev/null"), ) - node._client_count = 5 - - response = node._on_get_client_count(GetInt.Request(), GetInt.Response()) - - assert response.value == 5 - node.destroy_node() + try: + response = node._on_get_config(GetConfig.Request(), GetConfig.Response()) + assert response.success is False + assert response.robot_package == "test_pkg" + finally: + node.destroy_node() diff --git a/lucy_msgs/CMakeLists.txt b/lucy_msgs/CMakeLists.txt index e687ded..67fe2cb 100644 --- a/lucy_msgs/CMakeLists.txt +++ b/lucy_msgs/CMakeLists.txt @@ -10,6 +10,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/RawSensor.msg" "action/ConfigurePipeline.action" "srv/GetInt.srv" + "srv/ClientControl.srv" "srv/ListConfigs.srv" "srv/GetConfig.srv" "srv/GetMesh.srv" diff --git a/lucy_msgs/srv/ClientControl.srv b/lucy_msgs/srv/ClientControl.srv new file mode 100644 index 0000000..1dbbbc2 --- /dev/null +++ b/lucy_msgs/srv/ClientControl.srv @@ -0,0 +1,8 @@ +# Request +string client_id # requesting client's unique id +bool acquire # true = take control, false = release control +--- +# Response +bool success # whether the request was applied +string active_client # resulting controller id ("" = nobody in control) +string message