diff --git a/commands/basic_commands.py b/commands/basic_commands.py index 30a5ed9..150a156 100644 --- a/commands/basic_commands.py +++ b/commands/basic_commands.py @@ -6,7 +6,6 @@ import logging import numpy as np import PAROL6_ROBOT -from .ik_helpers import CommandValue logger = logging.getLogger(__name__) @@ -121,7 +120,7 @@ def __init__(self, joint, speed_percentage=None, duration=None, distance_deg=Non def prepare_for_execution(self, current_position_in): """Pre-computes speeds and target positions using live data.""" - logger.debug(f" -> Preparing for Jog command...") + logger.debug(" -> Preparing for Jog command...") # Joint direction and index mapping self.direction = 1 if 0 <= self.joint <= 5 else -1 @@ -261,7 +260,7 @@ def __init__(self, joints, speed_percentages, duration): def prepare_for_execution(self, current_position_in): """Pre-computes the speeds for each joint.""" - logger.debug(f" -> Preparing for MultiJog command...") + logger.debug(" -> Preparing for MultiJog command...") for i, joint in enumerate(self.joints): # Index mapping: 0-5 positive, 6-11 negative direction diff --git a/commands/gripper_commands.py b/commands/gripper_commands.py index 4387c0e..cee78d8 100644 --- a/commands/gripper_commands.py +++ b/commands/gripper_commands.py @@ -127,7 +127,7 @@ def execute_step(self, Position_in, Homed_in, Speed_out, Command_out, # Check for completion current_position = Gripper_data_in[1] if abs(current_position - self.target_position) <= 5: - logger.info(f" -> Gripper move complete.") + logger.info(" -> Gripper move complete.") self.is_finished = True # Set command back to idle bitfield = [1, 0, not InOut_in[4], 1, 0, 0, 0, 0] @@ -136,12 +136,12 @@ def execute_step(self, Position_in, Homed_in, Speed_out, Command_out, return True if (object_detection == 1) and (self.target_position > current_position): - logger.info(f" -> Gripper move holding position due to object detection when closing.") + logger.info(" -> Gripper move holding position due to object detection when closing.") self.is_finished = True return True if (object_detection == 2) and (self.target_position < current_position): - logger.info(f" -> Gripper move holding position due to object detection when opening.") + logger.info(" -> Gripper move holding position due to object detection when opening.") self.is_finished = True return True diff --git a/commands/ik_helpers.py b/commands/ik_helpers.py index be8ba99..d369d6b 100644 --- a/commands/ik_helpers.py +++ b/commands/ik_helpers.py @@ -6,7 +6,6 @@ import numpy as np import logging from collections import namedtuple -from typing import Optional from roboticstoolbox import DHRobot from spatialmath import SE3 from spatialmath.base import trinterp diff --git a/commands/joint_commands.py b/commands/joint_commands.py index 013d7ed..c254804 100644 --- a/commands/joint_commands.py +++ b/commands/joint_commands.py @@ -114,7 +114,7 @@ def prepare_for_execution(self, current_position_in): except Exception as e: logger.error(f" -> VALIDATION FAILED: Could not calculate velocity-based trajectory. Error: {e}") - logger.info(f" -> Please check Joint_min/max_speed and Joint_min/max_acc values in PAROL6_ROBOT.py.") + logger.info(" -> Please check Joint_min/max_speed and Joint_min/max_acc values in PAROL6_ROBOT.py.") self.is_valid = False return diff --git a/commands/smooth_commands.py b/commands/smooth_commands.py index c2263aa..0ba0847 100644 --- a/commands/smooth_commands.py +++ b/commands/smooth_commands.py @@ -8,16 +8,13 @@ import PAROL6_ROBOT from spatialmath import SE3 from smooth_motion import ( - CircularMotion, SplineMotion, MotionBlender, - HelixMotion, AdvancedMotionBlender, WaypointTrajectoryPlanner + CircularMotion, SplineMotion, HelixMotion, WaypointTrajectoryPlanner ) from .ik_helpers import solve_ik_with_adaptive_tol_subdivision +from .cartesian_commands import MovePoseCommand logger = logging.getLogger(__name__) -# Import MovePoseCommand for transition commands -from .cartesian_commands import MovePoseCommand - def transform_command_params_to_wrf(command_type: str, params: dict, frame: str, current_position_in) -> dict: """ Transform command parameters from TRF to WRF. @@ -313,7 +310,7 @@ def prepare_for_execution(self, current_position_in): if self.transition_command: self.transition_command.prepare_for_execution(current_position_in) if not self.transition_command.is_valid: - logger.error(f" -> ERROR: Cannot reach specified start position") + logger.error(" -> ERROR: Cannot reach specified start position") self.is_valid = False self.error_state = True self.error_message = "Cannot reach specified start position" @@ -342,7 +339,7 @@ def execute_step(self, Position_in, Homed_in, Speed_out, Command_out, **kwargs): ) if is_done: - logger.info(f" -> Transition complete") + logger.info(" -> Transition complete") self.transition_complete = True return False @@ -370,7 +367,7 @@ def execute_step(self, Position_in, Homed_in, Speed_out, Command_out, **kwargs): ) if not ik_result.success: - logger.error(f" -> ERROR: Cannot reach first trajectory point") + logger.error(" -> ERROR: Cannot reach first trajectory point") self.is_finished = True self.error_state = True self.error_message = "Cannot reach trajectory start" @@ -884,7 +881,7 @@ def prepare_for_execution(self, current_position_in): def generate_main_trajectory(self, effective_start_pose): """Generate helix with entry trajectory if needed and proper trajectory profile.""" # Import here to avoid circular dependencies - from smooth_motion import HelixMotion, CircularMotion + from smooth_motion import CircularMotion helix_gen = HelixMotion() # Get helix axis (default Z for WRF, transformed for TRF) @@ -1024,7 +1021,7 @@ def generate_main_trajectory(self, effective_start_pose): else: # Replace first waypoint with actual start to ensure continuity modified_waypoints = [effective_start_pose] + self.waypoints[1:] - logger.info(f" Replaced first waypoint with actual start position") + logger.info(" Replaced first waypoint with actual start position") timestamps = np.linspace(0, self.duration, len(modified_waypoints)) @@ -1291,7 +1288,6 @@ def prepare_for_execution(self, current_position_in): def generate_main_trajectory(self, effective_start_pose): """Generate waypoint trajectory with corner cutting.""" - from smooth_motion import WaypointTrajectoryPlanner # Ensure first waypoint matches effective start pose first_wp_error = np.linalg.norm( diff --git a/gcode/commands.py b/gcode/commands.py index 19cde6c..d748507 100644 --- a/gcode/commands.py +++ b/gcode/commands.py @@ -6,7 +6,7 @@ """ import numpy as np -from typing import Dict, List, Optional, Tuple +from typing import Dict, Optional import sys import os diff --git a/gcode/coordinates.py b/gcode/coordinates.py index daffe1b..9c08da5 100644 --- a/gcode/coordinates.py +++ b/gcode/coordinates.py @@ -7,8 +7,7 @@ import json import os -import numpy as np -from typing import Dict, List, Optional, Tuple +from typing import Dict, List, Optional class WorkCoordinateSystem: diff --git a/gcode/interpreter.py b/gcode/interpreter.py index e0105bc..36e5437 100644 --- a/gcode/interpreter.py +++ b/gcode/interpreter.py @@ -7,11 +7,11 @@ import os import numpy as np -from typing import List, Dict, Optional, Union, Tuple -from .parser import GcodeParser, GcodeToken +from typing import List, Dict, Optional, Union +from .parser import GcodeParser from .state import GcodeState from .coordinates import WorkCoordinateSystem -from .commands import create_command_from_token, GcodeCommand +from .commands import create_command_from_token class GcodeInterpreter: diff --git a/gcode/state.py b/gcode/state.py index 7119a60..8cc3ae6 100644 --- a/gcode/state.py +++ b/gcode/state.py @@ -11,8 +11,8 @@ import json import os -from typing import Dict, Optional, Tuple -from dataclasses import dataclass, field, asdict +from typing import Dict, Optional +from dataclasses import dataclass, field @dataclass diff --git a/gcode/utils.py b/gcode/utils.py index c4d8dd1..0dab484 100644 --- a/gcode/utils.py +++ b/gcode/utils.py @@ -6,7 +6,7 @@ import math import numpy as np -from typing import Dict, List, Tuple, Optional +from typing import Dict, List def feed_rate_to_duration(distance: float, feed_rate: float) -> float: diff --git a/parol6/__init__.py b/parol6/__init__.py new file mode 100644 index 0000000..6f3070e --- /dev/null +++ b/parol6/__init__.py @@ -0,0 +1,25 @@ +""" +PAROL6 Python Package + +A unified library for controlling PAROL6 robot arms with async-first UDP client, +optional sync wrapper, and server management capabilities. + +Key components: +- AsyncRobotClient: Async UDP client for robot operations +- RobotClient: Sync wrapper with automatic event loop handling +- ServerManager: Manages headless controller process lifecycle +- ensure_server: Convenience function to auto-start controller when needed +""" + +from ._version import __version__ +from .client.async_client import AsyncRobotClient +from .client.sync_client import RobotClient +from .server.manager import ServerManager, ensure_server + +__all__ = [ + "__version__", + "AsyncRobotClient", + "RobotClient", + "ServerManager", + "ensure_server" +] diff --git a/parol6/_version.py b/parol6/_version.py new file mode 100644 index 0000000..8545f72 --- /dev/null +++ b/parol6/_version.py @@ -0,0 +1,3 @@ +"""Version information for parol6 package.""" + +__version__ = "0.1.0" diff --git a/parol6/cli/__init__.py b/parol6/cli/__init__.py new file mode 100644 index 0000000..e16bac8 --- /dev/null +++ b/parol6/cli/__init__.py @@ -0,0 +1 @@ +"""Command-line interface modules.""" diff --git a/parol6/cli/server.py b/parol6/cli/server.py new file mode 100644 index 0000000..6acede8 --- /dev/null +++ b/parol6/cli/server.py @@ -0,0 +1,16 @@ +""" +CLI entry point for parol6-server command. + +This module provides the command-line interface for starting the PAROL6 headless controller. +""" + +from parol6.server.headless_commander import main + + +def main_entry(): + """Entry point for the parol6-server command.""" + main() + + +if __name__ == "__main__": + main_entry() diff --git a/parol6/client/__init__.py b/parol6/client/__init__.py new file mode 100644 index 0000000..eb1a788 --- /dev/null +++ b/parol6/client/__init__.py @@ -0,0 +1 @@ +"""Client modules for robot communication.""" diff --git a/parol6/client/async_client.py b/parol6/client/async_client.py new file mode 100644 index 0000000..fd895a5 --- /dev/null +++ b/parol6/client/async_client.py @@ -0,0 +1,1123 @@ +""" +Async UDP client for PAROL6 robot control. + +Provides an async-first interface for all robot operations with optional acknowledgment tracking. +""" + +import json +import os +import socket +import time +import math +from typing import Union, List, Optional, Literal, Dict + +from ..protocol.types import Frame, Axis, IOStatus, GripperStatus, StatusAggregate, TrackingStatus +from ..utils.tracking import send_robot_command_tracked, send_and_wait + + +class AsyncRobotClient: + """ + Async UDP client for the PAROL6 headless controller. + + This client provides async methods for all robot operations: + - Motion commands (home, stop, move_joints, move_pose, move_cartesian, jog) + - Status queries (get_angles, get_io, get_gripper_status, get_status) + - Control commands (enable, disable, clear_error, set_com_port, stream on/off) + - GCODE operations + + All methods support optional acknowledgment tracking for reliable operation. + """ + + def __init__( + self, + host: str = "127.0.0.1", + port: int = 5001, + timeout: float = 2.0, + retries: int = 1, + ack_port: int = 5002 + ) -> None: + self.host = host + self.port = port + self.timeout = timeout + self.retries = retries + self.ack_port = ack_port + + # --------------- Internal helpers --------------- + + async def _send(self, message: str) -> str: + """Fire-and-forget UDP send.""" + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: + sock.sendto(message.encode("utf-8"), (self.host, self.port)) + return f"Sent: {message}" + + async def _request(self, message: str, bufsize: int = 2048) -> str | None: + """Send a request and wait for a UDP response (with retry).""" + for _ in range(self.retries + 1): + try: + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: + sock.settimeout(self.timeout) + sock.sendto(message.encode("utf-8"), (self.host, self.port)) + data, _ = sock.recvfrom(bufsize) + return data.decode("utf-8") + except TimeoutError: + continue + except Exception: + break + return None + + async def _send_tracked(self, message: str, wait_for_ack: bool = False, timeout: float = 2.0, non_blocking: bool = False) -> Union[str, dict]: + """Send with optional tracking support.""" + # Check if tracking is explicitly disabled + disable_tracking = os.getenv("PAROL6_DISABLE_TRACKING", "").lower() in ("1", "true", "yes", "on") + + # Only use tracking if wait_for_ack or non_blocking is requested AND tracking is not disabled + if (wait_for_ack or non_blocking) and not disable_tracking: + result = send_and_wait(message, timeout, non_blocking) + return result if result is not None else {"status": "ERROR", "details": "Send failed"} + elif wait_for_ack and disable_tracking: + # If ACK was requested but tracking is disabled, return a NO_TRACKING response + await self._send(message) + return {"status": "NO_TRACKING", "details": "Tracking disabled by environment", "completed": True, "command_id": None} + else: + # Fire-and-forget send without initializing tracker + return await self._send(message) + + # --------------- Motion / Control --------------- + + async def ping(self) -> bool: + """True if the controller responds with a 'PONG' message.""" + resp = await self._request("PING", bufsize=256) + return bool(resp and resp.strip().upper().startswith("PONG")) + + async def home(self, wait_for_ack: bool = False, timeout: float = 30.0, non_blocking: bool = False) -> Union[str, dict]: + """Send HOME command.""" + return await self._send_tracked("HOME", wait_for_ack, timeout, non_blocking) + + async def stop(self, wait_for_ack: bool = False, timeout: float = 2.0, non_blocking: bool = False) -> Union[str, dict]: + """Send STOP command.""" + return await self._send_tracked("STOP", wait_for_ack, timeout, non_blocking) + + async def enable(self, wait_for_ack: bool = False, timeout: float = 2.0, non_blocking: bool = False) -> Union[str, dict]: + """Requires server support for ENABLE command.""" + return await self._send_tracked("ENABLE", wait_for_ack, timeout, non_blocking) + + async def disable(self, wait_for_ack: bool = False, timeout: float = 2.0, non_blocking: bool = False) -> Union[str, dict]: + """Requires server support for DISABLE command.""" + return await self._send_tracked("DISABLE", wait_for_ack, timeout, non_blocking) + + async def clear_error(self, wait_for_ack: bool = False, timeout: float = 2.0, non_blocking: bool = False) -> Union[str, dict]: + """Requires server support for CLEAR_ERROR command.""" + return await self._send_tracked("CLEAR_ERROR", wait_for_ack, timeout, non_blocking) + + async def stream_on(self, wait_for_ack: bool = False, timeout: float = 2.0, non_blocking: bool = False) -> Union[str, dict]: + """Enable zero-queue streaming mode on the server.""" + return await self._send_tracked("STREAM|ON", wait_for_ack, timeout, non_blocking) + + async def stream_off(self, wait_for_ack: bool = False, timeout: float = 2.0, non_blocking: bool = False) -> Union[str, dict]: + """Disable zero-queue streaming mode on the server.""" + return await self._send_tracked("STREAM|OFF", wait_for_ack, timeout, non_blocking) + + async def set_com_port(self, port_str: str, wait_for_ack: bool = False, timeout: float = 2.0, non_blocking: bool = False) -> Union[str, dict]: + """ + Best-effort COM port change. Requires server support to take effect immediately. + """ + if not port_str: + return "No port provided" + return await self._send_tracked(f"SET_PORT|{port_str}", wait_for_ack, timeout, non_blocking) + + # --------------- Status / Queries --------------- + + async def get_angles(self) -> list[float] | None: + """ + Returns list of 6 angles in degrees or None on failure. + Expected wire format: "ANGLES|j1,j2,j3,j4,j5,j6" + """ + resp = await self._request("GET_ANGLES", bufsize=1024) + if not resp: + return None + parts = resp.split("|") + if len(parts) != 2 or parts[0] != "ANGLES": + return None + return [float(v) for v in parts[1].split(",")] + + async def get_io(self) -> list[int] | None: + """ + Returns [IN1, IN2, OUT1, OUT2, ESTOP] or None on failure. + Expected wire format: "IO|in1,in2,out1,out2,estop" + """ + resp = await self._request("GET_IO", bufsize=1024) + if not resp: + return None + parts = resp.split("|") + if len(parts) != 2 or parts[0] != "IO": + return None + return [int(v) for v in parts[1].split(",")] + + async def get_gripper_status(self) -> list[int] | None: + """ + Returns [ID, Position, Speed, Current, StatusByte, ObjectDetected] or None. + Expected wire format: "GRIPPER|id,pos,spd,cur,status,obj" + """ + resp = await self._request("GET_GRIPPER", bufsize=1024) + if not resp: + return None + parts = resp.split("|") + if len(parts) != 2 or parts[0] != "GRIPPER": + return None + return [int(v) for v in parts[1].split(",")] + + async def get_speeds(self) -> list[float] | None: + """ + Returns list of 6 joint speeds in steps/sec or None on failure. + Expected wire format: "SPEEDS|s1,s2,s3,s4,s5,s6" + """ + resp = await self._request("GET_SPEEDS", bufsize=1024) + if not resp: + return None + parts = resp.split("|") + if len(parts) != 2 or parts[0] != "SPEEDS": + return None + return [float(v) for v in parts[1].split(",")] + + async def get_pose(self) -> list[float] | None: + """ + Returns 16-element transformation matrix (flattened) or None on failure. + Expected wire format: "POSE|p0,p1,p2,...,p15" + """ + resp = await self._request("GET_POSE", bufsize=2048) + if not resp: + return None + parts = resp.split("|") + if len(parts) != 2 or parts[0] != "POSE": + return None + return [float(v) for v in parts[1].split(",")] + + async def get_gripper(self) -> list[int] | None: + """Alias for get_gripper_status for compatibility.""" + return await self.get_gripper_status() + + async def get_status(self) -> dict | None: + """ + Aggregate status if supported by controller. + Expected format: + STATUS|POSE=p0,p1,...,p15|ANGLES=a0,...,a5|IO=in1,in2,out1,out2,estop|GRIPPER=id,pos,spd,cur,status,obj + Returns dict with keys: pose (list[float] len=16), angles (list[float] len=6), + io (list[int] len=5), gripper (list[int] len>=6) + """ + resp = await self._request("GET_STATUS", bufsize=4096) + if not resp or not resp.startswith("STATUS|"): + return None + # Split top-level sections after "STATUS|" + sections = resp.split("|")[1:] + result: dict[str, object] = { + "pose": None, + "angles": None, + "io": None, + "gripper": None, + } + for sec in sections: + if sec.startswith("POSE="): + vals = [float(x) for x in sec[len("POSE=") :].split(",") if x] + result["pose"] = vals + elif sec.startswith("ANGLES="): + vals = [float(x) for x in sec[len("ANGLES=") :].split(",") if x] + result["angles"] = vals + elif sec.startswith("IO="): + vals = [int(x) for x in sec[len("IO=") :].split(",") if x] + result["io"] = vals + elif sec.startswith("GRIPPER="): + vals = [int(x) for x in sec[len("GRIPPER=") :].split(",") if x] + result["gripper"] = vals + return result + + # --------------- Helper methods for compatibility --------------- + + async def get_pose_rpy(self) -> list[float] | None: + """ + Get robot pose as [x, y, z, rx, ry, rz] in mm and degrees. + Converts 4x4 matrix to xyz + RPY Euler angles. + """ + pose_matrix = await self.get_pose() + if not pose_matrix or len(pose_matrix) != 16: + return None + + try: + # Extract translation (convert to mm if needed - assume matrix is in mm) + x, y, z = pose_matrix[3], pose_matrix[7], pose_matrix[11] + + # Extract rotation matrix elements + r11, r12, r13 = pose_matrix[0], pose_matrix[1], pose_matrix[2] + r21, r22, r23 = pose_matrix[4], pose_matrix[5], pose_matrix[6] + r31, r32, r33 = pose_matrix[8], pose_matrix[9], pose_matrix[10] + + # Convert to RPY (XYZ convention) in degrees + # Handle gimbal lock cases + sy = math.sqrt(r11*r11 + r21*r21) + + if sy > 1e-6: # Not at gimbal lock + rx = math.atan2(r32, r33) + ry = math.atan2(-r31, sy) + rz = math.atan2(r21, r11) + else: # Gimbal lock case + rx = math.atan2(-r23, r22) + ry = math.atan2(-r31, sy) + rz = 0 + + # Convert to degrees + rx_deg = math.degrees(rx) + ry_deg = math.degrees(ry) + rz_deg = math.degrees(rz) + + return [x, y, z, rx_deg, ry_deg, rz_deg] + + except (ValueError, IndexError): + return None + + async def get_pose_xyz(self) -> list[float] | None: + """Get robot position as [x, y, z] in mm.""" + pose_rpy = await self.get_pose_rpy() + return pose_rpy[:3] if pose_rpy else None + + async def is_estop_pressed(self) -> bool: + """Check if E-stop is pressed. Returns True if pressed.""" + io_status = await self.get_io() + if io_status and len(io_status) >= 5: + return io_status[4] == 0 # E-stop at index 4, 0 means pressed + return False + + async def is_robot_stopped(self, threshold_speed: float = 2.0) -> bool: + """ + Check if robot has stopped moving. + + Args: + threshold_speed: Speed threshold in steps/sec + + Returns: + True if all joints below threshold + """ + speeds = await self.get_speeds() + if not speeds: + return False + + max_speed = max(abs(s) for s in speeds) + return max_speed < threshold_speed + + async def wait_until_stopped( + self, + timeout: float = 90.0, + settle_window: float = 1.0, + poll_interval: float = 0.2, + speed_threshold: float = 2.0, + angle_threshold: float = 0.5, + show_progress: bool = False + ) -> bool: + """ + Wait for robot to stop moving with responsive polling. + + Args: + timeout: Maximum time to wait in seconds + settle_window: How long robot must be stable to be considered stopped + poll_interval: How often to check status + speed_threshold: Max joint speed to be considered stopped (steps/sec) + angle_threshold: Max angle change to be considered stopped (degrees) + show_progress: Print dots to show progress + + Returns: + True if robot stopped, False if timeout + """ + import asyncio + + start_time = time.time() + last_angles = None + settle_start = None + last_progress = 0 + + if show_progress: + print("Waiting for robot to stop...", end="", flush=True) + + while time.time() - start_time < timeout: + # Try speed-based detection first (preferred) + speeds = await self.get_speeds() + if speeds: + max_speed = max(abs(s) for s in speeds) + if max_speed < speed_threshold: + if settle_start is None: + settle_start = time.time() + elif time.time() - settle_start > settle_window: + if show_progress: + print(" stopped via speeds!") + return True + else: + settle_start = None + else: + # Fallback to angle-based detection + angles = await self.get_angles() + if angles: + if last_angles is not None: + max_change = max(abs(a - b) for a, b in zip(angles, last_angles)) + if max_change < angle_threshold: + if settle_start is None: + settle_start = time.time() + elif time.time() - settle_start > settle_window: + if show_progress: + print(" stopped via angle delta!") + return True + else: + settle_start = None + last_angles = angles + + # Show progress dots every few seconds + if show_progress and int(time.time() - start_time) > last_progress: + print(".", end="", flush=True) + last_progress = int(time.time() - start_time) + + await asyncio.sleep(poll_interval) + + if show_progress: + print(" timeout!") + return False + + # --------------- Extended controls / motion --------------- + + async def move_joints( + self, + joint_angles: list[float], + duration: float | None = None, + speed_percentage: int | None = None, + accel_percentage: int | None = None, # kept for API compatibility; not sent + profile: str | None = None, # kept for API compatibility; not sent + tracking: str | None = None, # kept for API compatibility; not sent + wait_for_ack: bool = False, + timeout: float = 2.0, + non_blocking: bool = False + ) -> Union[str, dict]: + """ + Send minimal MOVEJOINT wire format expected by the server: + MOVEJOINT|j1|j2|j3|j4|j5|j6|DUR|SPD + Use "NONE" for omitted duration/speed. + """ + if duration is None and speed_percentage is None: + error = "Error: You must provide either a duration or a speed_percentage." + return {'status': 'INVALID', 'details': error} + + angles_str = "|".join(str(a) for a in joint_angles) + dur_str = "NONE" if duration is None else str(duration) + spd_str = "NONE" if speed_percentage is None else str(speed_percentage) + message = f"MOVEJOINT|{angles_str}|{dur_str}|{spd_str}" + + return await self._send_tracked(message, wait_for_ack, timeout, non_blocking) + + async def move_pose( + self, + pose: list[float], + duration: float | None = None, + speed_percentage: int | None = None, + accel_percentage: int | None = None, # kept; not sent + profile: str | None = None, # kept; not sent + tracking: str | None = None, # kept; not sent + wait_for_ack: bool = False, + timeout: float = 2.0, + non_blocking: bool = False + ) -> Union[str, dict]: + """ + Send minimal MOVEPOSE wire format expected by the server: + MOVEPOSE|x|y|z|rx|ry|rz|DUR|SPD + Use "NONE" for omitted duration/speed. + """ + if duration is None and speed_percentage is None: + error = "Error: You must provide either a duration or a speed_percentage." + return {'status': 'INVALID', 'details': error} + + pose_str = "|".join(str(v) for v in pose) + dur_str = "NONE" if duration is None else str(duration) + spd_str = "NONE" if speed_percentage is None else str(speed_percentage) + message = f"MOVEPOSE|{pose_str}|{dur_str}|{spd_str}" + + return await self._send_tracked(message, wait_for_ack, timeout, non_blocking) + + async def move_cartesian( + self, + pose: list[float], + duration: float | None = None, + speed_percentage: float | None = None, + accel_percentage: int | None = None, # kept; not sent + profile: str | None = None, # kept; not sent + tracking: str | None = None, # kept; not sent + wait_for_ack: bool = False, + timeout: float = 2.0, + non_blocking: bool = False + ) -> Union[str, dict]: + """ + Send minimal MOVECART wire format expected by the server: + MOVECART|x|y|z|rx|ry|rz|DUR|SPD + Use "NONE" for omitted duration/speed. + """ + if duration is None and speed_percentage is None: + error = "Error: You must provide either a duration or a speed_percentage." + return {'status': 'INVALID', 'details': error} + + pose_str = "|".join(str(v) for v in pose) + dur_str = "NONE" if duration is None else str(duration) + spd_str = "NONE" if speed_percentage is None else str(speed_percentage) + message = f"MOVECART|{pose_str}|{dur_str}|{spd_str}" + + return await self._send_tracked(message, wait_for_ack, timeout, non_blocking) + + async def move_cartesian_rel_trf( + self, + deltas: list[float], # [dx, dy, dz, rx, ry, rz] in mm/deg relative to TRF + duration: float | None = None, + speed_percentage: float | None = None, + accel_percentage: int | None = None, + profile: str | None = None, + tracking: str | None = None, + wait_for_ack: bool = False, + timeout: float = 2.0, + non_blocking: bool = False + ) -> Union[str, dict]: + """ + Send a MOVECARTRELTRF (relative straight-line in TRF) command. + Provide either duration or speed_percentage (1..100). + Optional: accel_percentage, trajectory profile, and tracking mode. + """ + delta_str = "|".join(str(v) for v in deltas) + dur_str = "NONE" if duration is None else str(duration) + spd_str = "NONE" if speed_percentage is None else str(speed_percentage) + acc_str = "NONE" if accel_percentage is None else str(int(accel_percentage)) + prof_str = (profile or "NONE").upper() + track_str = (tracking or "NONE").upper() + message = f"MOVECARTRELTRF|{delta_str}|{dur_str}|{spd_str}|{acc_str}|{prof_str}|{track_str}" + + return await self._send_tracked(message, wait_for_ack, timeout, non_blocking) + + async def jog_joint( + self, + joint_index: int, + speed_percentage: int, + duration: float | None = None, + distance_deg: float | None = None, + wait_for_ack: bool = False, + timeout: float = 2.0, + non_blocking: bool = False + ) -> Union[str, dict]: + """ + Send a JOG command for a single joint (0..5 positive, 6..11 negative for reverse). + duration and distance_deg are optional; at least one should be provided for one-shot jog. + For press-and-hold UI, send short duration repeatedly. + """ + if duration is None and distance_deg is None: + error = "Error: You must provide either a duration or a distance_deg." + return {'status': 'INVALID', 'details': error} + + dur_str = "NONE" if duration is None else str(duration) + dist_str = "NONE" if distance_deg is None else str(distance_deg) + message = f"JOG|{joint_index}|{speed_percentage}|{dur_str}|{dist_str}" + + return await self._send_tracked(message, wait_for_ack, timeout, non_blocking) + + async def jog_cartesian( + self, + frame: Frame, + axis: Axis, + speed_percentage: int, + duration: float, + wait_for_ack: bool = False, + timeout: float = 2.0, + non_blocking: bool = False + ) -> Union[str, dict]: + """ + Send a CARTJOG command (frame 'TRF' or 'WRF', axis in {X+/X-/Y+/.../RZ-}). + """ + message = f"CARTJOG|{frame}|{axis}|{speed_percentage}|{duration}" + return await self._send_tracked(message, wait_for_ack, timeout, non_blocking) + + async def jog_multiple( + self, + joints: list[int], + speeds: list[float], + duration: float, + wait_for_ack: bool = False, + timeout: float = 2.0, + non_blocking: bool = False + ) -> Union[str, dict]: + """ + Send a MULTIJOG command to jog multiple joints simultaneously for 'duration' seconds. + """ + if len(joints) != len(speeds): + error = "Error: The number of joints must match the number of speeds." + return {'status': 'INVALID', 'details': error} + + joints_str = ",".join(str(j) for j in joints) + speeds_str = ",".join(str(s) for s in speeds) + message = f"MULTIJOG|{joints_str}|{speeds_str}|{duration}" + + return await self._send_tracked(message, wait_for_ack, timeout, non_blocking) + + # --------------- IO / Gripper --------------- + + async def control_pneumatic_gripper( + self, + action: str, + port: int, + wait_for_ack: bool = False, + timeout: float = 2.0, + non_blocking: bool = False + ) -> Union[str, dict]: + """ + Control pneumatic gripper via digital outputs. + action: 'open' or 'close' + port: 1 or 2 + """ + action = action.lower() + if action not in ("open", "close"): + return {'status': 'INVALID', 'details': 'Invalid pneumatic action'} + if port not in (1, 2): + return {'status': 'INVALID', 'details': 'Invalid pneumatic port'} + + message = f"PNEUMATICGRIPPER|{action}|{port}" + return await self._send_tracked(message, wait_for_ack, timeout, non_blocking) + + async def control_electric_gripper( + self, + action: str, + position: int | None = 255, + speed: int | None = 150, + current: int | None = 500, + wait_for_ack: bool = False, + timeout: float = 2.0, + non_blocking: bool = False + ) -> Union[str, dict]: + """ + Control electric gripper. + action: 'move' or 'calibrate' + position: 0..255 + speed: 0..255 + current: 100..1000 (mA) + """ + action = action.lower() + if action not in ("move", "calibrate"): + return {'status': 'INVALID', 'details': 'Invalid electric gripper action'} + pos = 0 if position is None else int(position) + spd = 0 if speed is None else int(speed) + cur = 100 if current is None else int(current) + + message = f"ELECTRICGRIPPER|{action}|{pos}|{spd}|{cur}" + return await self._send_tracked(message, wait_for_ack, timeout, non_blocking) + + # --------------- GCODE operations --------------- + + async def execute_gcode( + self, + gcode_line: str, + wait_for_ack: bool = False, + timeout: float = 5.0, + non_blocking: bool = False + ) -> Union[str, dict]: + """ + Execute a single GCODE line. + """ + message = f"GCODE|{gcode_line}" + return await self._send_tracked(message, wait_for_ack, timeout, non_blocking) + + async def execute_gcode_program( + self, + program_lines: list[str], + wait_for_ack: bool = False, + timeout: float = 30.0, + non_blocking: bool = False + ) -> Union[str, dict]: + """ + Execute a GCODE program from a list of lines. + """ + # Validate program lines don't contain problematic characters + for i, line in enumerate(program_lines): + if '|' in line: + error_msg = f"Line {i+1} contains pipe character '|' which is not allowed" + return {'status': 'INVALID', 'details': error_msg} + + # Join lines with semicolons for inline program + program_str = ';'.join(program_lines) + message = f"GCODE_PROGRAM|INLINE|{program_str}" + + return await self._send_tracked(message, wait_for_ack, timeout, non_blocking) + + async def load_gcode_file( + self, + filepath: str, + wait_for_ack: bool = False, + timeout: float = 10.0, + non_blocking: bool = False + ) -> Union[str, dict]: + """ + Load and execute a GCODE program from a file. + """ + message = f"GCODE_PROGRAM|FILE|{filepath}" + return await self._send_tracked(message, wait_for_ack, timeout, non_blocking) + + async def get_gcode_status(self) -> dict | None: + """ + Get the current status of the GCODE interpreter. + """ + resp = await self._request("GET_GCODE_STATUS", bufsize=2048) + if not resp or not resp.startswith("GCODE_STATUS|"): + return None + + status_json = resp.split('|', 1)[1] + try: + return json.loads(status_json) + except json.JSONDecodeError: + return None + + async def pause_gcode_program( + self, + wait_for_ack: bool = False, + timeout: float = 5.0, + non_blocking: bool = False + ) -> Union[str, dict]: + """Pause the currently running GCODE program.""" + return await self._send_tracked("GCODE_PAUSE", wait_for_ack, timeout, non_blocking) + + async def resume_gcode_program( + self, + wait_for_ack: bool = False, + timeout: float = 5.0, + non_blocking: bool = False + ) -> Union[str, dict]: + """Resume a paused GCODE program.""" + return await self._send_tracked("GCODE_RESUME", wait_for_ack, timeout, non_blocking) + + async def stop_gcode_program( + self, + wait_for_ack: bool = False, + timeout: float = 5.0, + non_blocking: bool = False + ) -> Union[str, dict]: + """Stop the currently running GCODE program.""" + return await self._send_tracked("GCODE_STOP", wait_for_ack, timeout, non_blocking) + + # --------------- Smooth motion commands --------------- + + async def smooth_circle( + self, + center: List[float], + radius: float, + plane: Literal['XY', 'XZ', 'YZ'] = 'XY', + frame: Literal['WRF', 'TRF'] = 'WRF', + center_mode: Literal['ABSOLUTE', 'TOOL', 'RELATIVE'] = 'ABSOLUTE', + entry_mode: Literal['AUTO', 'TANGENT', 'DIRECT', 'NONE'] = 'NONE', + start_pose: Optional[List[float]] = None, + duration: Optional[float] = None, + speed_percentage: Optional[float] = None, + clockwise: bool = False, + trajectory_type: Literal['cubic', 'quintic', 's_curve'] = 'cubic', + jerk_limit: Optional[float] = None, + wait_for_ack: bool = False, + timeout: float = 10.0, + non_blocking: bool = False + ) -> Union[str, dict]: + """ + Execute a smooth circular motion. + + Args: + center: [x, y, z] center point in mm + radius: Circle radius in mm + plane: Plane of the circle ('XY', 'XZ', or 'YZ') + frame: Reference frame ('WRF' for World, 'TRF' for Tool) + center_mode: How to interpret center point + entry_mode: How to approach circle if not on perimeter + start_pose: Optional [x, y, z, rx, ry, rz] start pose + duration: Time to complete the circle in seconds + speed_percentage: Speed as percentage (1-100) + clockwise: Direction of motion + trajectory_type: Type of trajectory + jerk_limit: Optional jerk limit for s_curve trajectory + wait_for_ack: Enable command tracking + timeout: Timeout for acknowledgment + non_blocking: Return immediately with command ID + """ + if duration is None and speed_percentage is None: + error = "Error: You must provide either duration or speed_percentage." + return {'status': 'INVALID', 'details': error} + + center_str = ",".join(map(str, center)) + start_str = ",".join(map(str, start_pose)) if start_pose else "CURRENT" + clockwise_str = "1" if clockwise else "0" + + # Format timing + if duration is not None: + timing_str = f"DURATION|{duration}" + else: + timing_str = f"SPEED|{speed_percentage}" + + # Add trajectory type and new parameters + traj_params = f"|{trajectory_type}" + if trajectory_type == 's_curve' and jerk_limit is not None: + traj_params += f"|{jerk_limit}" + elif trajectory_type != 'cubic': + traj_params += "|DEFAULT" # Use default jerk limit for s_curve + + # Add center_mode and entry_mode parameters + mode_params = f"|{center_mode}|{entry_mode}" + + command = f"SMOOTH_CIRCLE|{center_str}|{radius}|{plane}|{frame}|{start_str}|{timing_str}|{clockwise_str}{traj_params}{mode_params}" + + return await self._send_tracked(command, wait_for_ack, timeout, non_blocking) + + async def smooth_arc_center( + self, + end_pose: List[float], + center: List[float], + frame: Literal['WRF', 'TRF'] = 'WRF', + start_pose: Optional[List[float]] = None, + duration: Optional[float] = None, + speed_percentage: Optional[float] = None, + clockwise: bool = False, + trajectory_type: Literal['cubic', 'quintic', 's_curve'] = 'cubic', + jerk_limit: Optional[float] = None, + wait_for_ack: bool = False, + timeout: float = 10.0, + non_blocking: bool = False + ) -> Union[str, dict]: + """ + Execute a smooth arc motion defined by center point. + + Args: + end_pose: [x, y, z, rx, ry, rz] end pose (mm and degrees) + center: [x, y, z] arc center point in mm + frame: Reference frame ('WRF' for World, 'TRF' for Tool) + start_pose: Optional [x, y, z, rx, ry, rz] start pose + duration: Time to complete the arc in seconds + speed_percentage: Speed as percentage (1-100) + clockwise: Direction of motion + trajectory_type: Type of trajectory + jerk_limit: Optional jerk limit for s_curve trajectory + wait_for_ack: Enable command tracking + timeout: Timeout for acknowledgment + non_blocking: Return immediately with command ID + """ + if duration is None and speed_percentage is None: + error = "Error: You must provide either duration or speed_percentage." + return {'status': 'INVALID', 'details': error} + + end_str = ",".join(map(str, end_pose)) + center_str = ",".join(map(str, center)) + start_str = ",".join(map(str, start_pose)) if start_pose else "CURRENT" + clockwise_str = "1" if clockwise else "0" + + # Format timing + if duration is not None: + timing_str = f"DURATION|{duration}" + else: + timing_str = f"SPEED|{speed_percentage}" + + # Add trajectory type if not default + traj_params = f"|{trajectory_type}" + if trajectory_type == 's_curve' and jerk_limit is not None: + traj_params += f"|{jerk_limit}" + elif trajectory_type != 'cubic': + traj_params += "|DEFAULT" + + command = f"SMOOTH_ARC_CENTER|{end_str}|{center_str}|{frame}|{start_str}|{timing_str}|{clockwise_str}{traj_params}" + + return await self._send_tracked(command, wait_for_ack, timeout, non_blocking) + + async def smooth_spline( + self, + waypoints: List[List[float]], + frame: Literal['WRF', 'TRF'] = 'WRF', + start_pose: Optional[List[float]] = None, + duration: Optional[float] = None, + speed_percentage: Optional[float] = None, + trajectory_type: Literal['cubic', 'quintic', 's_curve'] = 'cubic', + jerk_limit: Optional[float] = None, + wait_for_ack: bool = False, + timeout: float = 10.0, + non_blocking: bool = False + ) -> Union[str, dict]: + """ + Execute a smooth spline motion through waypoints. + + Args: + waypoints: List of [x, y, z, rx, ry, rz] poses (mm and degrees) + frame: Reference frame ('WRF' for World, 'TRF' for Tool) + start_pose: Optional [x, y, z, rx, ry, rz] start pose + duration: Total time for the motion in seconds + speed_percentage: Speed as percentage (1-100) + trajectory_type: Type of trajectory + jerk_limit: Optional jerk limit for s_curve trajectory + wait_for_ack: Enable command tracking + timeout: Timeout for acknowledgment + non_blocking: Return immediately with command ID + """ + if duration is None and speed_percentage is None: + error = "Error: You must provide either duration or speed_percentage." + return {'status': 'INVALID', 'details': error} + + num_waypoints = len(waypoints) + start_str = ",".join(map(str, start_pose)) if start_pose else "CURRENT" + + # Format timing + if duration is not None: + timing_str = f"DURATION|{duration}" + else: + timing_str = f"SPEED|{speed_percentage}" + + # Format waypoints - flatten each waypoint's 6 values + waypoint_strs = [] + for wp in waypoints: + waypoint_strs.extend(map(str, wp)) + + # Build command with trajectory type + command_parts = [f"SMOOTH_SPLINE", str(num_waypoints), frame, start_str, timing_str] + + # Add trajectory type + command_parts.append(trajectory_type) + if trajectory_type == 's_curve' and jerk_limit is not None: + command_parts.append(str(jerk_limit)) + elif trajectory_type == 's_curve': + command_parts.append("DEFAULT") + + # Add waypoints + command_parts.extend(waypoint_strs) + command = "|".join(command_parts) + + return await self._send_tracked(command, wait_for_ack, timeout, non_blocking) + + async def smooth_helix( + self, + center: List[float], + radius: float, + pitch: float, + height: float, + frame: Literal['WRF', 'TRF'] = 'WRF', + trajectory_type: Literal['cubic', 'quintic', 's_curve'] = 'cubic', + jerk_limit: Optional[float] = None, + start_pose: Optional[List[float]] = None, + duration: Optional[float] = None, + speed_percentage: Optional[float] = None, + clockwise: bool = False, + wait_for_ack: bool = False, + timeout: float = 10.0, + non_blocking: bool = False + ) -> Union[str, dict]: + """ + Execute a smooth helical motion. + + Args: + center: [x, y, z] helix center point in mm + radius: Helix radius in mm + pitch: Vertical distance per revolution in mm + height: Total height of helix in mm + frame: Reference frame ('WRF' for World, 'TRF' for Tool) + trajectory_type: Type of trajectory + jerk_limit: Optional jerk limit for s_curve trajectory + start_pose: Optional [x, y, z, rx, ry, rz] start pose + duration: Time to complete the helix in seconds + speed_percentage: Speed as percentage (1-100) + clockwise: Direction of motion + wait_for_ack: Enable command tracking + timeout: Timeout for acknowledgment + non_blocking: Return immediately with command ID + """ + if duration is None and speed_percentage is None: + error = "Error: You must provide either duration or speed_percentage." + return {'status': 'INVALID', 'details': error} + + center_str = ",".join(map(str, center)) + start_str = ",".join(map(str, start_pose)) if start_pose else "CURRENT" + clockwise_str = "1" if clockwise else "0" + + # Format timing + if duration is not None: + timing_str = f"DURATION|{duration}" + else: + timing_str = f"SPEED|{speed_percentage}" + + # Add trajectory type parameters + traj_params = f"|{trajectory_type}" + if trajectory_type == 's_curve' and jerk_limit is not None: + traj_params += f"|{jerk_limit}" + elif trajectory_type != 'cubic': + traj_params += "|DEFAULT" # Use default jerk limit for s_curve + + command = f"SMOOTH_HELIX|{center_str}|{radius}|{pitch}|{height}|{frame}|{start_str}|{timing_str}|{clockwise_str}{traj_params}" + + return await self._send_tracked(command, wait_for_ack, timeout, non_blocking) + + async def smooth_blend( + self, + segments: List[Dict], + blend_time: float = 0.5, + frame: Literal['WRF', 'TRF'] = 'WRF', + start_pose: Optional[List[float]] = None, + duration: Optional[float] = None, + speed_percentage: Optional[float] = None, + wait_for_ack: bool = False, + timeout: float = 15.0, + non_blocking: bool = False + ) -> Union[str, dict]: + """ + Execute a blended motion through multiple segments. + + Args: + segments: List of segment dictionaries + blend_time: Time to blend between segments in seconds + frame: Reference frame ('WRF' for World, 'TRF' for Tool) + start_pose: Optional [x, y, z, rx, ry, rz] start pose + duration: Total time for entire motion + speed_percentage: Speed as percentage (1-100) for entire motion + wait_for_ack: Enable command tracking + timeout: Timeout for acknowledgment + non_blocking: Return immediately with command ID + """ + num_segments = len(segments) + start_str = ",".join(map(str, start_pose)) if start_pose else "CURRENT" + + # Format timing + if duration is None and speed_percentage is None: + # Use individual segment durations + timing_str = "DEFAULT" + elif duration is not None: + timing_str = f"DURATION|{duration}" + else: + timing_str = f"SPEED|{speed_percentage}" + + # Format segments + segment_strs = [] + for seg in segments: + seg_type = seg['type'] + + if seg_type == 'LINE': + end_str = ",".join(map(str, seg['end'])) + seg_str = f"LINE|{end_str}|{seg.get('duration', 2.0)}" + + elif seg_type == 'CIRCLE': + center_str = ",".join(map(str, seg['center'])) + clockwise_str = "1" if seg.get('clockwise', False) else "0" + seg_str = f"CIRCLE|{center_str}|{seg['radius']}|{seg['plane']}|{seg.get('duration', 3.0)}|{clockwise_str}" + + elif seg_type == 'ARC': + end_str = ",".join(map(str, seg['end'])) + center_str = ",".join(map(str, seg['center'])) + clockwise_str = "1" if seg.get('clockwise', False) else "0" + seg_str = f"ARC|{end_str}|{center_str}|{seg.get('duration', 2.0)}|{clockwise_str}" + + elif seg_type == 'SPLINE': + waypoints_str = ";".join([",".join(map(str, wp)) for wp in seg['waypoints']]) + seg_str = f"SPLINE|{len(seg['waypoints'])}|{waypoints_str}|{seg.get('duration', 3.0)}" + + else: + continue + + segment_strs.append(seg_str) + + # Build command with || separators between segments + command = f"SMOOTH_BLEND|{num_segments}|{blend_time}|{frame}|{start_str}|{timing_str}|" + "||".join(segment_strs) + + return await self._send_tracked(command, wait_for_ack, timeout, non_blocking) + + # --------------- Work coordinate system helpers --------------- + + async def set_work_coordinate_offset( + self, + coordinate_system: str, + x: float | None = None, + y: float | None = None, + z: float | None = None, + wait_for_ack: bool = False, + timeout: float = 5.0, + non_blocking: bool = False + ) -> Union[str, dict]: + """ + Set work coordinate system offsets (G54-G59). + + Args: + coordinate_system: Work coordinate system to set ('G54' through 'G59') + x: X axis offset in mm (None to keep current) + y: Y axis offset in mm (None to keep current) + z: Z axis offset in mm (None to keep current) + wait_for_ack: If True, wait for confirmation + timeout: Maximum time to wait for acknowledgment + non_blocking: Return immediately with command ID + + Returns: + Success message, command ID, or dict with status details + + Example: + # Set G54 origin to current position + await client.set_work_coordinate_offset('G54', x=0, y=0, z=0) + + # Offset G55 by 100mm in X + await client.set_work_coordinate_offset('G55', x=100) + """ + # Validate coordinate system format + valid_systems = ['G54', 'G55', 'G56', 'G57', 'G58', 'G59'] + if coordinate_system not in valid_systems: + error_msg = f'Invalid coordinate system: {coordinate_system}. Must be one of {valid_systems}' + if wait_for_ack: + return {'status': 'INVALID', 'details': error_msg} + else: + return error_msg + + # Map coordinate system to P number (P1=G54, P2=G55, etc.) + coord_num = int(coordinate_system[1:]) - 53 # G54=1, G55=2, etc. + + # Build offset parameters + offset_params = [] + if x is not None: + offset_params.append(f"X{x}") + if y is not None: + offset_params.append(f"Y{y}") + if z is not None: + offset_params.append(f"Z{z}") + + if not offset_params: + # Just select the coordinate system + return await self.execute_gcode(coordinate_system, wait_for_ack=wait_for_ack, timeout=timeout, non_blocking=non_blocking) + + # Send coordinate system selection first, then offset command + if wait_for_ack: + # Send both commands with tracking + select_result = await self.execute_gcode(coordinate_system, wait_for_ack=True, timeout=timeout) + if isinstance(select_result, dict) and select_result.get('status') not in ['COMPLETED', 'QUEUED', 'EXECUTING']: + return select_result + + offset_cmd = f"G10 L2 P{coord_num} {' '.join(offset_params)}" + return await self.execute_gcode(offset_cmd, wait_for_ack=True, timeout=timeout, non_blocking=non_blocking) + else: + # Fire and forget + await self.execute_gcode(coordinate_system) + offset_cmd = f"G10 L2 P{coord_num} {' '.join(offset_params)}" + return await self.execute_gcode(offset_cmd) + + async def zero_work_coordinates( + self, + coordinate_system: str = 'G54', + wait_for_ack: bool = False, + timeout: float = 5.0, + non_blocking: bool = False + ) -> Union[str, dict]: + """ + Set the current position as zero in the specified work coordinate system. + + Args: + coordinate_system: Work coordinate system to zero ('G54' through 'G59') + wait_for_ack: If True, wait for confirmation + timeout: Maximum time to wait for acknowledgment + non_blocking: Return immediately with command ID + + Returns: + Success message, command ID, or dict with status details + + Example: + # Set current position as origin in G54 + await client.zero_work_coordinates('G54') + """ + # This sets the current position as 0,0,0 in the work coordinate system + return await self.set_work_coordinate_offset( + coordinate_system, + x=0, y=0, z=0, + wait_for_ack=wait_for_ack, + timeout=timeout, + non_blocking=non_blocking + ) diff --git a/parol6/client/sync_client.py b/parol6/client/sync_client.py new file mode 100644 index 0000000..1c297dd --- /dev/null +++ b/parol6/client/sync_client.py @@ -0,0 +1,92 @@ +""" +Sync wrapper for AsyncRobotClient using telethon-style syncify. + +This module rewrites all public methods in the AsyncRobotClient +so they can run the loop on their own if it's not already running. This +rewrite allows for quick scripts while maintaining async performance when +used in async contexts. +""" + +import asyncio +import functools +import inspect +from typing import TYPE_CHECKING + +from .async_client import AsyncRobotClient + +if TYPE_CHECKING: + from ..protocol.types import Frame, Axis + + +def _get_running_loop(): + """Get the currently running event loop or create one.""" + try: + return asyncio.get_running_loop() + except RuntimeError: + return asyncio.new_event_loop() + + +def _syncify_wrap(cls, method_name): + """Wrap an async method to work in both sync and async contexts.""" + method = getattr(cls, method_name) + + @functools.wraps(method) + def syncified(self, *args, **kwargs): + coro = method(self, *args, **kwargs) + try: + loop = asyncio.get_running_loop() + # Loop is running, return the coroutine + return coro + except RuntimeError: + # No running loop, create one and run to completion + loop = asyncio.new_event_loop() + try: + return loop.run_until_complete(coro) + finally: + loop.close() + + # Save an accessible reference to the original method + setattr(syncified, '__async_method__', method) + setattr(cls, method_name, syncified) + + +def syncify(*classes): + """ + Convert all async methods in the given classes into synchronous methods + that return either the coroutine or the result based on whether + asyncio's event loop is running. + """ + for cls in classes: + for name in dir(cls): + if not name.startswith('_') or name == '__call__': + attr = getattr(cls, name) + if inspect.iscoroutinefunction(attr): + _syncify_wrap(cls, name) + + +class RobotClient(AsyncRobotClient): + """ + Synchronous robot client with automatic event loop handling. + + This class inherits from AsyncRobotClient and applies syncify + to all async methods. When called: + - If an event loop is running: returns the coroutine (async behavior) + - If no event loop is running: runs the coroutine and returns the result + + This allows the same client to work seamlessly in both sync and async contexts: + + # Sync usage (no event loop) + client = RobotClient() + angles = client.get_angles() # Automatically runs async method + + # Async usage (with event loop) + client = RobotClient() + angles = await client.get_angles() # Returns coroutine + """ + + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + + +# Apply syncify to RobotClient +syncify(RobotClient) diff --git a/parol6/protocol/__init__.py b/parol6/protocol/__init__.py new file mode 100644 index 0000000..d00b764 --- /dev/null +++ b/parol6/protocol/__init__.py @@ -0,0 +1 @@ +"""Protocol types and definitions.""" diff --git a/parol6/protocol/types.py b/parol6/protocol/types.py new file mode 100644 index 0000000..21b96b1 --- /dev/null +++ b/parol6/protocol/types.py @@ -0,0 +1,54 @@ +""" +Type definitions for PAROL6 protocol. + +Defines enums, TypedDicts, and dataclasses used across the public API. +""" + +from datetime import datetime +from typing import Literal, TypedDict + + +# Frame literals +Frame = Literal['WRF', 'TRF'] + +# Axis literals +Axis = Literal['X+', 'X-', 'Y+', 'Y-', 'Z+', 'Z-', 'RX+', 'RX-', 'RY+', 'RY-', 'RZ+', 'RZ-'] + +# Acknowledgment status literals +AckStatus = Literal['SENT', 'QUEUED', 'EXECUTING', 'COMPLETED', 'FAILED', 'INVALID', 'CANCELLED', 'TIMEOUT', 'NO_TRACKING'] + + +class IOStatus(TypedDict): + """Digital I/O status.""" + in1: int + in2: int + out1: int + out2: int + estop: int + + +class GripperStatus(TypedDict): + """Electric gripper status.""" + id: int + position: int + speed: int + current: int + status_byte: int + object_detect: int + + +class StatusAggregate(TypedDict): + """Aggregate robot status.""" + pose: list[float] # 4x4 transformation matrix flattened (len=16) + angles: list[float] # 6 joint angles in degrees + io: IOStatus | list[int] # Back-compat with existing server format + gripper: GripperStatus | list[int] + + +class TrackingStatus(TypedDict): + """Command tracking status.""" + command_id: str | None + status: AckStatus + details: str + completed: bool + ack_time: datetime | None diff --git a/parol6/server/__init__.py b/parol6/server/__init__.py new file mode 100644 index 0000000..8b1b3a8 --- /dev/null +++ b/parol6/server/__init__.py @@ -0,0 +1 @@ +"""Server management modules.""" diff --git a/headless_commander.py b/parol6/server/headless_commander.py similarity index 94% rename from headless_commander.py rename to parol6/server/headless_commander.py index 541e67e..c17cead 100644 --- a/headless_commander.py +++ b/parol6/server/headless_commander.py @@ -28,7 +28,34 @@ from typing import Optional, Tuple from spatialmath.base import trinterp from collections import namedtuple, deque -import PAROL6_ROBOT +from pathlib import Path + +# Ensure both package dir (parol6) and project root are on sys.path to import PAROL6_ROBOT and others +_pkg_dir = Path(__file__).parent.parent # .../parol6 +_root_dir = Path(__file__).parents[2] # .../PAROL6-python-API +for _p in (str(_root_dir), str(_pkg_dir)): + if _p not in sys.path: + sys.path.insert(0, _p) + +# Robust import of robot constants/kinematics +try: + import PAROL6_ROBOT # from project root +except ModuleNotFoundError: + # Fallback: load directly from file path to handle non-standard execution contexts + try: + from importlib.util import spec_from_file_location, module_from_spec + _robot_path = (_root_dir / "PAROL6_ROBOT.py") + _spec = spec_from_file_location("PAROL6_ROBOT", str(_robot_path)) + if _spec and _spec.loader: + PAROL6_ROBOT = module_from_spec(_spec) + sys.modules["PAROL6_ROBOT"] = PAROL6_ROBOT + _spec.loader.exec_module(PAROL6_ROBOT) # type: ignore[attr-defined] + else: + raise + except Exception as e: + print(f"[FATAL] Unable to import PAROL6_ROBOT from {_robot_path}: {e}", file=sys.stderr) + raise + from smooth_motion import CircularMotion, SplineMotion, MotionBlender, SCurveProfile, QuinticPolynomial, MotionConstraints from gcode import GcodeInterpreter @@ -292,6 +319,7 @@ def parse_arguments(): sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) sock.bind((ip, port)) logger.info(f'Start listening to {ip}:{port}') +START_TIME = time.time() def Unpack_data(data_buffer_list, Position_in,Speed_in,Homed_in,InOut_in,Temperature_error_in,Position_error_in,Timeout_error,Timing_data_in, XTR_data,Gripper_data_in): @@ -1155,7 +1183,7 @@ def calculate_duration_from_speed(trajectory_length: float, speed_percentage: fl return None # Acknowledgment system configuration -CLIENT_ACK_PORT = 5002 # Port where clients listen for acknowledgments +CLIENT_ACK_PORT = int(os.getenv("PAROL6_ACK_PORT", "5002")) # Port where clients listen for acknowledgments ack_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) # Command tracking @@ -1223,11 +1251,16 @@ def parse_command_with_id(message: str) -> Tuple[Optional[str], str]: # --- Test 1: Homing and Initial Setup # -------------------------------------------------------------------------- -# 1. Optionally start with the Home command (can be bypassed via PAROL6_NOAUTOHOME) -if not str(os.getenv("PAROL6_NOAUTOHOME", "0")).lower() in ("1", "true", "yes", "on"): +# 1. Optionally start with the Home command (can be bypassed via PAROL6_NOAUTOHOME or --no-auto-home) +skip_auto_home = ( + str(os.getenv("PAROL6_NOAUTOHOME", "0")).lower() in ("1", "true", "yes", "on") or + args.no_auto_home +) +if not skip_auto_home: command_queue.append(HomeCommand()) else: - logging.info("PAROL6_NOAUTOHOME is set; skipping auto-home on startup.") + reason = "command line flag" if args.no_auto_home else "environment variable" + logging.info(f"Auto-home disabled via {reason}; skipping auto-home on startup.") # --- State variable for the currently running command --- active_command = None @@ -1253,7 +1286,7 @@ def parse_command_with_id(message: str) -> Tuple[Optional[str], str]: while timer.elapsed_time < 1100000: # --- Connection Handling (non-blocking) --- - if ser is None or not ser.is_open: + if not FAKE_SERIAL and (ser is None or not ser.is_open): now = time.time() if now - last_reconnect_attempt > 1.0: logging.warning("Serial port not open. Attempting to reconnect...") @@ -1275,6 +1308,10 @@ def parse_command_with_id(message: str) -> Tuple[Optional[str], str]: except serial.SerialException: ser = None # Do not block or continue; proceed to UDP handling every tick + elif FAKE_SERIAL: + # In FAKE_SERIAL mode, pretend we always have a connection + # This prevents the constant "Serial port not open" warnings + pass # ======================================================================= # === NETWORK COMMAND RECEPTION WITH ID PARSING === @@ -1402,6 +1439,26 @@ def parse_command_with_id(message: str) -> Tuple[Optional[str], str]: if cmd_id: send_acknowledgment(cmd_id, "COMPLETED", "PONG", addr) + elif command_name == 'GET_SERVER_STATE': + # Structured server state for external managers + try: + state = { + "listening": {"transport": "udp", "address": f"{ip}:{port}"}, + "serial_connected": bool(ser and getattr(ser, "is_open", False)), + "homed": any(bool(h) for h in Homed_in) if isinstance(Homed_in, list) else False, + "queue_depth": len(command_queue) if command_queue is not None else 0, + "active_command": type(active_command).__name__ if active_command is not None else None, + "stream_mode": bool(stream_mode), + "uptime_s": float(time.time() - START_TIME) if 'START_TIME' in globals() else 0.0, + } + payload = f"SERVER_STATE|{json.dumps(state)}" + sock.sendto(payload.encode('utf-8'), addr) + if cmd_id: + send_acknowledgment(cmd_id, "COMPLETED", "Server state sent", addr) + except Exception as e: + if cmd_id: + send_acknowledgment(cmd_id, "FAILED", f"State error: {e}", addr) + elif command_name == 'GET_STATUS': # Aggregate POSE, ANGLES, IO, GRIPPER into one frame try: @@ -2054,4 +2111,18 @@ def parse_command_with_id(message: str) -> Tuple[Optional[str], str]: active_command = None active_command_id = None - timer.checkpt() \ No newline at end of file + timer.checkpt() + + +def main(): + """ + Main entry point for the headless commander. + This function wraps the main execution loop for CLI usage. + """ + # The main loop is already implemented above as a module-level script + # This function exists to provide a clean entry point for the CLI + pass + + +if __name__ == "__main__": + main() diff --git a/parol6/server/manager.py b/parol6/server/manager.py new file mode 100644 index 0000000..b7ec6a3 --- /dev/null +++ b/parol6/server/manager.py @@ -0,0 +1,303 @@ +""" +Server management for PAROL6 headless controller. + +Provides lifecycle management and automatic spawning of the headless controller process. +""" + +import asyncio +import contextlib +import logging +import os +import signal +import subprocess +import sys +import threading +import time +from dataclasses import dataclass +from pathlib import Path +from typing import Optional +from collections import deque + + +@dataclass +class ServerOptions: + """Options for launching the headless controller.""" + + com_port: str | None = None + no_autohome: bool = True # Set PAROL6_NOAUTOHOME=1 by default + extra_env: dict | None = None + + +class ServerManager: + """ + Manages the lifecycle of the headless PAROL6 controller. + + - Writes com_port.txt in the controller working directory to preselect the port. + - Spawns the controller as a subprocess using sys.executable. + - Provides stop and liveness checks. + - Can be used with a custom controller script path or defaults to the package's bundled controller. + """ + + def __init__(self, controller_path: str | None = None) -> None: + if controller_path: + self.controller_path = Path(controller_path).resolve() + if not self.controller_path.exists(): + raise FileNotFoundError( + f"Controller script not found: {self.controller_path}" + ) + else: + # Use the package's bundled headless commander + self.controller_path = Path(__file__).parent / "headless_commander.py" + + self._proc: subprocess.Popen | None = None + self._reader_thread: threading.Thread | None = None + self._stop_reader = threading.Event() + self._log_buffer = deque(maxlen=5000) + + @property + def pid(self) -> int | None: + return self._proc.pid if self._proc and self._proc.poll() is None else None + + def is_running(self) -> bool: + return self._proc is not None and self._proc.poll() is None + + def _write_com_port_hint(self, com_port: str) -> None: + """ + The headless_commander.py reads com_port.txt at startup. + Write it unconditionally before launch for consistent behavior across OSes. + """ + cwd = self.controller_path.parent + hint = cwd / "com_port.txt" + try: + hint.write_text(com_port.strip() + "\n", encoding="utf-8") + except Exception as e: + # Non-fatal: controller can still prompt or auto-detect depending on OS + logging.warning("ServerManager: failed to write %s: %s", hint, e) + + async def start_controller( + self, + com_port: str | None = None, + no_autohome: bool = True, + extra_env: dict | None = None + ) -> None: + """Start the controller if not already running.""" + if self.is_running(): + return + + # Working directory should be the PAROL6-python-API root to find dependencies + cwd = self.controller_path.parent.parent + + # Optional COM port preseed + if com_port: + self._write_com_port_hint(com_port) + + env = os.environ.copy() + # Disable autohome unless explicitly overridden + if no_autohome: + env["PAROL6_NOAUTOHOME"] = "1" + if extra_env: + env.update(extra_env) + + # Launch the controller + args = [sys.executable, "-u", str(self.controller_path)] + + # Add log level argument if available + current_level = logging.getLogger().level + level_name = logging.getLevelName(current_level) + if level_name != "Level " + str(current_level): # Valid level name + args.append(f"--log-level={level_name}") + + try: + self._proc = subprocess.Popen( + args, + cwd=str(cwd), + env=env, + stdout=subprocess.PIPE, + stderr=subprocess.STDOUT, + text=True, + bufsize=1, # line-buffered + ) + except Exception as e: + raise RuntimeError(f"Failed to start controller: {e}") from e + + # Start background reader thread to stream server stdout/stderr to logging + if self._proc and self._proc.stdout is not None: + self._stop_reader.clear() + self._reader_thread = threading.Thread( + target=self._stream_output, + args=(self._proc,), + name="ServerOutputReader", + daemon=True, + ) + self._reader_thread.start() + + def _stream_output(self, proc: subprocess.Popen) -> None: + """Read controller stdout and forward to logging.""" + try: + assert proc.stdout is not None + for raw_line in iter(proc.stdout.readline, ""): + if self._stop_reader.is_set(): + break + line = raw_line.rstrip("\r\n") + if line: + # Skip timestamp prefix if present (format: HH:MM:SS.mmm [LEVEL] message) + space_pos = line.find(" ") + if space_pos > 0 and space_pos < 15: # Reasonable timestamp length + # Check if it looks like a timestamp + timestamp_part = line[:space_pos] + if ":" in timestamp_part: + line = line[space_pos + 1:].lstrip() + + # Preserve severity if headless prefixes with [LEVEL] + level = logging.INFO + msg = line + + if line.startswith("[DEBUG]"): + level, msg = logging.DEBUG, line[7:].lstrip() + elif line.startswith("[INFO]"): + level, msg = logging.INFO, line[6:].lstrip() + elif line.startswith("[WARNING]"): + level, msg = logging.WARNING, line[9:].lstrip() + elif line.startswith("[ERROR]"): + level, msg = logging.ERROR, line[7:].lstrip() + elif line.startswith("[CRITICAL]"): + level, msg = logging.CRITICAL, line[10:].lstrip() + + self._log_buffer.append(raw_line.rstrip("\r\n")) + logging.log(level, "[SERVER] %s", msg) + except Exception as e: + logging.warning("ServerManager: output reader stopped: %s", e) + + async def stop_controller(self, timeout: float = 5.0) -> None: + """Stop the controller process if running.""" + if not self.is_running(): + self._proc = None + return + + proc = self._proc + assert proc is not None + + try: + if os.name == "nt": + proc.terminate() + else: + proc.send_signal(signal.SIGTERM) + except Exception: + # Fall back to kill below + pass + + # Wait for graceful exit + t0 = time.time() + while proc.poll() is None and (time.time() - t0) < timeout: + await asyncio.sleep(0.1) + + if proc.poll() is None: + with contextlib.suppress(Exception): + proc.kill() + + # Stop and join background reader thread + with contextlib.suppress(Exception): + self._stop_reader.set() + if proc.stdout: + proc.stdout.close() + if self._reader_thread and self._reader_thread.is_alive(): + with contextlib.suppress(Exception): + self._reader_thread.join(timeout=1.0) + self._reader_thread = None + + self._proc = None + + def get_logs(self, tail: int = 200) -> list[str]: + """Return the last N lines captured from the controller stdout.""" + return list(self._log_buffer)[-tail:] + + async def get_status(self, host: str = "127.0.0.1", port: int = 5001, timeout: float = 1.0) -> dict: + """ + Query controller's server state over UDP and merge with process info. + Returns a dict; if unreachable, returns minimal info. + """ + status = { + "running": self.is_running(), + "pid": self.pid, + "server": None, + } + import socket, json + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: + sock.settimeout(timeout) + sock.sendto(b"GET_SERVER_STATE", (host, port)) + data, _ = sock.recvfrom(4096) + resp = data.decode("utf-8") + if resp.startswith("SERVER_STATE|"): + payload = resp.split("|", 1)[1] + status["server"] = json.loads(payload) + return status + + +async def ensure_server( + host: str = "127.0.0.1", + port: int = 5001, + manage: bool = False, + com_port: str | None = None, + extra_env: dict | None = None +) -> Optional[ServerManager]: + """ + Ensure a PAROL6 server is running and accessible. + + Args: + host: Server host to check/connect to + port: Server port to check/connect to + manage: If True, automatically spawn controller if ping fails + com_port: COM port for spawned controller + extra_env: Additional environment variables for spawned controller + + Returns: + ServerManager instance if manage=True and server was spawned, None otherwise + + Usage: + # Just check if server is running + await ensure_server() + + # Auto-spawn if not running + mgr = await ensure_server(manage=True, com_port="/dev/ttyACM0") + """ + # Test if server is already running + try: + import socket + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: + sock.settimeout(1.0) + sock.sendto(b"PING", (host, port)) + data, _ = sock.recvfrom(256) + if data.decode('utf-8').strip().upper() == "PONG": + logging.info(f"Server already running at {host}:{port}") + return None + except Exception: + pass + + if not manage: + logging.warning(f"Server not responding at {host}:{port} and manage=False") + return None + + # Spawn controller + logging.info(f"Server not responding at {host}:{port}, starting controller...") + manager = ServerManager() + await manager.start_controller( + com_port=com_port, + no_autohome=True, + extra_env=extra_env + ) + + try: + import socket + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: + sock.settimeout(2.0) + sock.sendto(b"PING", (host, port)) + data, _ = sock.recvfrom(256) + if data.decode('utf-8').strip().upper() == "PONG": + logging.info(f"Successfully started server at {host}:{port}") + return manager + except Exception as e: + logging.error(f"Failed to verify server startup: {e}") + + logging.error("Server spawn failed or not responding after startup") + await manager.stop_controller() + return None diff --git a/parol6/utils/__init__.py b/parol6/utils/__init__.py new file mode 100644 index 0000000..183c974 --- /dev/null +++ b/parol6/utils/__init__.py @@ -0,0 +1 @@ +"""Utility modules.""" diff --git a/parol6/utils/tracking.py b/parol6/utils/tracking.py new file mode 100644 index 0000000..89d8d17 --- /dev/null +++ b/parol6/utils/tracking.py @@ -0,0 +1,334 @@ +""" +Lazy command tracking utilities for PAROL6. + +Provides optional UDP acknowledgment tracking with zero overhead when not used. +The tracking system is only initialized when explicitly requested. +""" + +import os +import socket +import threading +import time +import uuid +from collections import deque +from datetime import datetime, timedelta +from typing import Dict, Optional, Tuple, Union + + +def _get_env_int(name: str, default: int) -> int: + """ + Safe environment variable parsing for integers. + Returns default for unset or empty string values. + """ + value = os.getenv(name) + if not value: # None or empty string + return default + try: + return int(value) + except ValueError: + raise ValueError(f"Environment variable {name}='{value}' is not a valid integer") + + +# Global configuration with environment variable overrides +SERVER_IP = os.getenv("PAROL6_SERVER_IP", "127.0.0.1") +SERVER_PORT = _get_env_int("PAROL6_SERVER_PORT", 5001) +ACK_PORT = _get_env_int("PAROL6_ACK_PORT", 5002) + +# Global tracker - starts as None (no resources) +_command_tracker = None +_tracker_lock = threading.Lock() + + +def reset_tracking(): + """ + Reset and cleanup the command tracker. + Useful for tests and cleanup scenarios. + """ + global _command_tracker, _tracker_lock + + with _tracker_lock: + if _command_tracker: + _command_tracker._cleanup() + _command_tracker = None + + +class LazyCommandTracker: + """ + Command tracker with lazy initialization. + Resources are ONLY allocated when tracking is actually used. + """ + + def __init__(self, listen_port=None, history_size=100): + # Use ACK_PORT constant if not specified + if listen_port is None: + listen_port = ACK_PORT + self.listen_port = listen_port + self.history_size = history_size + self.command_history = {} + self.lock = threading.Lock() + + # Lazy initialization flags + self._initialized = False + self._thread = None + self._socket = None + self._running = False + + def _lazy_init(self): + """ + Initialize resources only when first tracking is requested. + This is called ONLY when someone uses tracking features. + """ + if self._initialized: + return True + + try: + print("[Tracker] First tracking request - initializing resources...") + + # Socket initialization + self._socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + self._socket.bind(('', self.listen_port)) + self._socket.settimeout(0.1) + + # Start thread + self._running = True + self._thread = threading.Thread(target=self._listen_loop, daemon=True) + self._thread.start() + + self._initialized = True + print(f"[Tracker] Initialized on port {self.listen_port}") + return True + + except Exception as e: + print(f"[Tracker] Failed to initialize: {e}") + self._cleanup() + return False + + def _cleanup(self): + """Clean up resources""" + self._running = False + if self._thread: + self._thread.join(timeout=0.5) + self._thread = None + if self._socket: + self._socket.close() + self._socket = None + self._initialized = False + + def _listen_loop(self): + """Listener thread - only runs if tracking is used""" + while self._running: + try: + data, addr = self._socket.recvfrom(2048) + message = data.decode('utf-8') + + parts = message.split('|', 3) + if parts[0] == 'ACK' and len(parts) >= 3: + cmd_id = parts[1] + status = parts[2] + details = parts[3] if len(parts) > 3 else "" + + with self.lock: + if cmd_id in self.command_history: + self.command_history[cmd_id].update({ + 'status': status, + 'details': details, + 'ack_time': datetime.now(), + 'completed': status in ['COMPLETED', 'FAILED', 'INVALID', 'CANCELLED'] + }) + + # Clean old entries (only if we have many) + if len(self.command_history) > self.history_size: + self._cleanup_old_entries() + + except socket.timeout: + continue + except Exception: + if self._running: + pass # Silently continue + + def _cleanup_old_entries(self): + """Remove old entries to prevent memory growth""" + with self.lock: + now = datetime.now() + expired = [cmd_id for cmd_id, info in self.command_history.items() + if now - info['sent_time'] > timedelta(seconds=30)] + for cmd_id in expired: + del self.command_history[cmd_id] + + def track_command(self, command: str) -> Tuple[str, Optional[str]]: + """ + Track a command - initializes tracker if needed. + Returns (modified_command, cmd_id) + """ + # Initialize on first use + if not self._initialized: + if not self._lazy_init(): + # Initialization failed - fall back to non-tracking + return command, None + + # Generate ID and modify command + cmd_id = str(uuid.uuid4())[:8] + tracked_command = f"{cmd_id}|{command}" + + # Register in history + with self.lock: + self.command_history[cmd_id] = { + 'command': command, + 'sent_time': datetime.now(), + 'status': 'SENT', + 'details': '', + 'completed': False + } + + return tracked_command, cmd_id + + def get_status(self, cmd_id: str) -> Optional[Dict]: + """Get status if tracker is initialized""" + if not self._initialized: + return None + with self.lock: + return self.command_history.get(cmd_id, None) + + def wait_for_completion(self, cmd_id: str, timeout: float = 5.0) -> Dict: + """Wait for completion if tracker is initialized""" + if not self._initialized: + return {'status': 'NO_TRACKING', 'details': 'Tracker not initialized', 'completed': True} + + start_time = time.time() + while time.time() - start_time < timeout: + status = self.get_status(cmd_id) + if status and status['completed']: + return status + time.sleep(0.01) + + return self.get_status(cmd_id) or { + 'status': 'TIMEOUT', + 'details': 'No acknowledgment received', + 'completed': True + } + + def is_active(self) -> bool: + """Check if tracker is initialized and running""" + return self._initialized and self._running + + +def _get_tracker_if_needed() -> Optional[LazyCommandTracker]: + """ + Get tracker ONLY if tracking is requested. + This ensures zero overhead for non-tracking operations. + """ + global _command_tracker, _tracker_lock + + # Check if tracking is explicitly disabled + disable_tracking = os.getenv("PAROL6_DISABLE_TRACKING", "").lower() in ("1", "true", "yes", "on") + if disable_tracking: + return None + + # Fast path - tracker already exists + if _command_tracker is not None: + return _command_tracker + + # Slow path - create tracker (only happens once) + with _tracker_lock: + if _command_tracker is None: + _command_tracker = LazyCommandTracker() + return _command_tracker + + +def send_robot_command_tracked(command_string: str) -> Tuple[str, Optional[str]]: + """ + Send with tracking - initializes tracker on first use. + + Resource impact: + - First call: Starts tracker thread + - Subsequent calls: Minimal overhead (UUID generation) + """ + tracker = _get_tracker_if_needed() + if tracker: + tracked_cmd, cmd_id = tracker.track_command(command_string) + if cmd_id: + # Send tracked command + try: + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: + sock.sendto(tracked_cmd.encode('utf-8'), (SERVER_IP, SERVER_PORT)) + return f"Command sent with tracking (ID: {cmd_id})", cmd_id + except Exception as e: + return f"Error: {e}", None + + # Fall back to non-tracked + try: + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: + sock.sendto(command_string.encode('utf-8'), (SERVER_IP, SERVER_PORT)) + return f"Successfully sent command: '{command_string[:50]}...'", None + except Exception as e: + return f"Error sending command: {e}", None + + +def send_and_wait( + command_string: str, + timeout: float = 2.0, + non_blocking: bool = False +) -> Union[Dict, str, None]: + """ + Send and wait for acknowledgment OR return a command_id immediately. + First use initializes tracker. + """ + result, cmd_id = send_robot_command_tracked(command_string) + + if cmd_id: + # If non_blocking is True, return the ID right away + if non_blocking: + return cmd_id + + # Otherwise, proceed with the original blocking logic + tracker = _get_tracker_if_needed() + if tracker: + status_dict = tracker.wait_for_completion(cmd_id, timeout) + # Add the command_id to the returned dictionary + status_dict['command_id'] = cmd_id + return status_dict + + # Fallback for tracking failures + if non_blocking: + return None + else: + return {'status': 'NO_TRACKING', 'details': result, 'completed': True, 'command_id': None} + + +def check_command_status(command_id: str) -> Optional[Dict]: + """ + Check status - returns None if tracker not initialized. + Does NOT initialize tracker (read-only). + """ + if _command_tracker and _command_tracker.is_active(): + return _command_tracker.get_status(command_id) + return None + + +def is_tracking_active() -> bool: + """ + Check if tracking is active. + Returns False if never used (zero overhead check). + """ + return _command_tracker is not None and _command_tracker.is_active() + + +def get_tracking_stats() -> Dict: + """ + Get resource usage statistics. + """ + if _command_tracker and _command_tracker.is_active(): + with _command_tracker.lock: + return { + 'active': True, + 'commands_tracked': len(_command_tracker.command_history), + 'memory_bytes': len(str(_command_tracker.command_history)), + 'thread_active': _command_tracker._thread.is_alive() if _command_tracker._thread else False + } + else: + return { + 'active': False, + 'commands_tracked': 0, + 'memory_bytes': 0, + 'thread_active': False + } diff --git a/pyproject.toml b/pyproject.toml index a071a6f..af08c1a 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -1,8 +1,73 @@ -// filepath: pyproject.toml [build-system] requires = ["setuptools>=61.0"] build-backend = "setuptools.build_meta" [project] -name = "parol6-python-api" -version = "0.1.0" \ No newline at end of file +name = "parol6" +version = "0.1.0" +description = "Python library for controlling PAROL6 robot arms" + +requires-python = ">=3.11" +dependencies = [ + "numpy", + "pyserial", + "spatialmath-python", + "roboticstoolbox-python", + "oclock", + "keyboard", +] + +[tool.setuptools.packages.find] +include = ["parol6*"] + +[project.optional-dependencies] +dev = ["ruff", "mypy", "pytest", "pytest-asyncio", "pre-commit"] + +[project.scripts] +parol6-server = "parol6.cli.server:main_entry" + +[tool.pytest.ini_options] +# Limit discovery to the tests/ directory only +testpaths = ["tests"] + +# Test discovery patterns +python_files = ["test_*.py", "*_test.py"] +python_classes = ["Test*"] +python_functions = ["test_*"] + +# Output configuration +addopts = [ + "-v", + "--tb=short", + "--strict-markers", + "--disable-warnings", + "-p", "no:cacheprovider" +] + +# Timeout configuration (requires pytest-timeout) +timeout = 300 +timeout_method = "thread" + +# Logging configuration for tests +log_cli = true +log_cli_level = "INFO" +log_cli_format = "%(asctime)s [%(levelname)8s] %(name)s: %(message)s" +log_cli_date_format = "%Y-%m-%d %H:%M:%S" + +# Pytest markers for different test types +markers = [ + "unit: Unit tests that test individual components in isolation", + "integration: Integration tests that test component interactions with FAKE_SERIAL", + "hardware: Hardware tests that require actual robot hardware and human confirmation", + "slow: Slow-running tests (typically hardware or complex integration tests)", + "e2e: End-to-end tests that exercise complete workflows", + "gcode: Tests specifically for GCODE parsing and interpretation functionality" +] + +# Filter warnings +filterwarnings = [ + "ignore::DeprecationWarning", + "ignore::PendingDeprecationWarning", + "ignore::UserWarning:roboticstoolbox.*", + "ignore::UserWarning:spatialmath.*" +] diff --git a/requirements-test.txt b/requirements-test.txt new file mode 100644 index 0000000..17c1b46 --- /dev/null +++ b/requirements-test.txt @@ -0,0 +1,26 @@ +# Core testing framework +pytest>=7.0.0 +pytest-timeout>=2.1.0 +pytest-xdist>=3.0.0 +pytest-rerunfailures>=11.0.0 + +# Type checking support +typing-extensions>=4.0.0 + +# Core dependencies needed for integration tests +# (Pinned versions for CI reproducibility) +roboticstoolbox-python==1.0.3 +numpy==1.23.4 +scipy==1.11.4 +spatialmath>=1.0.0 + +# Hardware communication +pyserial>=3.5 + +# Timing and system utilities +oclock>=1.0.0 +keyboard>=0.13.5 + +# Additional test utilities +mock>=4.0.0 +pytest-mock>=3.10.0 diff --git a/robot_api.py b/robot_api.py index 014741e..7020bdd 100644 --- a/robot_api.py +++ b/robot_api.py @@ -12,17 +12,44 @@ import queue import uuid import json +import os from collections import deque from datetime import datetime, timedelta -# Global configuration -SERVER_IP = "127.0.0.1" -SERVER_PORT = 5001 +def _get_env_int(name: str, default: int) -> int: + """ + Safe environment variable parsing for integers. + Returns default for unset or empty string values. + """ + value = os.getenv(name) + if not value: # None or empty string + return default + try: + return int(value) + except ValueError: + raise ValueError(f"Environment variable {name}='{value}' is not a valid integer") + +# Global configuration with environment variable overrides +SERVER_IP = os.getenv("PAROL6_SERVER_IP", "127.0.0.1") +SERVER_PORT = _get_env_int("PAROL6_SERVER_PORT", 5001) +ACK_PORT = _get_env_int("PAROL6_ACK_PORT", 5002) # Global tracker - starts as None (no resources) _command_tracker = None _tracker_lock = threading.Lock() +def reset_tracking(): + """ + Reset and cleanup the command tracker. + Useful for tests and cleanup scenarios. + """ + global _command_tracker, _tracker_lock + + with _tracker_lock: + if _command_tracker: + _command_tracker._cleanup() + _command_tracker = None + # ============================================================================ # ORIGINAL SEND FUNCTION - ZERO OVERHEAD # ============================================================================ @@ -55,7 +82,10 @@ class LazyCommandTracker: Resources are ONLY allocated when tracking is actually used. """ - def __init__(self, listen_port=5002, history_size=100): + def __init__(self, listen_port=None, history_size=100): + # Use ACK_PORT constant if not specified + if listen_port is None: + listen_port = ACK_PORT self.listen_port = listen_port self.history_size = history_size self.command_history = {} @@ -149,7 +179,7 @@ def _cleanup_old_entries(self): for cmd_id in expired: del self.command_history[cmd_id] - def track_command(self, command: str) -> Tuple[str, str]: + def track_command(self, command: str) -> Tuple[str, Optional[str]]: """ Track a command - initializes tracker if needed. Returns (modified_command, cmd_id) @@ -342,7 +372,8 @@ def move_robot_pose( command = f"MOVEPOSE|{pose_str}|{duration_str}|{speed_str}" if wait_for_ack: - return send_and_wait(command, timeout, non_blocking) + result = send_and_wait(command, timeout, non_blocking) + return result if result is not None else {'status': 'ERROR', 'details': 'Send failed'} else: return send_robot_command(command) @@ -389,7 +420,7 @@ def jog_multiple_joints( wait_for_ack: bool = False, timeout: float = 2.0, non_blocking: bool = False -) -> str: +) -> Union[str, Dict]: """ Jogs multiple robot joints simultaneously for a specified duration. @@ -454,7 +485,7 @@ def move_robot_cartesian( wait_for_ack: bool = False, timeout: float = 2.0, non_blocking: bool = False -) -> str: +) -> Union[str, Dict]: """ Moves the robot's end-effector to a specific Cartesian pose in a straight line. @@ -1930,4 +1961,4 @@ def zero_work_coordinates(coordinate_system: str = 'G54', wait_for_ack: bool = F """ # This sets the current position as 0,0,0 in the work coordinate system return set_work_coordinate_offset(coordinate_system, x=0, y=0, z=0, - wait_for_ack=wait_for_ack, timeout=timeout) \ No newline at end of file + wait_for_ack=wait_for_ack, timeout=timeout) diff --git a/test_script.py b/test_script.py deleted file mode 100644 index 24a9be1..0000000 --- a/test_script.py +++ /dev/null @@ -1,33 +0,0 @@ -from robot_api import move_robot_joints, home_robot, delay_robot, get_robot_joint_angles, control_pneumatic_gripper,get_robot_pose, control_electric_gripper, move_robot_pose,move_robot_cartesian,get_electric_gripper_status,get_robot_io -import time -print("Homing robot...") -time.sleep(2) -control_electric_gripper(action = "calibrate") -time.sleep(2) -control_electric_gripper(action='move', position=100, speed=150, current = 200) -time.sleep(2) -control_electric_gripper(action='move', position=200, speed=150, current = 200) -time.sleep(2) -print(get_robot_joint_angles()) -print(get_robot_pose()) -print("Moving to new position...") -control_pneumatic_gripper("open",1) -time.sleep(0.3) -control_pneumatic_gripper("close",1) -time.sleep(0.3) -control_pneumatic_gripper("open",1) -time.sleep(0.3) -control_pneumatic_gripper("close",1) -time.sleep(0.3) -move_robot_joints([90, -90, 160, 12, 12, 180], duration=5.5) -time.sleep(6) -move_robot_joints([50, -60, 180, -12, 32, 0], duration=5.5) -time.sleep(6) -move_robot_joints([90, -90, 160, 12, 12, 180], duration=5.5) -time.sleep(6) -move_robot_pose([7, 250, 200, -100, 0, -90], duration=5.5) -time.sleep(6) -move_robot_cartesian([7, 250, 150, -100, 0, -90], speed_percentage=50) -delay_robot(0.2) -print(get_electric_gripper_status()) -print(get_robot_io()) diff --git a/test_smooth_motion.py b/test_smooth_motion.py deleted file mode 100644 index fe997bc..0000000 --- a/test_smooth_motion.py +++ /dev/null @@ -1,842 +0,0 @@ -import time -import robot_api -import numpy as np - -# Define the safe starting joint configuration for all smooth motion tests. -# This ensures consistency and repeatability for each test. -# Angles: [J1, J2, J3, J4, J5, J6] in degrees. -SAFE_SMOOTH_START_JOINTS = [42.697,-89.381,144.831,-0.436,31.528,180.0] - -def initialize_test_position(): - """ - Moves the robot to the predefined safe starting joint angles and waits. - This function is called before every smooth motion test. - - Returns: - list: The robot's Cartesian pose [x, y, z, rx, ry, rz] after moving, - or None if the move fails or the pose cannot be retrieved. - """ - print("\n" + "="*60) - print(f"MOVING TO SAFE STARTING POSITION: {SAFE_SMOOTH_START_JOINTS}") - print("="*60) - - # Move to the joint position with a 4-second duration and wait for acknowledgment. - result = robot_api.move_robot_joints( - SAFE_SMOOTH_START_JOINTS, - duration=4, - wait_for_ack=True, - timeout=5 - ) - print(f"--> Move command result: {result}") - - # Wait until the robot has physically stopped moving. - if robot_api.wait_for_robot_stopped(timeout=10): - print("--> Robot has reached the starting position.") - time.sleep(1) - start_pose = robot_api.get_robot_pose() - if start_pose: - print(f"--> Starting Pose confirmed at: {[round(p, 2) for p in start_pose]}") - return start_pose - else: - print("--> ERROR: Could not retrieve robot pose after moving.") - return None - else: - print("--> ERROR: Robot did not stop in time. Aborting test.") - return None - -def test_smooth_circle_basic(start_pose): - """Tests the smooth_circle command with different planes, directions, and timing modes.""" - print("\n--- TESTING SMOOTH CIRCLE (BASIC) ---") - - # Define a center point relative to the starting Z-height - radius = 30.0 # 30mm radius - - center_point = [start_pose[0], start_pose[1] + radius, start_pose[2]] # Changed from +50 to +radius - - # Test 1: XY plane, counter-clockwise with DURATION - print("\n[1/4] Testing Circle: XY Plane, Counter-Clockwise (Duration mode)") - result = robot_api.smooth_circle( - center=center_point, - radius=radius, - plane='XY', - duration=5.0, # Using duration - clockwise=False, - wait_for_ack=True - ) - print(f"--> Command result: {result}") - robot_api.wait_for_robot_stopped(timeout=10) - time.sleep(2) - - # Test 1: XY plane, counter-clockwise with DURATION in TRF - print("\n[2/4] Testing Circle: XY Plane, Counter-Clockwise (Duration mode)") - result = robot_api.smooth_circle( - center=center_point, - radius=radius, - frame='TRF', # NEW: Test in TRF - plane='XY', - duration=5.0, # Using duration - clockwise=False, - wait_for_ack=True - ) - print(f"--> Command result: {result}") - robot_api.wait_for_robot_stopped(timeout=10) - time.sleep(2) - - # Test 2: XZ plane, clockwise with SPEED PERCENTAGE - print("\n[3/4] Testing Circle: XZ Plane, Clockwise (Speed percentage mode)") - result = robot_api.smooth_circle( - center=center_point, - radius=radius, - plane='XZ', - speed_percentage=30, # Using speed percentage (30% speed) - clockwise=True, - wait_for_ack=True - ) - print(f"--> Command result: {result}") - robot_api.wait_for_robot_stopped(timeout=10) - time.sleep(2) - - # Test 3: YZ plane with specified start position (NEW) - print("\n[4/4] Testing Circle: YZ Plane with SPECIFIED START POSITION") - # Define a start position slightly offset from current - specified_start = [start_pose[0] + 10, start_pose[1] + 10, start_pose[2], - start_pose[3], start_pose[4], start_pose[5]] - result = robot_api.smooth_circle( - center=center_point, - radius=radius, - plane='YZ', - start_pose=specified_start, # NEW: Will transition here first - duration=5.0, - clockwise=False, - wait_for_ack=True - ) - print(f"--> Command result: {result}") - robot_api.wait_for_robot_stopped(timeout=10) - -def test_smooth_arc_with_start_positions(start_pose): - """Tests smooth arc commands with specified start positions and transitions.""" - print("\n--- TESTING SMOOTH ARC WITH START POSITIONS ---") - - # Test 1: Arc with FAR start position (should see smooth transition) - print("\n[1/4] Testing Arc with FAR START POSITION (big transition)") - far_start = [start_pose[0] + 40, start_pose[1] - 20, start_pose[2] + 10, - start_pose[3], start_pose[4], start_pose[5]] - arc_center = [far_start[0] - 20, far_start[1], far_start[2]] - end_pose_arc = [arc_center[0], arc_center[1] + 20, far_start[2], - far_start[3], far_start[4], far_start[5] + 45] - - print(f" Current position: {[round(p, 1) for p in start_pose[:3]]}") - print(f" Transition to: {[round(p, 1) for p in far_start[:3]]}") - print(f" Then arc to: {[round(p, 1) for p in end_pose_arc[:3]]}") - - result = robot_api.smooth_arc_center( - end_pose=end_pose_arc, - center=arc_center, - start_pose=far_start, # Will transition here first - duration=6.0, - clockwise=True, - wait_for_ack=True - ) - print(f"--> Command result: {result}") - robot_api.wait_for_robot_stopped(timeout=12) - time.sleep(2) - - # Re-initialize for next test - current_pose = initialize_test_position() - if not current_pose: return - - # Test 2: Arc with CLOSE start position (minimal transition) - print("\n[2/4] Testing Arc with CLOSE START POSITION (minimal transition)") - close_start = [current_pose[0] + 2, current_pose[1] + 2, current_pose[2], - current_pose[3], current_pose[4], current_pose[5]] - arc_center = [close_start[0] - 15, close_start[1], close_start[2]] - end_pose_arc = [arc_center[0], arc_center[1] + 15, close_start[2], - close_start[3], close_start[4], close_start[5] + 30] - - print(f" Current position: {[round(p, 1) for p in current_pose[:3]]}") - print(f" Transition to: {[round(p, 1) for p in close_start[:3]]}") - print(f" Then arc to: {[round(p, 1) for p in end_pose_arc[:3]]}") - - result = robot_api.smooth_arc_center( - end_pose=end_pose_arc, - center=arc_center, - start_pose=close_start, # Very close, minimal transition - speed_percentage=40, - clockwise=False, - wait_for_ack=True - ) - print(f"--> Command result: {result}") - robot_api.wait_for_robot_stopped(timeout=10) - time.sleep(2) - - # Test 3: Parametric arc with specified start - print("\n[3/4] Testing PARAMETRIC Arc with specified start") - param_start = [current_pose[0] - 10, current_pose[1] + 5, current_pose[2], - current_pose[3], current_pose[4], current_pose[5]] - end_pose_param = [param_start[0] + 20, param_start[1] - 10, param_start[2], - param_start[3], param_start[4], param_start[5]] - - result = robot_api.smooth_arc_parametric( - end_pose=end_pose_param, - radius=20.0, - arc_angle=60.0, - start_pose=param_start, - duration=4.0, - clockwise=False, - wait_for_ack=True - ) - print(f"--> Command result: {result}") - robot_api.wait_for_robot_stopped(timeout=10) - - # Test 4: Arc in TRF - arc plane follows tool orientation - print("\n[4/4] Testing Arc in TOOL REFERENCE FRAME (TRF)") - # In TRF, the arc is defined relative to the tool's coordinate system - trf_start = [10, 10, 10, 0, 0, 0] # Position relative to tool - trf_center = [0, 0, 0] # Center at tool origin - trf_end = [10, -10, 10, 0, 0, 45] # End position in tool frame - - print(f" TRF Arc - all coordinates relative to tool position/orientation") - print(f" If tool is tilted, the arc plane will be tilted too!") - - result = robot_api.smooth_arc_center( - end_pose=trf_end, - center=trf_center, - frame='TRF', # NEW: Using Tool Reference Frame - start_pose=trf_start, - duration=5.0, - clockwise=False, - wait_for_ack=True - ) - print(f"--> Command result: {result}") - robot_api.wait_for_robot_stopped(timeout=10) - -def test_motion_chaining(start_pose): - """Tests precise motion chaining using end pose of one motion as start of next.""" - print("\n--- TESTING MOTION CHAINING (NEW) ---") - print("This tests using the exact end pose of one motion as the start of the next") - - # Motion 1: Arc to a specific end pose - print("\n[1/4] First Motion: Arc") - arc_center = [start_pose[0] - 20, start_pose[1], start_pose[2]] - arc_end = [arc_center[0], arc_center[1] + 30, start_pose[2], - start_pose[3], start_pose[4] + 15, start_pose[5] + 45] - - result = robot_api.smooth_arc_center( - end_pose=arc_end, - center=arc_center, - duration=4.0, - clockwise=True, - wait_for_ack=True - ) - print(f"--> Arc ended at: {[round(p, 1) for p in arc_end[:3]]}") - robot_api.wait_for_robot_stopped(timeout=8) - time.sleep(1) - - # Motion 2: Circle in TRF starting exactly where arc ended - print("\n[2/4] Second Motion: Circle in TRF starting at arc's end position") - # In TRF, center is relative to current tool position - trf_circle_center = [0, 25, 0] # 25mm forward in tool Y-axis - - result = robot_api.smooth_circle( - center=trf_circle_center, - radius=25.0, - plane='XY', # This is the tool's XY plane, not world XY! - frame='TRF', # NEW: Circle plane follows tool orientation - start_pose=arc_end, # Start exactly where arc ended - speed_percentage=35, - clockwise=False, - wait_for_ack=True - ) - print(f"--> Circle in TRF completed (plane followed tool orientation)") - robot_api.wait_for_robot_stopped(timeout=10) - time.sleep(1) - - # Since circle returns to start, we know where we are - circle_end = arc_end # Circle returns to its start point - - # Motion 3: Helix starting where circle ended - print("\n[3/4] Third Motion: Helix starting at circle's position") - # Calculate actual radius from circle end position - helix_center = [circle_end[0], circle_end[1], circle_end[2] - 30] - # Use the actual distance as radius - actual_radius = np.sqrt((circle_end[0] - helix_center[0])**2 + - (circle_end[1] - helix_center[1])**2) - radius = max(actual_radius, 1.0) # Use actual distance, minimum 1mm - - result = robot_api.smooth_helix( - center=helix_center, - radius=15.0, - pitch=10.0, - height=30.0, - start_pose=circle_end, # Start where circle ended - duration=6.0, - clockwise=True, - wait_for_ack=True - ) - print(f"--> Helix completed") - robot_api.wait_for_robot_stopped(timeout=10) - time.sleep(1) - - # Calculate helix end position (approximately) - helix_end = [helix_center[0] + 15, helix_center[1], helix_center[2] + 30, - circle_end[3], circle_end[4], circle_end[5]] - - # Motion 4: Spline back to near start - print("\n[4/4] Fourth Motion: Spline path back near start") - waypoints = [ - helix_end, # Start from helix end - [helix_end[0] - 10, helix_end[1] - 10, helix_end[2] - 10, - helix_end[3], helix_end[4], helix_end[5] - 20], - [start_pose[0] + 5, start_pose[1] + 5, start_pose[2], - start_pose[3], start_pose[4], start_pose[5]] - ] - - result = robot_api.smooth_spline( - waypoints=waypoints[1:], # Skip first since we specify start_pose - start_pose=waypoints[0], # Explicitly start from helix end - speed_percentage=30, - wait_for_ack=True - ) - print(f"--> Spline completed - returned near start") - robot_api.wait_for_robot_stopped(timeout=10) - -def test_smooth_spline_with_starts(start_pose): - """Tests smooth_spline with various start position scenarios.""" - print("\n--- TESTING SMOOTH SPLINE WITH START POSITIONS ---") - - # Test 1: Spline with default start (current position) - print("\n[1/4] Spline with DEFAULT start (from current position)") - waypoints = [] - for i in range(4): - x = start_pose[0] + i * 15 - y = start_pose[1] + (15 if i % 2 else -15) - z = start_pose[2] - waypoints.append([x, y, z, start_pose[3], start_pose[4], start_pose[5]]) - - result = robot_api.smooth_spline( - waypoints=waypoints, - # No start_pose specified - uses current - duration=5.0, - wait_for_ack=True - ) - print(f"--> Command result: {result}") - robot_api.wait_for_robot_stopped(timeout=10) - time.sleep(2) - - # Re-initialize - current_pose = initialize_test_position() - if not current_pose: return - - # Test 2: Spline with specified start far from first waypoint - print("\n[2/4] Spline with SPECIFIED start (different from first waypoint)") - specified_start = [current_pose[0] - 20, current_pose[1] + 15, current_pose[2], - current_pose[3], current_pose[4], current_pose[5]] - - waypoints = [ - [specified_start[0] + 30, specified_start[1], specified_start[2], - specified_start[3], specified_start[4], specified_start[5]], - [specified_start[0] + 40, specified_start[1] + 20, specified_start[2], - specified_start[3], specified_start[4], specified_start[5]], - [specified_start[0] + 20, specified_start[1] + 30, specified_start[2], - specified_start[3], specified_start[4], specified_start[5]] - ] - - print(f" Current: {[round(p, 1) for p in current_pose[:3]]}") - print(f" Will transition to: {[round(p, 1) for p in specified_start[:3]]}") - print(f" Then follow spline through waypoints") - - result = robot_api.smooth_spline( - waypoints=waypoints, - start_pose=specified_start, # Will transition here first - speed_percentage=40, - wait_for_ack=True - ) - print(f"--> Command result: {result}") - robot_api.wait_for_robot_stopped(timeout=12) - time.sleep(2) - - # Re-initialize - current_pose = initialize_test_position() - if not current_pose: return - - # Test 3: Spline with start matching first waypoint (no transition needed) - print("\n[3/4] Spline with start MATCHING first waypoint (no transition)") - first_waypoint = [current_pose[0] + 5, current_pose[1] + 5, current_pose[2], - current_pose[3], current_pose[4], current_pose[5]] - - waypoints = [ - first_waypoint, # Same as start_pose - [first_waypoint[0] + 20, first_waypoint[1] + 10, first_waypoint[2], - first_waypoint[3], first_waypoint[4], first_waypoint[5]], - [first_waypoint[0] + 10, first_waypoint[1] + 25, first_waypoint[2], - first_waypoint[3], first_waypoint[4], first_waypoint[5]] - ] - - result = robot_api.smooth_spline( - waypoints=waypoints[1:], # Skip first since we use it as start_pose - start_pose=first_waypoint, # Same as would-be first waypoint - duration=4.0, - wait_for_ack=True - ) - print(f"--> Command result: {result}") - robot_api.wait_for_robot_stopped(timeout=8) - - # Test 4: Spline in TRF - waypoints relative to tool - print("\n[4/4] Spline in TOOL REFERENCE FRAME (TRF)") - # In TRF, all waypoints are relative to the tool's coordinate system - trf_waypoints = [ - [20, 0, 0, 0, 0, 0], # 20mm forward in tool X - [20, 20, 0, 0, 0, 15], # Add 20mm in tool Y - [0, 20, 10, 0, 0, 30], # Move to tool Y=20, Z=10 - [0, 0, 0, 0, 0, 0] # Return to tool origin - ] - - print(f" TRF Spline - all waypoints relative to tool coordinate system") - print(f" If tool is rotated, entire spline path rotates with it!") - - result = robot_api.smooth_spline( - waypoints=trf_waypoints, - frame='TRF', # NEW: Waypoints interpreted in tool frame - duration=6.0, - wait_for_ack=True - ) - print(f"--> Command result: {result}") - robot_api.wait_for_robot_stopped(timeout=10) - -def test_smooth_helix_with_starts(start_pose): - """Tests smooth_helix with specified start positions.""" - print("\n--- TESTING SMOOTH HELIX WITH START POSITIONS ---") - - # Test 1: Helix with default start - print("\n[1/3] Helix with DEFAULT start (from current position)") - center = [start_pose[0], start_pose[1] + 30, start_pose[2] - 40] - - result = robot_api.smooth_helix( - center=center, - radius=30.0, - pitch=12.0, - height=36.0, # 3 revolutions - # No start_pose - uses current - duration=10.0, - clockwise=True, - wait_for_ack=True - ) - print(f"--> Command result: {result}") - robot_api.wait_for_robot_stopped(timeout=10) - time.sleep(2) - - # Re-initialize - current_pose = initialize_test_position() - if not current_pose: return - - # Test 2: Helix with specified start on the helix perimeter - print("\n[2/3] Helix with SPECIFIED start on perimeter") - center = [current_pose[0], current_pose[1] + 30, current_pose[2] - 40] - # Start position on the helix perimeter (different angle) - start_on_perimeter = [ - center[0] + 20, # radius * cos(0) - center[1], # radius * sin(0) - center[2], # Starting height - current_pose[3], current_pose[4], current_pose[5] - ] - - print(f" Current: {[round(p, 1) for p in current_pose[:3]]}") - print(f" Will transition to helix start: {[round(p, 1) for p in start_on_perimeter[:3]]}") - - result = robot_api.smooth_helix( - center=center, - radius=20.0, - pitch=12.0, - height=36.0, - start_pose=start_on_perimeter, - speed_percentage=30, - clockwise=False, - wait_for_ack=True - ) - print(f"--> Command result: {result}") - robot_api.wait_for_robot_stopped(timeout=12) - - # Test 3: Helix in TRF - helix axis follows tool Z-axis - print("\n[3/3] Helix in TOOL REFERENCE FRAME (TRF)") - # In TRF, the helix rises along the tool's Z-axis, not world Z - trf_center = [0, 30, -40] # Center relative to tool - trf_start = [20, 30, -40, 0, 0, 0] # Start on perimeter - - print(f" TRF Helix - rises along TOOL'S Z-axis") - print(f" If tool is horizontal, helix will be horizontal too!") - - result = robot_api.smooth_helix( - center=trf_center, - radius=20.0, - pitch=12.0, - height=36.0, - frame='TRF', # NEW: Helix axis follows tool orientation - start_pose=trf_start, - duration=8.0, - clockwise=True, - wait_for_ack=True - ) - print(f"--> Command result: {result}") - robot_api.wait_for_robot_stopped(timeout=12) - -def test_smooth_blend_with_starts(start_pose): - """Tests smooth_blend with specified start position for first segment.""" - print("\n--- TESTING SMOOTH BLEND WITH START POSITIONS ---") - - # Test 1: Blend with default start - print("\n[1/4] Blend with DEFAULT start") - p1 = start_pose - p2 = [p1[0] + 25, p1[1] + 10, p1[2], p1[3], p1[4], p1[5] + 20] - arc_center = [p2[0] - 10, p2[1] + 10, p2[2]] - p3 = [arc_center[0], arc_center[1] + 15, arc_center[2], p1[3], p1[4], p1[5] + 40] - - segments = [ - {'type': 'LINE', 'end': p2, 'duration': 2.0}, - {'type': 'ARC', 'end': p3, 'center': arc_center, 'duration': 3.0, 'clockwise': False}, - ] - - result = robot_api.smooth_blend( - segments=segments, - blend_time=0.5, - # No start_pose - uses current - duration=6.0, - wait_for_ack=True, - timeout=15 - ) - print(f"--> Command result: {result}") - robot_api.wait_for_robot_stopped(timeout=15) - time.sleep(2) - - # Re-initialize - current_pose = initialize_test_position() - if not current_pose: return - - # Test 2: Blend with specified start for first segment - print("\n[2/4] Blend with SPECIFIED start (adds transition)") - specified_start = [current_pose[0] + 15, current_pose[1] - 10, current_pose[2], - current_pose[3], current_pose[4], current_pose[5]] - p2 = [specified_start[0] + 20, specified_start[1] + 15, specified_start[2], - specified_start[3], specified_start[4], specified_start[5] + 30] - circle_center = [p2[0], p2[1] + 20, p2[2]] - - segments = [ - {'type': 'LINE', 'end': p2, 'duration': 2.5}, - {'type': 'CIRCLE', 'center': circle_center, 'radius': 20, 'plane': 'XY', - 'duration': 4.0, 'clockwise': True}, - ] - - print(f" Current: {[round(p, 1) for p in current_pose[:3]]}") - print(f" Will transition to: {[round(p, 1) for p in specified_start[:3]]}") - print(f" Then execute blend segments") - - result = robot_api.smooth_blend( - segments=segments, - blend_time=0.75, - start_pose=specified_start, # First segment starts here - speed_percentage=35, - wait_for_ack=True, - timeout=20 - ) - print(f"--> Command result: {result}") - robot_api.wait_for_robot_stopped(timeout=20) - time.sleep(2) - - # Re-initialize - current_pose = initialize_test_position() - if not current_pose: return - - # Test 3: Complex blend with spline segment and specified start - print("\n[3/4] Complex blend with SPLINE segment and specified start") - blend_start = [current_pose[0] - 10, current_pose[1] + 10, current_pose[2], - current_pose[3], current_pose[4], current_pose[5]] - - # Define waypoints for spline segment - spline_waypoints = [ - [blend_start[0] + 30, blend_start[1], blend_start[2], - blend_start[3], blend_start[4], blend_start[5]], - [blend_start[0] + 35, blend_start[1] + 15, blend_start[2], - blend_start[3], blend_start[4], blend_start[5] + 15], - [blend_start[0] + 25, blend_start[1] + 25, blend_start[2], - blend_start[3], blend_start[4], blend_start[5] + 30] - ] - - segments = [ - {'type': 'LINE', 'end': spline_waypoints[0], 'duration': 2.0}, - {'type': 'SPLINE', 'waypoints': spline_waypoints, 'duration': 4.0}, - {'type': 'LINE', 'end': [blend_start[0], blend_start[1] + 20, blend_start[2], - blend_start[3], blend_start[4], blend_start[5]], - 'duration': 2.0} - ] - - result = robot_api.smooth_blend( - segments=segments, - blend_time=0.5, - start_pose=blend_start, - duration=10.0, # Overall duration - wait_for_ack=True, - timeout=20 - ) - print(f"--> Command result: {result}") - robot_api.wait_for_robot_stopped(timeout=20) - - # Test 4: Blend in TRF - all segments relative to tool - print("\n[4/4] Blend in TOOL REFERENCE FRAME (TRF)") - # All segment coordinates are relative to tool position/orientation - trf_segments = [ - {'type': 'LINE', 'end': [30, 0, 0, 0, 0, 0], 'duration': 2.0}, - {'type': 'CIRCLE', 'center': [30, 20, 0], 'radius': 20, 'plane': 'XY', - 'duration': 4.0, 'clockwise': False}, # Tool's XY plane - {'type': 'LINE', 'end': [0, 20, 0, 0, 0, 0], 'duration': 2.0} - ] - - print(f" TRF Blend - all segments in tool coordinate system") - print(f" Circle plane is tool's XY, not world XY!") - - result = robot_api.smooth_blend( - segments=trf_segments, - blend_time=0.5, - frame='TRF', # NEW: All segments in tool frame - duration=10.0, - wait_for_ack=True, - timeout=20 - ) - print(f"--> Command result: {result}") - robot_api.wait_for_robot_stopped(timeout=20) - -def test_transition_distances(): - """Test transitions with various distances to verify smooth transition behavior.""" - print("\n--- TESTING TRANSITION DISTANCES ---") - - # Get current position - start_pose = initialize_test_position() - if not start_pose: return - - # Define test distances: very close, medium, far - test_cases = [ - ("Very Close (3mm)", 3), - ("Close (10mm)", 10), - ("Medium (30mm)", 30), - ("Far (50mm)", 50) - ] - - for description, distance in test_cases: - print(f"\n[{test_cases.index((description, distance)) + 1}/{len(test_cases)}] Testing transition: {description}") - - # Create a start position at the specified distance - transition_start = [ - start_pose[0] + distance, - start_pose[1], - start_pose[2], - start_pose[3], start_pose[4], start_pose[5] - ] - - # Use a simple circle to observe the transition - circle_center = [transition_start[0], transition_start[1] + 30, transition_start[2]] - - print(f" Current position: {[round(p, 1) for p in start_pose[:3]]}") - print(f" Transition to: {[round(p, 1) for p in transition_start[:3]]}") - print(f" Distance: {distance}mm") - - start_time = time.time() - result = robot_api.smooth_circle( - center=circle_center, - radius=30.0, - plane='XY', - start_pose=transition_start, - duration=5.0, - clockwise=False, - wait_for_ack=True - ) - - # Note the transition time - robot_api.wait_for_robot_stopped(timeout=10) - total_time = time.time() - start_time - - print(f" Total execution time: {total_time:.2f}s") - if distance <= 5: - print(f" -> Minimal transition expected and observed") - else: - transition_time = distance / 30.0 # Assuming 30mm/s transition speed - print(f" -> Estimated transition time: {transition_time:.2f}s") - - time.sleep(2) - - # Return to start for next test - if test_cases.index((description, distance)) < len(test_cases) - 1: - initialize_test_position() - - # Additional test: Transition in TRF - print("\n[BONUS] Testing transition in TRF") - print("In TRF, transition is relative to tool, not world") - - # TRF start position (30mm forward in tool X) - trf_transition_start = [30, 0, 0, 0, 0, 0] - trf_circle_center = [30, 30, 0] # Center in tool frame - - result = robot_api.smooth_circle( - center=trf_circle_center, - radius=30.0, - plane='XY', # Tool's XY plane - frame='TRF', # NEW: Transition happens in tool space - start_pose=trf_transition_start, - duration=5.0, - clockwise=False, - wait_for_ack=True - ) - print(f" -> TRF transition completed") - robot_api.wait_for_robot_stopped(timeout=10) - -def test_timing_comparison_with_starts(): - """Compare timing modes with specified start positions.""" - print("\n--- TESTING TIMING MODES WITH START POSITIONS ---") - - # Initialize - start_pose = initialize_test_position() - if not start_pose: return - - # Define a specific start position for both tests - test_start = [start_pose[0] + 20, start_pose[1] - 10, start_pose[2], - start_pose[3], start_pose[4], start_pose[5]] - center = [test_start[0], test_start[1] + 30, test_start[2]] - radius = 30.0 - - print("\n[1/3] Circle with specified start + 5-second DURATION") - print(f" Transition from: {[round(p, 1) for p in start_pose[:3]]}") - print(f" To start position: {[round(p, 1) for p in test_start[:3]]}") - - start_time = time.time() - result = robot_api.smooth_circle( - center=center, - radius=radius, - plane='XY', - start_pose=test_start, - duration=5.0, - clockwise=False, - wait_for_ack=True - ) - robot_api.wait_for_robot_stopped(timeout=12) - elapsed = time.time() - start_time - print(f"--> Total execution time (including transition): {elapsed:.2f}s") - time.sleep(2) - - # Return to start - initialize_test_position() - - print("\n[2/3] Same circle with specified start + 40% SPEED") - print(f" Same transition and circle path") - - start_time = time.time() - result = robot_api.smooth_circle( - center=center, - radius=radius, - plane='XY', - start_pose=test_start, - speed_percentage=40, - clockwise=False, - wait_for_ack=True - ) - robot_api.wait_for_robot_stopped(timeout=12) - elapsed = time.time() - start_time - print(f"--> Total execution time (including transition): {elapsed:.2f}s") - - print("\n[3/3] Same circle in TRF with 40% SPEED") - print(f" Testing how TRF affects timing with transitions") - - # TRF coordinates (relative to tool) - trf_start = [20, -10, 0, 0, 0, 0] - trf_center = [20, 20, 0] # 30mm forward in tool Y from start - - start_time = time.time() - result = robot_api.smooth_circle( - center=trf_center, - radius=30.0, - plane='XY', # Tool's XY plane - frame='TRF', # NEW: Using tool reference frame - start_pose=trf_start, - speed_percentage=40, - clockwise=False, - wait_for_ack=True - ) - robot_api.wait_for_robot_stopped(timeout=12) - elapsed = time.time() - start_time - print(f"--> TRF execution time: {elapsed:.2f}s") - print(f" Note: TRF doesn't change timing, just coordinate interpretation") - - # Calculate expected times - circumference = 2 * np.pi * radius - transition_dist = np.sqrt((test_start[0] - start_pose[0])**2 + - (test_start[1] - start_pose[1])**2 + - (test_start[2] - start_pose[2])**2) - print(f"\nAnalysis:") - print(f" Transition distance: {transition_dist:.1f}mm") - print(f" Circle circumference: {circumference:.1f}mm") - print(f" At 40% speed (~40mm/s), circle should take ~{circumference/40:.1f}s") - print(f" Transition at ~30mm/s should take ~{transition_dist/30:.1f}s") - -if __name__ == "__main__": - print("="*70) - print("COMPREHENSIVE SMOOTH MOTION TEST SUITE") - print("Testing NEW features: Start Positions & Automatic Transitions") - print("="*70) - - - # Test 1: Basic tests with new start position feature - print("\n[TEST GROUP 1: BASIC COMMANDS WITH START POSITIONS]") - start_pose = initialize_test_position() - if start_pose: - test_smooth_circle_basic(start_pose) - - # Test 2: Arc commands with various start positions - print("\n[TEST GROUP 2: ARC COMMANDS WITH TRANSITIONS]") - start_pose = initialize_test_position() - if start_pose: - test_smooth_arc_with_start_positions(start_pose) - - # Test 3: Motion chaining - using end of one as start of next - print("\n[TEST GROUP 3: PRECISE MOTION CHAINING]") - start_pose = initialize_test_position() - if start_pose: - test_motion_chaining(start_pose) - - # Test 4: Spline with various start scenarios - print("\n[TEST GROUP 4: SPLINE WITH START POSITIONS]") - start_pose = initialize_test_position() - if start_pose: - test_smooth_spline_with_starts(start_pose) - - # Test 5: Helix with start positions - print("\n[TEST GROUP 5: HELIX WITH START POSITIONS]") - start_pose = initialize_test_position() - if start_pose: - test_smooth_helix_with_starts(start_pose) - - # Test 6: Blend with start positions - print("\n[TEST GROUP 6: BLEND WITH START POSITIONS]") - start_pose = initialize_test_position() - if start_pose: - test_smooth_blend_with_starts(start_pose) - - # Test 7: Transition distance testing - print("\n[TEST GROUP 7: TRANSITION DISTANCE BEHAVIOR]") - test_transition_distances() - - # Test 8: Timing comparison with transitions - print("\n[TEST GROUP 8: TIMING MODES WITH TRANSITIONS]") - test_timing_comparison_with_starts() - - print("\n" + "="*70) - print("COMPREHENSIVE TEST SUITE COMPLETE") - print("Tested features:") - print(" ✓ All commands with duration mode") - print(" ✓ All commands with speed percentage mode") - print(" ✓ Default start positions (current position)") - print(" ✓ Specified start positions with automatic transitions") - print(" ✓ Motion chaining with precise continuity") - print(" ✓ Transition behavior for various distances") - print(" ✓ Blend segments with overall timing control") - print("="*70) - - # Final return to safe position - print("\nReturning to safe position...") - initialize_test_position() - print("\n===== All Tests Finished =====") \ No newline at end of file diff --git a/tests/__init__.py b/tests/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/tests/conftest.py b/tests/conftest.py new file mode 100644 index 0000000..e0f689b --- /dev/null +++ b/tests/conftest.py @@ -0,0 +1,452 @@ +""" +Pytest configuration and shared fixtures for PAROL6 Python API tests. + +Provides command line options, fixtures for port management, server process management, +environment configuration, and test utilities used across the test suite. +""" + +import os +import sys +import pytest +import time +from typing import Generator, Dict, Optional +from dataclasses import dataclass +import logging + +# Add the parent directory to Python path so we can import the API modules +sys.path.insert(0, os.path.dirname(os.path.dirname(__file__))) + +# Import parol6 for server management +from parol6.server.manager import ServerManager + +# Import utilities for port detection +def find_available_ports(start_port: int = 5001, count: int = 2): + """Simple fallback port finder if utils import fails.""" + import socket + available_ports = [] + current_port = start_port + + while len(available_ports) < count: + try: + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: + sock.bind(('127.0.0.1', current_port)) + available_ports.append(current_port) + except OSError: + # Port in use, reset search if we were building a consecutive sequence + available_ports.clear() + + current_port += 1 + + # Prevent infinite loop + if current_port > start_port + 1000: + break + + return available_ports + +logger = logging.getLogger(__name__) + + +@dataclass +class TestPorts: + """Configuration for test server ports.""" + server_ip: str = "127.0.0.1" + server_port: int = 5001 + ack_port: int = 5002 + + +# ============================================================================ +# PYTEST COMMAND LINE OPTIONS +# ============================================================================ + +def pytest_addoption(parser): + """Add custom command line options for the test suite.""" + parser.addoption( + "--run-hardware", + action="store_true", + default=False, + help="Enable hardware tests that require actual robot hardware and human confirmation" + ) + parser.addoption( + "--server-ip", + action="store", + default="127.0.0.1", + help="IP address for test server communication" + ) + parser.addoption( + "--server-port", + action="store", + type=int, + default=None, + help="Port for robot server communication (auto-detected if not specified)" + ) + parser.addoption( + "--ack-port", + action="store", + type=int, + default=None, + help="Port for acknowledgment communication (auto-detected if not specified)" + ) + parser.addoption( + "--keep-server-running", + action="store_true", + default=False, + help="Keep the test server running between test sessions for debugging" + ) + + +# ============================================================================ +# PORT MANAGEMENT FIXTURE +# ============================================================================ + +@pytest.fixture(scope="session") +def ports(request) -> TestPorts: + """ + Provide test port configuration. + + Automatically finds available ports if not specified via command line. + Ensures ports don't conflict with existing services. + """ + server_ip = request.config.getoption("--server-ip") + server_port = request.config.getoption("--server-port") + ack_port = request.config.getoption("--ack-port") + + # Auto-detect available ports if not specified + if server_port is None or ack_port is None: + logger.info("Auto-detecting available ports...") + available_ports = find_available_ports(start_port=5001, count=2) + + if len(available_ports) < 2: + pytest.fail("Could not find 2 consecutive available ports for testing") + + if server_port is None: + server_port = available_ports[0] + if ack_port is None: + ack_port = available_ports[1] + + logger.info(f"Using auto-detected ports: server={server_port}, ack={ack_port}") + + return TestPorts( + server_ip=server_ip, + server_port=server_port, + ack_port=ack_port + ) + + +# ============================================================================ +# ENVIRONMENT CONFIGURATION FIXTURE +# ============================================================================ + +@pytest.fixture(scope="session") +def robot_api_env(ports: TestPorts) -> Generator[Dict[str, str], None, None]: + """ + Configure environment variables for robot_api client to use test ports. + + Sets environment variables so that robot_api.py will connect to the test + server instead of the default production server. + """ + # Store original environment values + original_env = {} + env_vars = { + "PAROL6_SERVER_IP": ports.server_ip, + "PAROL6_SERVER_PORT": str(ports.server_port), + "PAROL6_ACK_PORT": str(ports.ack_port), + } + + for key, value in env_vars.items(): + original_env[key] = os.environ.get(key) + os.environ[key] = value + + logger.debug(f"Set test environment: {env_vars}") + + try: + yield env_vars + finally: + # Restore original environment + for key, original_value in original_env.items(): + if original_value is None: + os.environ.pop(key, None) + else: + os.environ[key] = original_value + logger.debug("Restored original environment") + + +# ============================================================================ +# SERVER PROCESS FIXTURE +# ============================================================================ + +@pytest.fixture(scope="session") +def server_proc(request, ports: TestPorts, robot_api_env): + """ + Launch parol6 server for integration tests using ServerManager. + + Starts the server with FAKE_SERIAL mode and waits for readiness. + Automatically cleans up the server when tests complete. + """ + import asyncio + import socket + + keep_running = request.config.getoption("--keep-server-running") + + # Create server manager + manager = ServerManager() + + async def start_and_wait(): + # Start the controller process + await manager.start_controller( + no_autohome=True, + extra_env={ + "PAROL6_FAKE_SERIAL": "1", + "PAROL6_NOAUTOHOME": "1", + "PAROL6_SERVER_IP": ports.server_ip, + "PAROL6_SERVER_PORT": str(ports.server_port), + "PAROL6_ACK_PORT": str(ports.ack_port), + } + ) + + # Wait for server to be ready with custom ping logic + timeout = 10.0 + start_time = time.time() + + while time.time() - start_time < timeout: + try: + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: + sock.settimeout(1.0) + sock.sendto(b"PING", (ports.server_ip, ports.server_port)) + data, _ = sock.recvfrom(256) + if data.decode('utf-8').strip() == "PONG": + return True + except (socket.timeout, Exception): + pass + await asyncio.sleep(0.5) + return False + + # Start server using parol6's ServerManager + logger.info(f"Starting test server on {ports.server_ip}:{ports.server_port}") + + ready = asyncio.run(start_and_wait()) + if not ready: + pytest.fail("Failed to start headless commander server for testing") + + try: + # Wait a bit for full initialization + time.sleep(1.0) + yield manager + + finally: + if not keep_running: + logger.info("Stopping test server") + async def stop_server(): + await manager.stop_controller() + asyncio.run(stop_server()) + else: + logger.info("Leaving test server running (--keep-server-running)") + + +# ============================================================================ +# HARDWARE TEST SUPPORT +# ============================================================================ + +@pytest.fixture +def human_prompt(request): + """ + Provide human confirmation prompts for hardware tests. + + Automatically skips tests marked with @pytest.mark.hardware unless + --run-hardware is specified. For enabled hardware tests, provides + a utility function to prompt for human confirmation. + """ + # Check if hardware tests are enabled + run_hardware = request.config.getoption("--run-hardware") + + # Skip hardware tests if not enabled + if request.node.get_closest_marker("hardware") and not run_hardware: + pytest.skip("Hardware tests disabled. Use --run-hardware to enable.") + + def prompt_user(message: str, timeout: Optional[float] = None) -> bool: + """ + Prompt user for confirmation during hardware tests. + + Args: + message: Message to display to user + timeout: Optional timeout in seconds + + Returns: + True if user confirms, False otherwise + """ + if not run_hardware: + return False + + print(f"\n{'='*60}") + print("HARDWARE TEST CONFIRMATION REQUIRED") + print(f"{'='*60}") + print(f"{message}") + print(f"{'='*60}") + + try: + if timeout: + import signal + def timeout_handler(signum, frame): + raise TimeoutError("User confirmation timeout") + signal.signal(signal.SIGALRM, timeout_handler) + signal.alarm(int(timeout)) + + response = input("Continue? [y/N]: ").strip().lower() + + if timeout: + signal.alarm(0) # Cancel timeout + + return response in ['y', 'yes'] + + except (KeyboardInterrupt, TimeoutError): + print("\nUser confirmation cancelled or timed out") + return False + except Exception as e: + print(f"\nError getting user confirmation: {e}") + return False + + return prompt_user + + +# ============================================================================ +# COMMON TEST UTILITIES +# ============================================================================ + +@pytest.fixture +def temp_env(): + """ + Provide temporary environment variable context manager. + + Useful for tests that need to modify environment variables temporarily. + """ + class TempEnv: + def __init__(self): + self.original = {} + + def set(self, key: str, value: str): + """Set an environment variable temporarily.""" + if key not in self.original: + self.original[key] = os.environ.get(key) + os.environ[key] = value + + def restore(self): + """Restore all modified environment variables.""" + for key, original_value in self.original.items(): + if original_value is None: + os.environ.pop(key, None) + else: + os.environ[key] = original_value + self.original.clear() + + temp = TempEnv() + try: + yield temp + finally: + temp.restore() + + +@pytest.fixture +def mock_time(monkeypatch): + """ + Provide controllable time mocking for tests that depend on timing. + + Useful for testing timeout behavior without actually waiting. + """ + import time + + class MockTime: + def __init__(self): + self.current_time = time.time() + + def time(self): + return self.current_time + + def advance(self, seconds: float): + """Advance the mock time by the specified number of seconds.""" + self.current_time += seconds + + def sleep(self, seconds: float): + """Mock sleep that just advances time.""" + self.advance(seconds) + + mock = MockTime() + monkeypatch.setattr(time, 'time', mock.time) + monkeypatch.setattr(time, 'sleep', mock.sleep) + return mock + + +# ============================================================================ +# PYTEST CONFIGURATION HOOKS +# ============================================================================ + +def pytest_configure(config): + """Configure pytest with custom markers.""" + config.addinivalue_line( + "markers", "unit: Unit tests that test individual components in isolation" + ) + config.addinivalue_line( + "markers", "integration: Integration tests that test component interactions with FAKE_SERIAL" + ) + config.addinivalue_line( + "markers", "hardware: Hardware tests that require actual robot hardware and human confirmation" + ) + config.addinivalue_line( + "markers", "slow: Slow-running tests (typically hardware or complex integration tests)" + ) + config.addinivalue_line( + "markers", "e2e: End-to-end tests that exercise complete workflows" + ) + config.addinivalue_line( + "markers", "gcode: Tests specifically for GCODE parsing and interpretation functionality" + ) + + +def pytest_collection_modifyitems(config, items): + """Modify test collection to add markers and skip conditions.""" + # Skip hardware tests by default unless --run-hardware is specified + if not config.getoption("--run-hardware"): + skip_hardware = pytest.mark.skip(reason="Hardware tests disabled (use --run-hardware to enable)") + for item in items: + if item.get_closest_marker("hardware"): + item.add_marker(skip_hardware) + + +def pytest_sessionstart(session): + """Called after the Session object has been created.""" + logger.info("Starting PAROL6 Python API test session") + + # Print test configuration info + config = session.config + logger.info(f"Hardware tests: {'enabled' if config.getoption('--run-hardware') else 'disabled'}") + logger.info(f"Server IP: {config.getoption('--server-ip')}") + + server_port = config.getoption('--server-port') + ack_port = config.getoption('--ack-port') + if server_port and ack_port: + logger.info(f"Server ports: {server_port}/{ack_port}") + else: + logger.info("Server ports: auto-detect") + + +# ============================================================================ +# CLIENT FIXTURE +# ============================================================================ + +@pytest.fixture +def client(ports: TestPorts): + """ + Provide a RobotClient configured for the test ports. + + This ensures all tests use the same connection configuration + and connect to the auto-detected test server ports. + """ + from parol6 import RobotClient + return RobotClient( + host=ports.server_ip, + port=ports.server_port, + ack_port=ports.ack_port + ) + + +def pytest_sessionfinish(session, exitstatus): + """Called after whole test run finished.""" + logger.info(f"PAROL6 Python API test session finished with exit status: {exitstatus}") diff --git a/tests/hardware/__init__.py b/tests/hardware/__init__.py new file mode 100644 index 0000000..81d7dc6 --- /dev/null +++ b/tests/hardware/__init__.py @@ -0,0 +1,7 @@ +""" +Hardware tests package. + +Contains tests that require actual robot hardware and human confirmation. +These tests are marked with @pytest.mark.hardware and are only executed +when the --run-hardware flag is provided. +""" diff --git a/tests/hardware/test_manual_moves.py b/tests/hardware/test_manual_moves.py new file mode 100644 index 0000000..730ed62 --- /dev/null +++ b/tests/hardware/test_manual_moves.py @@ -0,0 +1,724 @@ +""" +Hardware tests for manual robot movements. + +These tests require actual robot hardware and human confirmation. +They are marked with @pytest.mark.hardware and require the --run-hardware flag. + +SAFETY NOTICE: These tests will move the physical robot. Ensure the robot +workspace is clear and E-stop is within reach before running. +""" + +import pytest +import sys +import os +import time +import numpy as np +from typing import List, Optional + +# Add the parent directory to Python path +sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..', '..')) + +# Removed direct robot_api import - using client fixture from conftest.py + +# Define the safe starting joint configuration for all hardware tests +# This ensures consistency and repeatability for each test +# Angles: [J1, J2, J3, J4, J5, J6] in degrees +SAFE_SMOOTH_START_JOINTS = [42.697, -89.381, 144.831, -0.436, 31.528, 180.0] + + +def initialize_hardware_position(client, human_prompt) -> Optional[List[float]]: + """ + Moves the robot to the predefined safe starting joint angles. + + Args: + client: RobotClient fixture from conftest.py + human_prompt: Fixture for human confirmation + + Returns: + Robot's Cartesian pose after moving, or None if failed + """ + + print(f"Moving to safe starting position: {SAFE_SMOOTH_START_JOINTS}") + + # Move to the joint position + result = client.move_joints( + SAFE_SMOOTH_START_JOINTS, + duration=4, + wait_for_ack=True, + timeout=15 + ) + print(f"Move command result: {result}") + + # Wait until robot stops + if client.wait_until_stopped(timeout=15): + print("Robot has reached the starting position.") + time.sleep(1) + start_pose = client.get_pose_rpy() # Get [x,y,z,rx,ry,rz] format + if start_pose: + print(f"Starting pose: {[round(p, 2) for p in start_pose]}") + return start_pose + else: + print("ERROR: Could not retrieve robot pose after moving.") + return None + else: + print("ERROR: Robot did not stop in time.") + return None + + +@pytest.mark.hardware +@pytest.mark.slow +class TestHardwareBasicMoves: + """Test basic robot movements with hardware.""" + + def test_hardware_homing(self, client, human_prompt): + """Test robot homing sequence.""" + if not human_prompt( + "Ready to test robot homing?\n" + "This will home all joints to their reference positions.\n" + "Ensure robot workspace is completely clear." + ): + pytest.skip("User declined homing test") + + # Check E-stop status first + if client.is_estop_pressed(): + pytest.fail("E-stop is pressed. Release E-stop before testing.") + + print("Starting homing sequence...") + result = client.home(wait_for_ack=True, timeout=60) + + assert isinstance(result, dict) + assert result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING'] + + # Wait for homing to complete - use client's built-in wait + if client.wait_until_stopped(timeout=90, show_progress=True): + print("Homing completed successfully") + else: + pytest.fail("Robot homing did not complete within timeout") + + def test_small_joint_movements(self, client, human_prompt): + """Test small joint movements for safety verification.""" + start_pose = initialize_hardware_position(client, human_prompt) + if not start_pose: + pytest.skip("Failed to reach starting position") + + if not human_prompt( + "Ready to test small joint movements?\n" + "Robot will move each joint individually by small amounts.\n" + "Watch for smooth, controlled motion." + ): + pytest.skip("User declined joint movement test") + + # Test small movements on each joint + for joint_idx in range(6): + print(f"Testing joint {joint_idx + 1} movement...") + + # Small positive movement + result = client.jog_joint( + joint_idx, + speed_percentage=20, + duration=1.0, + wait_for_ack=True + ) + + assert isinstance(result, dict) + assert result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING'] + + client.wait_until_stopped(timeout=5) + + # Small negative movement (return) - use joint_idx+6 for reverse direction + result = client.jog_joint( + joint_idx + 6, # +6 indicates reverse direction + speed_percentage=20, + duration=1.0, + wait_for_ack=True + ) + + client.wait_until_stopped(timeout=5) + + print("All joint movements completed successfully") + + def test_small_cartesian_movements(self, client, human_prompt): + """Test small Cartesian movements in different axes.""" + start_pose = initialize_hardware_position(client, human_prompt) + if not start_pose: + pytest.skip("Failed to reach starting position") + + if not human_prompt( + "Ready to test small Cartesian movements?\n" + "Robot will move end-effector in X, Y, Z directions.\n" + "Movements will be small (10mm) and slow (20% speed)." + ): + pytest.skip("User declined Cartesian movement test") + + # Test movements in each axis + axes = ['X+', 'X-', 'Y+', 'Y-', 'Z+', 'Z-'] + + for axis in axes: + print(f"Testing Cartesian jog in {axis} direction...") + + result = client.jog_cartesian( + frame='WRF', + axis=axis, + speed_percentage=20, + duration=1.0, + wait_for_ack=True + ) + + assert isinstance(result, dict) + assert result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING'] + + time.sleep(2.0) + client.wait_until_stopped(timeout=5) + + print("All Cartesian movements completed successfully") + + +@pytest.mark.hardware +@pytest.mark.slow +class TestHardwareSmoothMotion: + """Test smooth motion commands with actual hardware.""" + + def test_hardware_smooth_circle(self, client, human_prompt): + """Test smooth circular motion on hardware.""" + start_pose = initialize_hardware_position(client, human_prompt) + if not start_pose: + pytest.skip("Failed to reach starting position") + + if not human_prompt( + "Ready to test smooth circular motion?\n" + "Robot will execute a 30mm radius circle in XY plane.\n" + "Motion will be slow and controlled (5 second duration).\n" + "Watch for smooth, continuous motion without stops." + ): + pytest.skip("User declined circle test") + + # Execute smooth circle + center_point = [start_pose[0], start_pose[1] + 30, start_pose[2]] + + print(f"Executing circle: center={center_point}, radius=30mm") + result = client.smooth_circle( + center=center_point, + radius=30.0, + plane='XY', + duration=5.0, + clockwise=False, + wait_for_ack=True, + timeout=15 + ) + + assert isinstance(result, dict) + assert result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING'] + + # Wait for completion + if client.wait_until_stopped(timeout=15): + print("Circle motion completed successfully") + + if not human_prompt( + "Did the robot execute a smooth circular motion?\n" + "Motion should have been continuous without stops or jerks." + ): + pytest.fail("User reported motion was not smooth") + else: + pytest.fail("Robot did not stop after circle motion timeout") + + def test_hardware_smooth_arc(self, client, human_prompt): + """Test smooth arc motion on hardware.""" + start_pose = initialize_hardware_position(client, human_prompt) + if not start_pose: + pytest.skip("Failed to reach starting position") + + if not human_prompt( + "Ready to test smooth arc motion?\n" + "Robot will execute an arc from current position to a new pose.\n" + "Motion will be controlled and smooth." + ): + pytest.skip("User declined arc test") + + # Define arc motion + end_pose = [start_pose[0] + 40, start_pose[1] + 20, start_pose[2], + start_pose[3], start_pose[4], start_pose[5] + 45] + center = [start_pose[0] + 20, start_pose[1], start_pose[2]] + + print(f"Executing arc: end={end_pose[:3]}, center={center}") + result = client.smooth_arc_center( + end_pose=end_pose, + center=center, + duration=4.0, + clockwise=True, + wait_for_ack=True, + timeout=12 + ) + + assert isinstance(result, dict) + assert result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING'] + + if client.wait_until_stopped(timeout=12): + print("Arc motion completed successfully") + + if not human_prompt( + "Did the robot execute a smooth arc motion?\n" + "Path should have been curved, not straight lines." + ): + pytest.fail("User reported arc motion was not smooth") + else: + pytest.fail("Robot did not stop after arc motion timeout") + + def test_hardware_smooth_spline(self, client, human_prompt): + """Test smooth spline motion through multiple waypoints.""" + start_pose = initialize_hardware_position(client, human_prompt) + if not start_pose: + pytest.skip("Failed to reach starting position") + + if not human_prompt( + "Ready to test smooth spline motion?\n" + "Robot will move through multiple waypoints with smooth transitions.\n" + "Motion should be continuous without stops at waypoints." + ): + pytest.skip("User declined spline test") + + # Define spline waypoints + waypoints = [ + [start_pose[0] + 20, start_pose[1] + 10, start_pose[2], + start_pose[3], start_pose[4], start_pose[5]], + [start_pose[0] + 35, start_pose[1] + 25, start_pose[2] + 10, + start_pose[3], start_pose[4], start_pose[5] + 20], + [start_pose[0] + 20, start_pose[1] + 35, start_pose[2], + start_pose[3], start_pose[4], start_pose[5] + 40], + [start_pose[0] + 5, start_pose[1] + 20, start_pose[2], + start_pose[3], start_pose[4], start_pose[5]] + ] + + print(f"Executing spline through {len(waypoints)} waypoints") + result = client.smooth_spline( + waypoints=waypoints, + duration=6.0, + frame='WRF', + wait_for_ack=True, + timeout=20 + ) + + assert isinstance(result, dict) + assert result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING'] + + if client.wait_until_stopped(timeout=20): + print("Spline motion completed successfully") + + if not human_prompt( + "Did the robot move smoothly through all waypoints?\n" + "Motion should have been continuous without stops at intermediate points." + ): + pytest.fail("User reported spline motion was not smooth") + else: + pytest.fail("Robot did not stop after spline motion timeout") + + +@pytest.mark.hardware +@pytest.mark.slow +class TestHardwareAdvancedSmooth: + """Test advanced smooth motion features with hardware.""" + + def test_hardware_helix_motion(self, client, human_prompt): + """Test helical motion on hardware.""" + start_pose = initialize_hardware_position(client, human_prompt) + if not start_pose: + pytest.skip("Failed to reach starting position") + + if not human_prompt( + "Ready to test helical motion?\n" + "Robot will execute a helical (screw-like) motion.\n" + "Motion combines circular movement with vertical progression.\n" + "Radius: 25mm, Height: 40mm, 3 revolutions." + ): + pytest.skip("User declined helix test") + + # Execute helix motion + center = [start_pose[0], start_pose[1] + 25, start_pose[2] - 20] + + print(f"Executing helix: center={center}, radius=25mm, height=40mm") + result = client.smooth_helix( + center=center, + radius=25.0, + pitch=13.0, # 40mm / ~3 revolutions + height=40.0, + duration=8.0, + clockwise=False, + wait_for_ack=True, + timeout=20 + ) + + assert isinstance(result, dict) + assert result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING'] + + if client.wait_until_stopped(timeout=20): + print("Helix motion completed successfully") + + if not human_prompt( + "Did the robot execute a smooth helical motion?\n" + ): + pytest.fail("User reported helix motion was incorrect") + else: + pytest.fail("Robot did not stop after helix motion timeout") + + def test_hardware_reference_frame_comparison(self, client, human_prompt): + """Test motion in different reference frames (WRF vs TRF).""" + start_pose = initialize_hardware_position(client, human_prompt) + if not start_pose: + pytest.skip("Failed to reach starting position") + + if not human_prompt( + "Ready to test reference frame comparison?\n" + "Robot will execute similar motions in World frame (WRF) and Tool frame (TRF).\n" + "You should observe different motion patterns." + ): + pytest.skip("User declined reference frame test") + + # Test 1: Circle in World Reference Frame + print("Executing circle in World Reference Frame (WRF)...") + result_wrf = client.smooth_circle( + center=[start_pose[0], start_pose[1] + 30, start_pose[2]], + radius=20, + duration=4.0, + frame='WRF', + plane='XY', + wait_for_ack=True, + timeout=12 + ) + + assert isinstance(result_wrf, dict) + assert result_wrf.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING'] + + client.wait_until_stopped(timeout=12) + time.sleep(2) + + # Test 2: Circle in Tool Reference Frame + print("Executing circle in Tool Reference Frame (TRF)...") + result_trf = client.smooth_circle( + center=[0, 30, 0], # Relative to tool position + radius=20, + duration=4.0, + frame='TRF', + plane='XY', # Tool's XY plane + wait_for_ack=True, + timeout=12 + ) + + assert isinstance(result_trf, dict) + assert result_trf.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING'] + + client.wait_until_stopped(timeout=12) + + if not human_prompt( + "Did you observe different motion patterns?\n" + "WRF motion should follow world coordinates.\n" + "TRF motion should follow tool orientation." + ): + pytest.fail("User reported motion patterns were not as expected") + + +@pytest.mark.hardware +@pytest.mark.slow +class TestHardwareSafety: + """Test hardware safety features and error conditions.""" + + def test_joint_limit_safety(self, client, human_prompt): + """Test joint limit safety (if supported by controller).""" + if not human_prompt( + "Ready to test joint limit safety?\n" + "This will attempt to move a joint near its limit to test safety systems.\n" + "Controller should prevent unsafe movements.\n" + "This test is informational only." + ): + pytest.skip("User declined joint limit test") + + # Try to move to a potentially extreme position (should be rejected or limited) + extreme_joints = [180.0, -180.0, 180.0, -180.0, 180.0, -180.0] # Extreme angles as floats + + print("Testing extreme joint angles (should be rejected or limited)...") + result = client.move_joints( + extreme_joints, + speed_percentage=5, # Very slow for safety + wait_for_ack=True, + timeout=10 + ) + + print(f"Result of extreme joint command: {result}") + + # This test is informational - we just want to see how the system responds + time.sleep(5.0) + + # Return to safe position + initialize_hardware_position(client, human_prompt) + + +@pytest.mark.hardware +@pytest.mark.slow +class TestHardwareLegacySequence: + """Test the exact sequence from the legacy test_script.py for verified safe operation.""" + + def test_legacy_script_safe_sequence(self, client, human_prompt): + """ + Reproduce the exact sequence from test_script.py with verified safe waypoints. + + This test uses the exact same joint angles, poses, and parameters that were + manually verified to be safe in the original test script. + """ + if not human_prompt( + "Ready to execute the legacy safe test sequence?\n" + "This will reproduce the exact movements from test_script.py:\n" + "- Electric gripper calibration and moves (pos 100, then 200)\n" + "- Pneumatic gripper open/close sequence\n" + "- Joint moves: [90,-90,160,12,12,180] -> [50,-60,180,-12,32,0] -> back\n" + "- Pose move: [7,250,200,-100,0,-90]\n" + "- Cartesian move: [7,250,150,-100,0,-90]\n" + "These waypoints were verified safe in the original script." + ): + pytest.skip("User declined legacy sequence test") + + # Check E-stop status first + if client.is_estop_pressed(): + pytest.fail("E-stop is pressed. Release E-stop before testing.") + + print("Starting legacy test sequence with verified safe waypoints...") + + # Electric gripper calibration and moves + print("Calibrating electric gripper...") + result = client.control_electric_gripper( + action="calibrate", + wait_for_ack=True, + timeout=10 + ) + if isinstance(result, dict): + assert result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING'] + time.sleep(2) + + print("Moving electric gripper to position 100...") + result = client.control_electric_gripper( + action='move', + position=100, + speed=150, + current=200, + wait_for_ack=True, + timeout=10 + ) + if isinstance(result, dict): + assert result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING'] + time.sleep(2) + + print("Moving electric gripper to position 200...") + result = client.control_electric_gripper( + action='move', + position=200, + speed=150, + current=200, + wait_for_ack=True, + timeout=10 + ) + if isinstance(result, dict): + assert result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING'] + time.sleep(2) + + # Get and verify initial status + print("Getting robot joint angles and pose...") + angles = client.get_angles() + pose = client.get_pose_rpy() + assert angles is not None + assert pose is not None + print(f"Initial angles: {angles}") + print(f"Initial pose: {pose}") + + # Pneumatic gripper sequence (exact timing from test_script.py) + print("Testing pneumatic gripper sequence...") + client.control_pneumatic_gripper("open", 1) + time.sleep(0.3) + client.control_pneumatic_gripper("close", 1) + time.sleep(0.3) + client.control_pneumatic_gripper("open", 1) + time.sleep(0.3) + client.control_pneumatic_gripper("close", 1) + time.sleep(0.3) + + # Joint movement sequence (exact waypoints and timing from test_script.py) + print("Moving to first joint position: [90, -90, 160, 12, 12, 180]...") + result = client.move_joints( + [90, -90, 160, 12, 12, 180], + duration=5.5, + wait_for_ack=True, + timeout=15 + ) + if isinstance(result, dict): + assert result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING'] + time.sleep(6) + + print("Moving to second joint position: [50, -60, 180, -12, 32, 0]...") + result = client.move_joints( + [50, -60, 180, -12, 32, 0], + duration=5.5, + wait_for_ack=True, + timeout=15 + ) + if isinstance(result, dict): + assert result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING'] + time.sleep(6) + + print("Moving back to first joint position: [90, -90, 160, 12, 12, 180]...") + result = client.move_joints( + [90, -90, 160, 12, 12, 180], + duration=5.5, + wait_for_ack=True, + timeout=15 + ) + if isinstance(result, dict): + assert result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING'] + time.sleep(6) + + # Pose movement (exact waypoint from test_script.py) + print("Moving to pose: [7, 250, 200, -100, 0, -90]...") + result = client.move_pose( + [7, 250, 200, -100, 0, -90], + duration=5.5, + wait_for_ack=True, + timeout=15 + ) + if isinstance(result, dict): + assert result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING'] + time.sleep(6) + + # Cartesian movement (exact waypoint from test_script.py) + print("Moving cartesian to: [7, 250, 150, -100, 0, -90]...") + result = client.move_cartesian( + [7, 250, 150, -100, 0, -90], + speed_percentage=50, + wait_for_ack=True, + timeout=15 + ) + if isinstance(result, dict): + assert result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING'] + + # Final status checks (exact from test_script.py) + print("Getting final gripper and IO status...") + gripper_status = client.get_gripper_status() + io_status = client.get_io() + + assert gripper_status is not None + assert io_status is not None + + print(f"Final gripper status: {gripper_status}") + print(f"Final IO status: {io_status}") + + if not human_prompt( + "Legacy test sequence completed successfully.\n" + "Did all movements execute safely and as expected?\n" + "This sequence replicates the verified safe waypoints from the original test_script.py." + ): + pytest.fail("User reported legacy sequence did not execute correctly") + + print("Legacy safe sequence test completed successfully") + + +@pytest.mark.hardware +@pytest.mark.slow +class TestHardwareGripper: + """Test gripper functionality with hardware.""" + + def test_pneumatic_gripper(self, client, human_prompt): + """Test pneumatic gripper operation.""" + if not human_prompt( + "Ready to test pneumatic gripper?\n" + "Ensure gripper is connected and air pressure is available.\n" + "Gripper will open and close on port 1." + ): + pytest.skip("User declined gripper test") + + # Test gripper open + print("Opening pneumatic gripper...") + result = client.control_pneumatic_gripper( + 'open', 1, + wait_for_ack=True, + timeout=5 + ) + + assert isinstance(result, dict) + assert result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING'] + + time.sleep(2.0) + + if not human_prompt("Did the gripper open? Confirm before continuing."): + pytest.fail("User reported gripper did not open") + + # Test gripper close + print("Closing pneumatic gripper...") + result = client.control_pneumatic_gripper( + 'close', 1, + wait_for_ack=True, + timeout=5 + ) + + assert isinstance(result, dict) + assert result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING'] + + time.sleep(2.0) + + if not human_prompt("Did the gripper close? Confirm operation."): + pytest.fail("User reported gripper did not close") + + print("Pneumatic gripper test completed successfully") + + def test_electric_gripper(self, client, human_prompt): + """Test electric gripper operation including calibration.""" + if not human_prompt( + "Ready to test electric gripper?\n" + "Ensure electric gripper is connected and powered.\n" + "Gripper will calibrate, then move to different positions." + ): + pytest.skip("User declined electric gripper test") + + # Get current gripper status + gripper_status = client.get_gripper_status() + if gripper_status: + print(f"Initial gripper status: {gripper_status}") + + # Test gripper calibration (from legacy test_script.py) + print("Calibrating electric gripper...") + result = client.control_electric_gripper( + action="calibrate", + wait_for_ack=True, + timeout=10 + ) + + assert isinstance(result, dict) + assert result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING'] + + time.sleep(2.0) + + # Test gripper movement + print("Moving electric gripper to position 100...") + result = client.control_electric_gripper( + 'move', + position=100, + speed=100, + current=400, + wait_for_ack=True, + timeout=10 + ) + + assert isinstance(result, dict) + assert result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING'] + + time.sleep(3.0) + + # Check new position + new_status = client.get_gripper_status() + if new_status: + print(f"Gripper status after move: {new_status}") + + if not human_prompt( + "Did the electric gripper move to the commanded position?\n" + "Check gripper position and movement quality." + ): + pytest.fail("User reported electric gripper did not move correctly") + + print("Electric gripper test completed successfully") + + +if __name__ == "__main__": + pytest.main([__file__]) diff --git a/tests/integration/__init__.py b/tests/integration/__init__.py new file mode 100644 index 0000000..242efae --- /dev/null +++ b/tests/integration/__init__.py @@ -0,0 +1,6 @@ +""" +Integration tests package. + +Contains tests that exercise component interactions using FAKE_SERIAL mode, +testing end-to-end UDP communication flows without requiring hardware. +""" diff --git a/tests/integration/test_ack_and_nonblocking.py b/tests/integration/test_ack_and_nonblocking.py new file mode 100644 index 0000000..0d4d090 --- /dev/null +++ b/tests/integration/test_ack_and_nonblocking.py @@ -0,0 +1,172 @@ +""" +Integration tests for acknowledgment and non-blocking behavior with parol6. + +Tests wait_for_ack functionality, non-blocking command flows, status polling, +timeout handling, and command tracking with live server communication. +""" + +import pytest +import sys +import os +import time +import uuid +from typing import Dict, Any + +# Add the parent directory to Python path +sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..', '..')) + +from tests.utils.udp import UDPClient +from parol6 import RobotClient + + +@pytest.mark.integration +class TestAcknowledmentTracking: + """Test command acknowledgment tracking functionality.""" + + def test_tracked_command_basic_flow(self, server_proc): + """Test basic acknowledgment flow with tracked commands.""" + client = RobotClient() + + # Send a tracked command and wait for acknowledgment + result = client.home(wait_for_ack=True, timeout=10.0) + + # Should get acknowledgment response + assert isinstance(result, dict) + assert 'status' in result + + # Status should indicate completion or execution + assert result['status'] in ['COMPLETED', 'QUEUED', 'EXECUTING'] + + def test_non_blocking_command_returns_id(self, server_proc): + """Test that non-blocking commands return command ID immediately.""" + client = RobotClient() + + # Send non-blocking command + result = client.move_joints( + [0, 0, 0, 0, 0, 0], + duration=1.0, + wait_for_ack=True, + non_blocking=True + ) + + # Should return command ID string immediately + assert isinstance(result, str) + assert len(result) == 8 # UUID prefix length + + def test_multiple_tracked_commands(self, server_proc): + """Test tracking multiple commands simultaneously.""" + client = RobotClient() + + # Send several commands with tracking + commands = [] + + for i in range(3): + result = client.move_joints( + [i, i, i, i, i, i], + duration=0.2, + wait_for_ack=True, + non_blocking=True + ) + if isinstance(result, str): + commands.append(result) + + # Each should have unique ID + assert len(set(commands)) == len(commands) + + # Wait for all to complete + time.sleep(1.0) + + def test_command_status_polling(self, server_proc): + """Test polling command status during execution.""" + client = RobotClient() + + # Send a longer running command + result = client.move_joints( + [5, 5, 5, 5, 5, 5], + duration=1.0, + wait_for_ack=True, + non_blocking=True + ) + + if isinstance(result, str): + cmd_id = result + + # Poll status while command runs + start_time = time.time() + seen_statuses = [] + + while time.time() - start_time < 2.0: + # In parol6, we would need to implement status polling or use tracker + time.sleep(0.1) + + # For now, just verify command was sent + break + + +@pytest.mark.integration +class TestErrorConditions: + """Test error conditions with acknowledgment tracking.""" + + def test_invalid_command_with_tracking(self, server_proc): + """Test that invalid commands return proper error acknowledgments.""" + client = RobotClient() + + # Try to send invalid command with tracking + result = client.move_joints([0, 0, 0, 0, 0, 0]) # Missing timing parameters + + # Should get error status + assert isinstance(result, dict) + assert result.get('status') == 'INVALID' + assert 'details' in result + + def test_malformed_parameters_with_tracking(self, server_proc): + """Test acknowledgment for commands with malformed parameters.""" + client = RobotClient() + + # Test with out-of-range speed percentage - must use wait_for_ack=True to get dict response + result = client.move_cartesian( + pose=[100, 100, 100, 0, 0, 0], + speed_percentage=200, # Invalid (>100) + wait_for_ack=True, + timeout=3.0 + ) + + # Should get validation error as tracked response + assert isinstance(result, dict) + assert result.get('status') in ['INVALID', 'FAILED', 'QUEUED', 'EXECUTING'] + + +@pytest.mark.integration +class TestBasicCommands: + """Test basic commands work with the server.""" + + def test_ping(self, server_proc): + """Test ping functionality.""" + client = RobotClient() + result = client.ping() + assert result is True + + def test_get_angles(self, server_proc): + """Test angle retrieval.""" + client = RobotClient() + angles = client.get_angles() + assert isinstance(angles, list) + assert len(angles) == 6 + + def test_get_io(self, server_proc): + """Test IO status retrieval.""" + client = RobotClient() + io_status = client.get_io() + assert isinstance(io_status, list) + assert len(io_status) >= 5 + + def test_stop_command(self, server_proc): + """Test stop command.""" + client = RobotClient() + result = client.stop(wait_for_ack=True, timeout=5.0) + assert isinstance(result, dict) + assert result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING'] + + +if __name__ == "__main__": + pytest.main([__file__]) diff --git a/tests/integration/test_smooth_commands_e2e.py b/tests/integration/test_smooth_commands_e2e.py new file mode 100644 index 0000000..f15bff5 --- /dev/null +++ b/tests/integration/test_smooth_commands_e2e.py @@ -0,0 +1,175 @@ +""" +Integration tests for smooth motion commands (minimal set). + +Tests one representative command per smooth motion family in FAKE_SERIAL mode. +Verifies command acceptance, completion status transitions, and basic functionality. +""" + +import pytest +import sys +import os +import time +from typing import Dict, Any, List + +# Add the parent directory to Python path +sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..', '..')) + +from parol6 import RobotClient + + +def _check_if_fake_serial_xfail(result): + """Helper to mark test as xfail if smooth motion fails due to IK in FAKE_SERIAL.""" + if (isinstance(result, dict) and + result.get('status') == 'FAILED' and + 'IK failed' in result.get('details', '')): + pytest.xfail("Smooth motion commands fail IK in FAKE_SERIAL mode (expected)") + + +@pytest.mark.integration +class TestSmoothMotionMinimal: + """Minimal set of smooth motion tests - one per command family.""" + + @pytest.fixture + def homed_robot(self, client, server_proc, robot_api_env): + """Ensure robot is homed before smooth motion tests.""" + print("Homing robot for smooth motion tests...") + + # Home the robot first + result = client.home(wait_for_ack=True, timeout=15.0) + if isinstance(result, dict): + assert result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING'] + + # Wait for homing to complete + import time + time.sleep(3.0) + + # Wait for robot to be stopped + assert client.wait_until_stopped(timeout=10.0) + print("Robot homed and ready for smooth motion tests") + + return True + + def test_smooth_circle_basic(self, client, server_proc, robot_api_env, homed_robot): + """Test basic circular motion in FAKE_SERIAL mode.""" + result = client.smooth_circle( + center=[0, 0, 100], + radius=30, + duration=2.0, + plane='XY', + frame='WRF', + wait_for_ack=True, + timeout=10.0 + ) + + # Check if we should xfail due to FAKE_SERIAL limitations + _check_if_fake_serial_xfail(result) + + # Should complete successfully in FAKE_SERIAL mode + assert isinstance(result, dict) + assert result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING'] + + # Wait for completion and verify robot stops + time.sleep(3.0) + assert client.is_robot_stopped(threshold_speed=5.0) + + def test_smooth_arc_center_basic(self, client, server_proc, robot_api_env, homed_robot): + """Test basic arc motion defined by center point.""" + result = client.smooth_arc_center( + end_pose=[100, 100, 150, 0, 0, 90], + center=[50, 50, 150], + duration=2.0, + frame='WRF', + wait_for_ack=True, + timeout=10.0 + ) + + _check_if_fake_serial_xfail(result) + + assert isinstance(result, dict) + assert result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING'] + + time.sleep(3.0) + assert client.is_robot_stopped(threshold_speed=5.0) + + def test_smooth_spline_basic(self, client, server_proc, robot_api_env, homed_robot): + """Test basic spline motion through waypoints.""" + waypoints = [ + [100.0, 100.0, 120.0, 0.0, 0.0, 0.0], + [150.0, 150.0, 130.0, 0.0, 0.0, 30.0], + [200.0, 100.0, 120.0, 0.0, 0.0, 60.0] + ] + + result = client.smooth_spline( + waypoints=waypoints, + duration=3.0, + frame='WRF', + wait_for_ack=True, + timeout=12.0 + ) + + _check_if_fake_serial_xfail(result) + + assert isinstance(result, dict) + assert result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING'] + + time.sleep(4.0) + assert client.is_robot_stopped(threshold_speed=5.0) + + def test_smooth_helix_basic(self, client, server_proc, robot_api_env, homed_robot): + """Test basic helical motion.""" + result = client.smooth_helix( + center=[100, 100, 80], + radius=25, + pitch=20, + height=60, + duration=3.0, + frame='WRF', + wait_for_ack=True, + timeout=12.0 + ) + + _check_if_fake_serial_xfail(result) + + assert isinstance(result, dict) + assert result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING'] + + time.sleep(4.0) + assert client.is_robot_stopped(threshold_speed=5.0) + + def test_smooth_blend_basic(self, client, server_proc, robot_api_env, homed_robot): + """Test basic blended motion through segments.""" + segments = [ + { + 'type': 'LINE', + 'end': [120.0, 80.0, 140.0, 0.0, 0.0, 30.0], + 'duration': 1.0 + }, + { + 'type': 'CIRCLE', + 'center': [120, 120, 140], + 'radius': 25, + 'plane': 'XY', + 'duration': 2.0, + 'clockwise': False + } + ] + + result = client.smooth_blend( + segments=segments, + blend_time=0.3, + frame='WRF', + wait_for_ack=True, + timeout=12.0 + ) + + _check_if_fake_serial_xfail(result) + + assert isinstance(result, dict) + assert result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING'] + + time.sleep(4.0) + assert client.is_robot_stopped(threshold_speed=5.0) + + +if __name__ == "__main__": + pytest.main([__file__]) diff --git a/tests/integration/test_udp_smoke.py b/tests/integration/test_udp_smoke.py new file mode 100644 index 0000000..4c6da0c --- /dev/null +++ b/tests/integration/test_udp_smoke.py @@ -0,0 +1,300 @@ +""" +Integration smoke tests for UDP communication using parol6. + +Tests basic UDP communication with headless_commander.py running in FAKE_SERIAL mode. +Covers PING/PONG, GET_* endpoints, STOP semantics, and basic functionality. +""" + +import pytest +import sys +import os +import time +from typing import Dict, Any + +# Add the parent directory to Python path +sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..', '..')) + +from tests.utils.udp import UDPClient +from parol6 import RobotClient + + +@pytest.mark.integration +class TestBasicCommunication: + """Test basic UDP communication with the server.""" + + def test_ping_pong(self, client, server_proc): + """Test PING/PONG communication.""" + result = client.ping() + assert result is True + + def test_server_process_running(self, client, server_proc): + """Verify the server process is running and responsive.""" + assert client.ping() is True # Server should be responsive + + +@pytest.mark.integration +class TestGetEndpoints: + """Test GET_* command endpoints that return immediate data.""" + + def test_get_pose(self, client, server_proc): + """Test GET_POSE command.""" + pose = client.get_pose() + assert pose is not None + assert isinstance(pose, list) + assert len(pose) == 16 # 4x4 transformation matrix flattened + + # Test helper methods too + pose_rpy = client.get_pose_rpy() + assert pose_rpy is not None + assert isinstance(pose_rpy, list) + assert len(pose_rpy) == 6 # [x, y, z, rx, ry, rz] + + pose_xyz = client.get_pose_xyz() + assert pose_xyz is not None + assert isinstance(pose_xyz, list) + assert len(pose_xyz) == 3 # [x, y, z] + + def test_get_angles(self, client, server_proc): + """Test GET_ANGLES command.""" + angles = client.get_angles() + assert angles is not None + assert isinstance(angles, list) + assert len(angles) == 6 # 6 joint angles + + def test_get_io(self, client, server_proc): + """Test GET_IO command.""" + io_status = client.get_io() + assert io_status is not None + assert isinstance(io_status, list) + assert len(io_status) == 5 # IN1, IN2, OUT1, OUT2, ESTOP + + # In FAKE_SERIAL mode, ESTOP should be released (1) + assert io_status[4] == 1 + + # Test helper method too + assert not client.is_estop_pressed() # Should be False in FAKE_SERIAL + + def test_get_gripper(self, client, server_proc): + """Test GET_GRIPPER command.""" + gripper = client.get_gripper_status() + assert gripper is not None + assert isinstance(gripper, list) + assert len(gripper) == 6 # ID, Position, Speed, Current, Status, ObjDetection + + def test_get_speeds(self, client, server_proc): + """Test GET_SPEEDS command.""" + speeds = client.get_speeds() + assert speeds is not None + assert isinstance(speeds, list) + assert len(speeds) == 6 # 6 joint speeds + + # Test helper method too + stopped = client.is_robot_stopped() + assert isinstance(stopped, bool) + + def test_get_status_aggregate(self, client, server_proc): + """Test GET_STATUS aggregate command.""" + status = client.get_status() + assert status is not None + assert isinstance(status, dict) + + # Should contain all status components + assert 'pose' in status + assert 'angles' in status + assert 'io' in status + assert 'gripper' in status + + + +@pytest.mark.integration +class TestStreamMode: + """Test streaming mode functionality.""" + + def test_stream_mode_toggle(self, server_proc, ports): + """Test STREAM ON/OFF commands.""" + client = UDPClient(ports.server_ip, ports.server_port) + + # Enable stream mode + success = client.send_command_no_response("STREAM|ON") + assert success + time.sleep(0.1) + + # Server should acknowledge and remain responsive + response = client.send_command("PING", timeout=2.0) + assert response == "PONG" + + # Disable stream mode + success = client.send_command_no_response("STREAM|OFF") + assert success + time.sleep(0.1) + + response = client.send_command("PING", timeout=2.0) + assert response == "PONG" + + +@pytest.mark.integration +class TestBasicMotionCommands: + """Test basic motion commands with improved assertions.""" + + def test_home_command(self, client, server_proc): + """Test HOME command with acknowledgment tracking.""" + # Send HOME command with tracking + result = client.home(wait_for_ack=True, timeout=15.0) + assert isinstance(result, dict) + assert result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING'] + + # Wait for completion and verify robot stops + assert client.wait_until_stopped(timeout=10.0) + + # Check that robot is responsive after homing + assert client.ping() is True + + # Check that angles are available after homing + angles = client.get_angles() + assert angles is not None + assert len(angles) == 6 + + def test_basic_joint_move(self, client, server_proc): + """Test basic joint movement command with tracking.""" + # Send a small joint move command with acknowledgment + result = client.move_joints( + [0, 5, 10, 15, 20, 25], + duration=2.0, + wait_for_ack=True, + timeout=10.0 + ) + assert isinstance(result, dict) + # In FAKE_SERIAL mode without proper homing, joint moves may fail validation + assert result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING', 'FAILED', 'INVALID'] + + # Only wait for completion if command was accepted + if result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING']: + assert client.wait_until_stopped(timeout=5.0) + + # Verify robot state after move attempt + angles = client.get_angles() + assert angles is not None + assert client.ping() is True + + def test_basic_pose_move(self, client, server_proc): + """Test basic pose movement command with validation.""" + # Send a pose move command with tracking + result = client.move_pose( + [100, 100, 100, 0, 0, 0], + speed_percentage=50, + wait_for_ack=True, + timeout=10.0 + ) + assert isinstance(result, dict) + # In FAKE_SERIAL mode, pose targets may fail IK validation + assert result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING', 'FAILED', 'INVALID'] + + # Only wait for completion if command was accepted + if result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING']: + assert client.wait_until_stopped(timeout=5.0) + + # Verify robot state + pose = client.get_pose_rpy() + assert pose is not None + assert len(pose) == 6 + + def test_cartesian_move_validation(self, client, server_proc): + """Test cartesian movement with proper validation.""" + # Test that move requires either duration or speed + result = client.move_cartesian([50, 50, 50, 0, 0, 0]) # No duration or speed + assert isinstance(result, dict) + assert result.get('status') == 'INVALID' + + # Valid cartesian move (may still fail IK in FAKE_SERIAL) + result = client.move_cartesian( + [50, 50, 50, 0, 0, 0], + duration=2.0, + wait_for_ack=True + ) + assert isinstance(result, dict) + assert result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING', 'FAILED', 'INVALID'] + + +@pytest.mark.integration +class TestErrorHandling: + """Test error handling and edge cases.""" + + def test_invalid_command_format(self, server_proc, ports): + """Test server response to invalid commands.""" + client = UDPClient(ports.server_ip, ports.server_port) + + # Send malformed command + success = client.send_command_no_response("INVALID_COMMAND|BAD|PARAMS") + assert success + + # Server should remain responsive despite invalid command + time.sleep(0.2) + response = client.send_command("PING", timeout=2.0) + assert response == "PONG" + + def test_empty_command(self, server_proc, ports): + """Test server response to empty commands.""" + client = UDPClient(ports.server_ip, ports.server_port) + + # Send empty command + success = client.send_command_no_response("") + assert success + + # Server should remain responsive + time.sleep(0.1) + response = client.send_command("PING", timeout=2.0) + assert response == "PONG" + + def test_rapid_command_sequence(self, server_proc, ports): + """Test server stability under rapid command sequence.""" + client = UDPClient(ports.server_ip, ports.server_port) + + # Send multiple commands rapidly + for i in range(10): + success = client.send_command_no_response("PING") + assert success + + # Give server time to process + time.sleep(0.5) + + # Server should still be responsive + response = client.send_command("PING", timeout=2.0) + assert response == "PONG" + + +@pytest.mark.integration +class TestCommandQueuing: + """Test basic command queuing behavior.""" + + def test_command_sequence_execution(self, server_proc, ports): + """Test that commands execute in sequence.""" + client = UDPClient(ports.server_ip, ports.server_port) + + # Send a sequence of commands + commands = [ + "HOME", + "DELAY|0.2", + "DELAY|0.2", + "DELAY|0.2" + ] + + start_time = time.time() + for cmd in commands: + success = client.send_command_no_response(cmd) + assert success + + # Wait for all commands to complete + # In FAKE_SERIAL mode, these should execute relatively quickly + time.sleep(2.0) + + # Server should be responsive after sequence + response = client.send_command("PING", timeout=2.0) + assert response == "PONG" + + # Total time should be reasonable (commands + processing overhead) + total_time = time.time() - start_time + assert total_time < 5.0 # Should complete within reasonable time + + +if __name__ == "__main__": + pytest.main([__file__]) diff --git a/tests/unit/__init__.py b/tests/unit/__init__.py new file mode 100644 index 0000000..c75290d --- /dev/null +++ b/tests/unit/__init__.py @@ -0,0 +1,6 @@ +""" +Unit tests package. + +Contains tests for individual components tested in isolation, +without requiring external processes or hardware. +""" diff --git a/tests/unit/test_robot_api_commands.py b/tests/unit/test_robot_api_commands.py new file mode 100644 index 0000000..1c2013f --- /dev/null +++ b/tests/unit/test_robot_api_commands.py @@ -0,0 +1,245 @@ +""" +Unit tests for parol6 client functionality. + +Simplified tests focusing on actual parol6 client methods rather than +the old robot_api interface. +""" + +import pytest +import os +from unittest.mock import patch, MagicMock + +# Import the parol6 modules +from parol6 import RobotClient +from parol6.utils.tracking import LazyCommandTracker + + +@pytest.mark.unit +class TestEnvironmentConfiguration: + """Test client configuration.""" + + def test_default_configuration(self): + """Test that default values are used.""" + client = RobotClient() + assert client.host == "127.0.0.1" + assert client.port == 5001 + + def test_custom_configuration(self): + """Test that custom values work.""" + client = RobotClient(host="192.168.1.100", port=6001) + assert client.host == "192.168.1.100" + assert client.port == 6001 + + def test_tracker_port_configuration(self): + """Test that LazyCommandTracker accepts port configuration.""" + tracker = LazyCommandTracker(listen_port=7002) + assert tracker.listen_port == 7002 + + +@pytest.mark.unit +class TestCommandValidation: + """Test command validation and error handling.""" + + def test_move_joints_validation(self): + """Test validation of move_joints parameters.""" + client = RobotClient() + + # Test missing both duration and speed + with patch.object(client, '_send_tracked', return_value={'status': 'INVALID', 'details': 'Must specify either duration or speed_percentage'}): + result = client.move_joints([0, 0, 0, 0, 0, 0]) + assert isinstance(result, dict) + assert result.get('status') == 'INVALID' + + def test_move_pose_validation(self): + """Test validation of move_pose parameters.""" + client = RobotClient() + + # Test missing both duration and speed + with patch.object(client, '_send_tracked', return_value={'status': 'INVALID', 'details': 'Must specify either duration or speed_percentage'}): + result = client.move_pose([100, 100, 100, 0, 0, 0]) + assert isinstance(result, dict) + assert result.get('status') == 'INVALID' + + def test_jog_joint_validation(self): + """Test validation of jog_joint parameters.""" + client = RobotClient() + + # Test missing both duration and distance + with patch.object(client, '_send_tracked', return_value={'status': 'INVALID', 'details': 'Must specify either duration or distance_deg'}): + result = client.jog_joint(0, 50) + assert isinstance(result, dict) + assert result.get('status') == 'INVALID' + + def test_move_cartesian_validation(self): + """Test validation of move_cartesian parameters.""" + client = RobotClient() + + # Test missing both duration and speed + with patch.object(client, '_send_tracked', return_value={'status': 'INVALID', 'details': 'Must specify either duration or speed_percentage'}): + result = client.move_cartesian([100, 100, 100, 0, 0, 0]) + assert isinstance(result, dict) + assert result.get('status') == 'INVALID' + + def test_jog_multiple_validation(self): + """Test validation of jog_multiple parameters.""" + client = RobotClient() + + # Test mismatched joints and speeds length + with patch.object(client, '_send_tracked', return_value={'status': 'INVALID', 'details': 'Number of joints must match number of speeds'}): + result = client.jog_multiple([0, 1], [50], 2.0) + assert isinstance(result, dict) + assert result.get('status') == 'INVALID' + + +@pytest.mark.unit +class TestCommandFormatting: + """Test command string formatting without network operations.""" + + @patch.object(RobotClient, '_send_tracked') + def test_move_joints_command_format(self, mock_send): + """Test that move_joints formats commands correctly.""" + client = RobotClient() + mock_send.return_value = "Success" + + client.move_joints([10, 20, 30, 40, 50, 60], duration=5.0) + + # Verify the command format + mock_send.assert_called_once() + command = mock_send.call_args[0][0] + assert command.startswith("MOVEJOINT|") + assert "10|20|30|40|50|60" in command + assert "5.0" in command + assert "NONE" in command # speed should be None + + @patch.object(RobotClient, '_send_tracked') + def test_move_pose_command_format(self, mock_send): + """Test that move_pose formats commands correctly.""" + client = RobotClient() + mock_send.return_value = "Success" + + client.move_pose([100, 200, 300, 0, 90, 180], speed_percentage=75) + + mock_send.assert_called_once() + command = mock_send.call_args[0][0] + assert command.startswith("MOVEPOSE|") + assert "100|200|300|0|90|180" in command + assert "NONE" in command # duration should be None + assert "75" in command + + @patch.object(RobotClient, '_send_tracked') + def test_jog_command_format(self, mock_send): + """Test that jog_joint formats commands correctly.""" + client = RobotClient() + mock_send.return_value = "Success" + + client.jog_joint(2, 50, duration=1.0) + + mock_send.assert_called_once() + command = mock_send.call_args[0][0] + assert command.startswith("JOG|") + assert "2|50|1.0|NONE" in command + + @patch.object(RobotClient, '_send_tracked') + def test_cartesian_jog_command_format(self, mock_send): + """Test that jog_cartesian formats commands correctly.""" + client = RobotClient() + mock_send.return_value = "Success" + + client.jog_cartesian('WRF', 'X+', 25, 2.0) + + mock_send.assert_called_once() + command = mock_send.call_args[0][0] + assert command.startswith("CARTJOG|") + assert "WRF|X+|25|2.0" in command + + @patch.object(RobotClient, '_send_tracked') + def test_gripper_command_format(self, mock_send): + """Test gripper command formatting.""" + client = RobotClient() + mock_send.return_value = "Success" + + # Test pneumatic gripper + client.control_pneumatic_gripper('open', 1) + mock_send.assert_called_with("PNEUMATICGRIPPER|open|1", False, 2.0, False) + + # Test electric gripper + client.control_electric_gripper('move', position=100, speed=150, current=500) + # Check the last call + last_call = mock_send.call_args_list[-1] + assert "ELECTRICGRIPPER|move|100|150|500" == last_call[0][0] + + +@pytest.mark.unit +class TestStatusQueries: + """Test status query methods.""" + + @patch.object(RobotClient, '_request') + def test_get_angles(self, mock_request): + """Test angle retrieval.""" + client = RobotClient() + + # Mock successful response + mock_request.return_value = "ANGLES|10.0,20.0,30.0,40.0,50.0,60.0" + result = client.get_angles() + + assert result == [10.0, 20.0, 30.0, 40.0, 50.0, 60.0] + mock_request.assert_called_with("GET_ANGLES", bufsize=1024) + + @patch.object(RobotClient, '_request') + def test_get_io(self, mock_request): + """Test IO status retrieval.""" + client = RobotClient() + + # Mock successful response + mock_request.return_value = "IO|0,1,0,1,1" + result = client.get_io() + + assert result == [0, 1, 0, 1, 1] + mock_request.assert_called_with("GET_IO", bufsize=1024) + + @patch.object(RobotClient, '_request') + def test_get_gripper_status(self, mock_request): + """Test gripper status retrieval.""" + client = RobotClient() + + # Mock successful response + mock_request.return_value = "GRIPPER|1,128,150,500,0,2" + result = client.get_gripper_status() + + assert result == [1, 128, 150, 500, 0, 2] + mock_request.assert_called_with("GET_GRIPPER", bufsize=1024) + + +@pytest.mark.unit +class TestBasicTracker: + """Test basic tracker functionality without network operations.""" + + def test_tracker_initialization(self): + """Test tracker initializes in lazy mode.""" + tracker = LazyCommandTracker(listen_port=6002) + + # Should not be initialized until first use + assert not hasattr(tracker, '_initialized') or not tracker._initialized + assert tracker.listen_port == 6002 + + @patch('socket.socket') + def test_tracker_lazy_initialization(self, mock_socket_class): + """Test that tracker initializes only when first needed.""" + # Mock socket creation and binding + mock_socket = MagicMock() + mock_socket_class.return_value = mock_socket + + tracker = LazyCommandTracker() + + # Mock successful initialization + with patch('threading.Thread'): + # Try to track a command (should trigger initialization) + command, cmd_id = tracker.track_command("TEST") + + # Should get a command ID + assert cmd_id is not None + assert command == f"{cmd_id}|TEST" + + +if __name__ == "__main__": + pytest.main([__file__]) diff --git a/tests/utils/__init__.py b/tests/utils/__init__.py new file mode 100644 index 0000000..45fe6af --- /dev/null +++ b/tests/utils/__init__.py @@ -0,0 +1,19 @@ +""" +Test utilities package. + +Provides helper functions and classes for testing the PAROL6 Python API. +""" + +from .process import HeadlessCommanderProc, wait_for_completion, find_available_ports +from .udp import UDPClient, AckListener, send_command_with_ack, create_tracked_command, parse_server_response + +__all__ = [ + 'HeadlessCommanderProc', + 'wait_for_completion', + 'find_available_ports', + 'UDPClient', + 'AckListener', + 'send_command_with_ack', + 'create_tracked_command', + 'parse_server_response' +] diff --git a/tests/utils/process.py b/tests/utils/process.py new file mode 100644 index 0000000..c9de47a --- /dev/null +++ b/tests/utils/process.py @@ -0,0 +1,324 @@ +""" +Process management utilities for testing. + +Provides classes and functions to manage the headless_commander.py subprocess +during integration tests, including startup, readiness checks, and cleanup. +""" + +import os +import sys +import time +import socket +import subprocess +import threading +from typing import Optional, Dict, Any, List +import logging + +logger = logging.getLogger(__name__) + + +class HeadlessCommanderProc: + """ + Manages a headless_commander.py subprocess for integration testing. + + Handles starting, stopping, and checking readiness of the commander process + with configurable ports and environment variables. + """ + + def __init__( + self, + ip: str = "127.0.0.1", + port: int = 5001, + ack_port: int = 5002, + env: Optional[Dict[str, str]] = None + ): + """ + Initialize the process manager. + + Args: + ip: IP address for the server to bind to + port: UDP port for command reception + ack_port: UDP port for acknowledgment sending + env: Additional environment variables to set + """ + self.ip = ip + self.port = port + self.ack_port = ack_port + self.env = env or {} + + self.process: Optional[subprocess.Popen] = None + self._output_lines: List[str] = [] + self._error_lines: List[str] = [] + self._output_thread: Optional[threading.Thread] = None + self._error_thread: Optional[threading.Thread] = None + + def start(self, log_level: str = "WARNING", timeout: float = 10.0) -> bool: + """ + Start the headless commander process. + + Args: + log_level: Logging level for the subprocess + timeout: Maximum time to wait for process startup + + Returns: + True if started successfully, False otherwise + """ + if self.process and self.process.poll() is None: + logger.warning("Process already running") + return True + + # Prepare environment + process_env = os.environ.copy() + process_env.update({ + "PAROL6_FAKE_SERIAL": "1", # Enable fake serial simulation + "PAROL6_NOAUTOHOME": "1", # Disable auto-homing for tests + "PAROL6_SERVER_IP": self.ip, + "PAROL6_SERVER_PORT": str(self.port), + "PAROL6_ACK_PORT": str(self.ack_port), + }) + process_env.update(self.env) + + # Find the headless_commander.py script + script_path = os.path.join( + os.path.dirname(os.path.dirname(os.path.dirname(__file__))), + "headless_commander.py" + ) + + if not os.path.exists(script_path): + logger.error(f"headless_commander.py not found at {script_path}") + return False + + # Prepare command + cmd = [ + sys.executable, script_path, + "--log-level", log_level, + "--no-auto-home" + ] + + try: + logger.info(f"Starting headless commander: {' '.join(cmd)}") + logger.debug(f"Environment: FAKE_SERIAL=1, NOAUTOHOME=1, IP={self.ip}, PORT={self.port}, ACK_PORT={self.ack_port}") + + self.process = subprocess.Popen( + cmd, + env=process_env, + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + text=True, + bufsize=1, # Line buffered + universal_newlines=True + ) + + # Start output capture threads + self._start_output_capture() + + # Wait for process to be ready + if self.wait_ready(timeout): + logger.info(f"Headless commander started successfully (PID: {self.process.pid})") + return True + else: + logger.error("Process failed to become ready within timeout") + self.stop() + return False + + except Exception as e: + logger.error(f"Failed to start process: {e}") + if self.process: + self.process.terminate() + self.process = None + return False + + def _start_output_capture(self): + """Start threads to capture stdout and stderr.""" + if not self.process: + return + + def capture_output(stream, output_list): + try: + for line in iter(stream.readline, ''): + if line: + line = line.rstrip('\n\r') + output_list.append(line) + # Also log to our test logger for debugging + logger.debug(f"PROC: {line}") + except Exception as e: + logger.debug(f"Output capture error: {e}") + + self._output_thread = threading.Thread( + target=capture_output, + args=(self.process.stdout, self._output_lines), + daemon=True + ) + self._error_thread = threading.Thread( + target=capture_output, + args=(self.process.stderr, self._error_lines), + daemon=True + ) + + self._output_thread.start() + self._error_thread.start() + + def wait_ready(self, timeout: float = 10.0) -> bool: + """ + Wait for the process to be ready by sending PING commands. + + Args: + timeout: Maximum time to wait in seconds + + Returns: + True if process responds to PING, False otherwise + """ + start_time = time.time() + + while time.time() - start_time < timeout: + if self.process and self.process.poll() is not None: + # Process died + logger.error("Process terminated during startup") + return False + + # Try to ping the server + try: + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: + sock.settimeout(1.0) + sock.sendto(b"PING", (self.ip, self.port)) + + try: + data, _ = sock.recvfrom(1024) + if data.decode('utf-8').strip() == "PONG": + return True + except socket.timeout: + pass # No response yet + + except Exception as e: + logger.debug(f"Ping attempt failed: {e}") + + time.sleep(0.5) # Wait before retry + + return False + + def stop(self) -> bool: + """ + Stop the headless commander process. + + Returns: + True if stopped successfully, False otherwise + """ + if not self.process: + return True + + try: + # Try graceful shutdown first + logger.debug("Attempting graceful shutdown...") + self.process.terminate() + + try: + self.process.wait(timeout=5.0) + logger.info(f"Process terminated gracefully (exit code: {self.process.returncode})") + except subprocess.TimeoutExpired: + logger.warning("Process did not terminate gracefully, forcing shutdown...") + self.process.kill() + self.process.wait() + logger.info(f"Process killed (exit code: {self.process.returncode})") + + except Exception as e: + logger.error(f"Error stopping process: {e}") + return False + finally: + self.process = None + + return True + + def is_running(self) -> bool: + """Check if the process is currently running.""" + return self.process is not None and self.process.poll() is None + + def get_output_lines(self) -> List[str]: + """Get captured stdout lines.""" + return self._output_lines.copy() + + def get_error_lines(self) -> List[str]: + """Get captured stderr lines.""" + return self._error_lines.copy() + + def clear_output(self): + """Clear captured output lines.""" + self._output_lines.clear() + self._error_lines.clear() + + +def wait_for_completion(result_or_id: Any, timeout: float = 10.0) -> Dict[str, Any]: + """ + Unified waiting logic for acknowledgment-tracked results in tests. + + Handles both direct result dictionaries and command IDs that need polling. + + Args: + result_or_id: Either a result dict with status info, or a command ID string + timeout: Maximum time to wait for completion + + Returns: + Dictionary with status information + """ + if isinstance(result_or_id, dict): + # Already a result dictionary + return result_or_id + + # If it's a string, it might be a command ID - for now just return a placeholder + # In a real implementation, this would poll the robot_api for status + return { + 'status': 'TIMEOUT', + 'details': 'wait_for_completion not fully implemented for command IDs', + 'completed': True, + 'command_id': result_or_id + } + + +def find_available_ports(start_port: int = 5001, count: int = 2) -> List[int]: + """ + Find available UDP ports starting from the given port. + + Args: + start_port: Port to start searching from + count: Number of consecutive ports needed + + Returns: + List of available port numbers + """ + available_ports = [] + current_port = start_port + + while len(available_ports) < count: + try: + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: + sock.bind(('127.0.0.1', current_port)) + available_ports.append(current_port) + except OSError: + # Port in use, reset search if we were building a consecutive sequence + available_ports.clear() + + current_port += 1 + + # Prevent infinite loop + if current_port > start_port + 1000: + break + + return available_ports + + +def check_port_available(port: int, host: str = '127.0.0.1') -> bool: + """ + Check if a UDP port is available. + + Args: + port: Port number to check + host: Host address to check + + Returns: + True if port is available, False otherwise + """ + try: + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: + sock.bind((host, port)) + return True + except OSError: + return False diff --git a/tests/utils/udp.py b/tests/utils/udp.py new file mode 100644 index 0000000..9ec8e59 --- /dev/null +++ b/tests/utils/udp.py @@ -0,0 +1,378 @@ +""" +UDP utilities for testing. + +Provides helper functions for UDP communication during tests, +including acknowledgment listening and command sending utilities. +""" + +import socket +import time +import threading +from typing import Optional, Dict, Any, Callable, List, Tuple +import logging +import queue + +logger = logging.getLogger(__name__) + + +class UDPClient: + """ + Simple UDP client for sending commands to the robot server during tests. + """ + + def __init__(self, server_ip: str = "127.0.0.1", server_port: int = 5001): + """ + Initialize UDP client. + + Args: + server_ip: IP address of the robot server + server_port: Port of the robot server + """ + self.server_ip = server_ip + self.server_port = server_port + + def send_command(self, command: str, timeout: float = 2.0) -> Optional[str]: + """ + Send a command and wait for immediate response (for GET commands). + + Args: + command: Command string to send + timeout: Timeout for waiting for response + + Returns: + Response string if received, None otherwise + """ + try: + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: + sock.settimeout(timeout) + + # Send command + sock.sendto(command.encode('utf-8'), (self.server_ip, self.server_port)) + logger.debug(f"Sent command: {command}") + + # Wait for response (for GET commands) + try: + data, _ = sock.recvfrom(2048) + response = data.decode('utf-8') + logger.debug(f"Received response: {response}") + return response + except socket.timeout: + logger.debug(f"No response received for command: {command}") + return None + + except Exception as e: + logger.error(f"Error sending command '{command}': {e}") + return None + + def send_command_no_response(self, command: str) -> bool: + """ + Send a command without waiting for response (for motion commands). + + Args: + command: Command string to send + + Returns: + True if sent successfully, False otherwise + """ + try: + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: + sock.sendto(command.encode('utf-8'), (self.server_ip, self.server_port)) + logger.debug(f"Sent command (no response): {command}") + return True + except Exception as e: + logger.error(f"Error sending command '{command}': {e}") + return False + + +class AckListener: + """ + Listens for UDP acknowledgment messages during tests. + + Provides functionality to capture and wait for specific acknowledgments + from the robot controller during command execution. + """ + + def __init__(self, listen_port: int = 5002): + """ + Initialize acknowledgment listener. + + Args: + listen_port: Port to listen on for acknowledgments + """ + self.listen_port = listen_port + self.socket: Optional[socket.socket] = None + self.thread: Optional[threading.Thread] = None + self.running = False + + # Storage for received acknowledgments + self.ack_queue = queue.Queue() + self.ack_history: List[Dict[str, Any]] = [] + + def start(self) -> bool: + """ + Start listening for acknowledgments. + + Returns: + True if started successfully, False otherwise + """ + if self.running: + logger.warning("AckListener already running") + return True + + try: + self.socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + self.socket.bind(('127.0.0.1', self.listen_port)) + self.socket.settimeout(0.1) # Short timeout for non-blocking operation + + self.running = True + self.thread = threading.Thread(target=self._listen_loop, daemon=True) + self.thread.start() + + logger.debug(f"AckListener started on port {self.listen_port}") + return True + + except Exception as e: + logger.error(f"Failed to start AckListener: {e}") + self.stop() + return False + + def stop(self): + """Stop listening for acknowledgments.""" + self.running = False + + if self.thread: + self.thread.join(timeout=1.0) + self.thread = None + + if self.socket: + self.socket.close() + self.socket = None + + logger.debug("AckListener stopped") + + def _listen_loop(self): + """Main listening loop (runs in separate thread).""" + while self.running and self.socket: + try: + data, addr = self.socket.recvfrom(1024) + message = data.decode('utf-8') + + # Parse acknowledgment message: ACK|cmd_id|status|details + parts = message.split('|', 3) + if parts[0] == 'ACK' and len(parts) >= 3: + ack_info = { + 'cmd_id': parts[1], + 'status': parts[2], + 'details': parts[3] if len(parts) > 3 else '', + 'timestamp': time.time(), + 'sender': addr + } + + # Add to both queue and history + self.ack_queue.put(ack_info) + self.ack_history.append(ack_info) + + logger.debug(f"Received ACK: {ack_info}") + + except socket.timeout: + continue # Normal timeout, keep listening + except Exception as e: + if self.running: # Only log if we should still be running + logger.debug(f"Listen loop error: {e}") + + def wait_for_ack( + self, + cmd_id: str, + timeout: float = 5.0, + expected_status: Optional[str] = None + ) -> Optional[Dict[str, Any]]: + """ + Wait for a specific acknowledgment. + + Args: + cmd_id: Command ID to wait for + timeout: Maximum time to wait + expected_status: Specific status to wait for (optional) + + Returns: + Acknowledgment info dict if received, None if timeout + """ + start_time = time.time() + + while time.time() - start_time < timeout: + try: + # Check queue with short timeout + ack_info = self.ack_queue.get(timeout=0.1) + + if ack_info['cmd_id'] == cmd_id: + if expected_status is None or ack_info['status'] == expected_status: + return ack_info + else: + # Put back in queue if status doesn't match + self.ack_queue.put(ack_info) + else: + # Put back in queue if cmd_id doesn't match + self.ack_queue.put(ack_info) + + except queue.Empty: + continue + + logger.debug(f"Timeout waiting for ACK: cmd_id={cmd_id}, expected_status={expected_status}") + return None + + def get_all_acks_for_command(self, cmd_id: str) -> List[Dict[str, Any]]: + """ + Get all acknowledgments received for a specific command ID. + + Args: + cmd_id: Command ID to search for + + Returns: + List of acknowledgment info dicts + """ + return [ack for ack in self.ack_history if ack['cmd_id'] == cmd_id] + + def get_recent_acks(self, count: int = 10) -> List[Dict[str, Any]]: + """ + Get the most recent acknowledgments. + + Args: + count: Number of recent acknowledgments to return + + Returns: + List of recent acknowledgment info dicts + """ + return self.ack_history[-count:] if self.ack_history else [] + + def clear_history(self): + """Clear acknowledgment history and queue.""" + self.ack_history.clear() + while not self.ack_queue.empty(): + try: + self.ack_queue.get_nowait() + except queue.Empty: + break + + +def send_command_with_ack( + command: str, + server_ip: str = "127.0.0.1", + server_port: int = 5001, + ack_port: int = 5002, + timeout: float = 5.0 +) -> Tuple[Optional[str], Optional[Dict[str, Any]]]: + """ + Send a command with acknowledgment tracking. + + This is a convenience function that sets up an acknowledgment listener, + sends a command, and waits for the acknowledgment. + + Args: + command: Command to send + server_ip: Server IP address + server_port: Server command port + ack_port: Acknowledgment port + timeout: Timeout for acknowledgment + + Returns: + Tuple of (immediate_response, final_ack_info) + """ + # Set up acknowledgment listener + ack_listener = AckListener(ack_port) + if not ack_listener.start(): + return None, None + + try: + # Send command + client = UDPClient(server_ip, server_port) + response = client.send_command(command, timeout=1.0) + + # For tracked commands, the response might be empty and we need to wait for ACK + # For immediate commands (GET_*), we get response right away + if response and not response.startswith('ACK'): + # Got immediate response, no ACK expected + return response, None + + # Wait for acknowledgment (extract command ID if present) + # This is a simplified version - in practice, you'd extract the actual command ID + # from the command string if it contains one + parts = command.split('|', 1) + if len(parts) > 1 and len(parts[0]) == 8 and parts[0].replace('-', '').isalnum(): + cmd_id = parts[0] + ack_info = ack_listener.wait_for_ack(cmd_id, timeout) + return response, ack_info + else: + # No command ID in command, can't track ACK + return response, None + + finally: + ack_listener.stop() + + +def create_tracked_command(base_command: str, cmd_id: Optional[str] = None) -> str: + """ + Create a command with tracking ID. + + Args: + base_command: Base command string + cmd_id: Command ID to use (generates one if None) + + Returns: + Command string with tracking ID prepended + """ + import uuid + if cmd_id is None: + cmd_id = str(uuid.uuid4())[:8] + + return f"{cmd_id}|{base_command}" + + +def parse_server_response(response: str) -> Dict[str, Any]: + """ + Parse a server response into a structured format. + + Args: + response: Raw response string from server + + Returns: + Dictionary with parsed response data + """ + if not response: + return {'type': 'empty', 'data': None} + + parts = response.split('|', 1) + response_type = parts[0] + + parsed = { + 'type': response_type, + 'raw': response + } + + if len(parts) > 1: + data = parts[1] + + if response_type in ['POSE', 'ANGLES', 'SPEEDS']: + # Numeric array data + try: + parsed['data'] = [float(x) for x in data.split(',')] + except ValueError: + parsed['data'] = data + elif response_type in ['IO', 'GRIPPER']: + # Integer array data + try: + parsed['data'] = [int(x) for x in data.split(',')] + except ValueError: + parsed['data'] = data + elif response_type == 'STATUS': + # Complex status data + parsed['data'] = {} + for item in data.split('|'): + if '=' in item: + key, value = item.split('=', 1) + parsed['data'][key] = value + else: + parsed['data'] = data + else: + parsed['data'] = None + + return parsed