Merge branch 'dev'

Merge new feature replay mode and cleanup to main

Author: Meisdy

Backend/communication_server.py

@@ -34,14 +34,14 @@ def start(self) -> None:
             while self.running:
                 try:
                     message = self.socket.recv_json()
-                    logger.debug(f"Received message: {message}")
+                    logger.debug("Received message: %s", message)
                     response = self.handler.process_message(message=message)
                     self.socket.send_json(response)
-                    logger.debug(f"Sent response: {response}")
+                    logger.debug("Sent response: %s", response)
                 except zmq.Again:
                     continue
                 except zmq.ZMQError as e:
-                    logger.error(f"ZMQ error in server loop: {e}")
+                    logger.error("ZMQ error in server loop: %s", e)
         finally:
             self.close()
 
@@ -52,6 +52,6 @@ def close(self) -> None:
         self.socket.close()
         self.context.term()
 
-    def _handle_signal(self, sig, frame: object):
+    def _handle_signal(self, sig, frame: object) -> None:
         logger.info('Signal %s received, shutting down', sig)
         self.running = False

Backend/config_backend.py

@@ -1,7 +1,9 @@
 BINDING_ADDRESS     = "tcp://*:5555"                                # Binding address for backend ZeroMQ Server
 ZMQ_TIMEOUT_MS      = 1000                                          # Timeout for ZeroMQ
 AVAILABLE_ROBOTS    = ["mock", "franka", "ur"]                      # ur, franka, mock : Add new adapter here
-ALLOWED_COMMANDS    = ["ping", "get_status", "execute_sequence"]    # allowed main level commands for backend to accept
+ALLOWED_COMMANDS    = ["ping", "get_status", "execute_sequence",    # allowed main level commands for backend to accept
+                       "save_script", "run_script", "stop_script",
+                       "get_script_status", "delete_script"]
 LOGGING_LEVEL       = "INFO"                                        # Log level for console, Options: DEBUG, INFO, WARNING, ERROR (for console)
 LOGGING_LEVEL_FILE  = "DEBUG"                                       # Log level for file output in Backend/log/
 PC_IP               = "192.168.1.101"                               # your Backend PC IP as seen by the UR Robot. Needed for callback for finished pos

Backend/main.py

@@ -11,14 +11,14 @@
 def main() -> None:
     """Entry point for the robot backend server."""
     logger = setup_logging()
-    logger.info(f"Starting backend server on {BINDING_ADDRESS}")
+    logger.info("Starting backend server on %s", BINDING_ADDRESS)
 
     server = ServerZeroMQ(BINDING_ADDRESS)
 
     try:
         server.start()
     except Exception as e:
-        logger.exception(f"Unhandled server error: {e}")
+        logger.exception("Unhandled server error: %s", e)
 
 
 if __name__ == "__main__":

Backend/message_handler.py

@@ -2,10 +2,11 @@
 import inspect
 import logging
 import time
+import threading
 from typing import Dict, Optional, Type
 
 from Backend.robot_controllers.base_robot_controller import BaseRobotController
-from Backend.config_backend import AVAILABLE_ROBOTS
+from Backend.config_backend import AVAILABLE_ROBOTS, ALLOWED_COMMANDS
 
 logger = logging.getLogger("cobot_backend")
 
@@ -32,10 +33,11 @@ def _load_controllers() -> Dict[str, Type[BaseRobotController]]:
 class MessageHandler:
     """Processes incoming commands and dispatches them to the robot controller."""
 
-    ALLOWED_COMMANDS = ["ping", "get_status", "execute_sequence"]
-
-    def __init__(self):
+    def __init__(self) -> None:
         self.robot: Optional[BaseRobotController] = None
+        self._script_stop_event: threading.Event = threading.Event()
+        self._script_thread: Optional[threading.Thread] = None
+
         for robot_type, cls in CONTROLLERS.items():
             logger.debug("Controller available: '%s' -> %s", robot_type, cls.__name__)
 
@@ -54,13 +56,18 @@ def process_message(self, message: dict) -> dict:
             command = message.get("command", "")
             data = message.get("data", {})
 
-            if command not in self.ALLOWED_COMMANDS:
+            if command not in ALLOWED_COMMANDS:
                 return self._unknown_command(command)
 
             commands = {
                 "ping": self._answer_ping,
                 "get_status": self._send_status,
                 "execute_sequence": lambda: self._execute_sequence(data),
+                "save_script": lambda: self._save_script(data),
+                "run_script": lambda: self._run_script(data),
+                "stop_script": self._stop_script,
+                "get_script_status": self._get_script_status,
+                "delete_script": lambda: self._delete_script(data),
             }
 
             return commands[command]()
@@ -69,6 +76,8 @@ def process_message(self, message: dict) -> dict:
             logger.exception("Error processing message: %s", message)
             return self._formatted_response("error", {"error message": str(e)})
 
+
+
     def _ensure_robot_ready(self, robot_type: str) -> dict:
         self._current_robot_type = robot_type
         expected_class = CONTROLLERS[robot_type]
@@ -187,7 +196,7 @@ def _process_command(self, command: dict, robot: BaseRobotController) -> dict:
             offset = None
             if target.get("type") == "offset_from_pose":
                 raw = target.get("offset", {})
-                offset = [raw["x_mm"], raw["y_mm"], raw["z_mm"]]
+                offset = [raw.get("x_mm", 0.0), raw.get("y_mm", 0.0), raw.get("z_mm", 0.0)]
 
             if motion_type == "moveJ":
                 return robot.move_joint(pose, speed, offset)
@@ -230,4 +239,114 @@ def _process_command(self, command: dict, robot: BaseRobotController) -> dict:
                 return self._ensure_robot_ready(self._current_robot_type)
 
         else:
-            return {"success": False, "message": f"Unknown action: {action}"}
+            return {"success": False, "message": f"Unknown action: {action}"}
+
+    def _save_script(self, data: dict) -> dict:
+        robot_type = data.get("robot")
+        script_name = data.get("script_name")
+        commands = data.get("commands", [])
+
+        if not robot_type or robot_type not in CONTROLLERS:
+            return self._formatted_response("rejected", {"message": f"Unsupported robot type: {robot_type}"})
+        if not script_name or not isinstance(commands, list):
+            return self._formatted_response("rejected", {"message": "Missing script_name or commands"})
+
+        result = self._ensure_robot_ready(robot_type)
+        if not result["success"]:
+            return self._formatted_response("rejected",
+                                            {"message": f"Could not connect to {robot_type}: {result['message']}"})
+
+        result = self.robot.save_script(script_name, commands)
+        if not result["success"]:
+            return self._formatted_response("error", {"message": result["message"]})
+
+        logger.info("Script '%s' saved for robot '%s' with %d command(s)", script_name, robot_type, len(commands))
+        return self._formatted_response("success", {"message": f"Script '{script_name}' saved"})
+
+    def _run_script(self, data: dict) -> dict:
+        robot_type = data.get("robot")
+        script_name = data.get("script_name")
+        loop = data.get("loop", 1)
+
+        if not robot_type or robot_type not in CONTROLLERS:
+            return self._formatted_response("rejected", {"message": f"Unsupported robot type: {robot_type}"})
+
+        result = self._ensure_robot_ready(robot_type)
+        if not result["success"]:
+            return self._formatted_response("rejected",
+                                            {"message": f"Could not connect to {robot_type}: {result['message']}"})
+
+        script = self.robot.get_script(script_name)
+        if script is None:
+            return self._formatted_response("rejected", {"message": f"Unknown script: '{script_name}'"})
+
+        if self._script_thread and self._script_thread.is_alive():
+            return self._formatted_response("rejected", {"message": "A script is already running"})
+
+        validation_errors = [
+            error
+            for cmd in script
+            if (error := self._validate_command(cmd, self.robot)) is not None
+        ]
+        if validation_errors:
+            logger.warning("Script '%s' rejected: %s", script_name, validation_errors)
+            return self._formatted_response("rejected", {"reasons": validation_errors})
+
+        self._script_stop_event.clear()
+        self._script_thread = threading.Thread(
+            target=self._run_script_loop,
+            args=(script_name, loop, script),
+            daemon=True,
+            name="thread_script_loop"
+        )
+        self._script_thread.start()
+        logger.info("Script '%s' started for robot '%s', loop=%d", script_name, robot_type, loop)
+        return self._formatted_response("success", {"message": f"Script '{script_name}' started"})
+
+    def _stop_script(self) -> dict:
+        """Signal the running script loop to stop after the current command."""
+        self._script_stop_event.set()
+        logger.info("Stop signal sent to script loop")
+        return self._formatted_response("success", {"message": "Stop signal sent"})
+
+    def _run_script_loop(self, script_name: str, loop: int, commands: list) -> None:
+        """Execute script commands in a loop. Checks stop event between commands."""
+        iteration = 0
+        while not self._script_stop_event.is_set():
+            logger.info("Script '%s' — iteration %d", script_name, iteration + 1)
+            for cmd in commands:
+                if self._script_stop_event.is_set():
+                    break
+                result = self._process_command(cmd, self.robot)
+                if not result["success"]:
+                    logger.error("Script '%s' aborted at command: %s", script_name, result["message"])
+                    return
+
+            iteration += 1
+            if loop != -1 and iteration >= loop:
+                break
+
+        logger.info("Script '%s' finished after %d iteration(s)", script_name, iteration)
+
+    def _get_script_status(self) -> dict:
+        is_running = self._script_thread is not None and self._script_thread.is_alive()
+        return self._formatted_response("success", {"is_running": is_running})
+
+    def _delete_script(self, data: dict) -> dict:
+        robot_type = data.get("robot")
+        script_name = data.get("script_name")
+
+        if not robot_type or robot_type not in CONTROLLERS:
+            return self._formatted_response("rejected", {"message": f"Unsupported robot type: {robot_type}"})
+
+        result = self._ensure_robot_ready(robot_type)
+        if not result["success"]:
+            return self._formatted_response("rejected",
+                                            {"message": f"Could not connect to {robot_type}: {result['message']}"})
+
+        result = self.robot.delete_script(script_name)
+        if not result["success"]:
+            return self._formatted_response("error", {"message": result["message"]})
+
+        logger.info("Script '%s' deleted", script_name)
+        return self._formatted_response("success", {"message": f"Script '{script_name}' deleted"})

Backend/poses/mock_poses.jsonl

@@ -1,2 +1,5 @@
 {"name": "home", "pos": [0.0, 0.0, 0.0], "quat": [1.0, 0.0, 0.0, 0.0], "joints": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]}
 {"name": "test_pose", "pos": [0.0, 0.0, 0.0], "quat": [1.0, 0.0, 0.0, 0.0], "joints": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]}
+{"name": "position_1", "pos": [0.0, 0.0, 0.0], "quat": [1.0, 0.0, 0.0, 0.0], "joints": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]}
+{"name": "position_2", "pos": [0.0, 0.0, 0.0], "quat": [1.0, 0.0, 0.0, 0.0], "joints": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]}
+{"name": "p1", "pos": [20.0, 0.0, 0.0], "quat": [1.0, 0.0, 0.0, 0.0], "joints": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]}

Backend/robot_controllers/__init__.py

@@ -0,0 +1 @@
+# Backend robot controllers — package placeholder

Backend/robot_controllers/base_robot_controller.py

@@ -10,9 +10,11 @@
 class BaseRobotController(ABC):
     """Abstract base class defining common robot controller interface."""
 
-    def __init__(self, poses_file: str):
+    def __init__(self, poses_file: str) -> None:
         self.poses_file: str = poses_file
         self.poses: dict = self._load_poses()
+        self._scripts_file: str = poses_file.replace("_poses.jsonl", "_scripts.jsonl")
+        self.scripts: dict = self._load_scripts()
         self.connected: bool = False
         self.gripper_state: Optional[str] = None
 
@@ -35,19 +37,38 @@ def save_pose(self, name: str, overwrite: bool = False) -> dict:
         }
         self.poses[name] = entry
         self._write_poses()
-        logger.info(f"Saved position '{name}'")
+        logger.info("Saved position '%s'", name)
         return {"success": True, "message": f"Pose '{name}' saved"}
 
     def delete_pose(self, name: str) -> dict:
         """Delete a named pose."""
         if name not in self.poses:
-            logger.info(f"Pose '{name}' unknown")
+            logger.info("Pose '%s' unknown", name)
             return {"success": False, "message": f"Pose '{name}' not found"}
         del self.poses[name]
         self._write_poses()
-        logger.info(f"Deleted position '{name}'")
+        logger.info("Deleted position '%s'", name)
         return {"success": True, "message": f"Pose '{name}' deleted"}
 
+    def save_script(self, name: str, commands: list) -> dict:
+        """Save a named command sequence for later replay."""
+        self.scripts[name] = commands
+        self._write_scripts()
+        logger.info("Saved script '%s' with %d command(s)", name, len(commands))
+        return {"success": True, "message": f"Script '{name}' saved"}
+
+    def get_script(self, name: str) -> Optional[list]:
+        """Return the command list for a named script, or None if not found."""
+        return self.scripts.get(name)
+
+    def delete_script(self, name: str) -> dict:
+        if name not in self.scripts:
+            return {"success": False, "message": f"Script '{name}' not found"}
+        del self.scripts[name]
+        self._write_scripts()
+        logger.info("Deleted script '%s'", name)
+        return {"success": True, "message": f"Script '{name}' deleted"}
+
     def is_ready(self) -> bool:
         """Returns connected status if not overridden by robot controller."""
         return self.connected
@@ -128,4 +149,22 @@ def _load_poses(self) -> dict:
     def _write_poses(self) -> None:
         with open(self.poses_file, 'w') as f:
             for entry in self.poses.values():
-                f.write(json.dumps(entry) + '\n')
+                f.write(json.dumps(entry) + '\n')
+
+    def _load_scripts(self) -> dict:
+        if not os.path.exists(self._scripts_file):
+            os.makedirs(os.path.dirname(self._scripts_file), exist_ok=True)
+            with open(self._scripts_file, 'w'):
+                pass
+            return {}
+        scripts = {}
+        with open(self._scripts_file, 'r') as f:
+            for line in f:
+                entry = json.loads(line.strip())
+                scripts[entry["name"]] = entry["commands"]
+        return scripts
+
+    def _write_scripts(self) -> None:
+        with open(self._scripts_file, 'w') as f:
+            for name, commands in self.scripts.items():
+                f.write(json.dumps({"name": name, "commands": commands}) + '\n')

Backend/robot_controllers/franka_controller.py

@@ -60,7 +60,7 @@
 
 class FrankaController(BaseRobotController):
 
-    def __init__(self):
+    def __init__(self) -> None:
         super().__init__(POSES_FILE)
         self._robot: Optional[FrankaRobot] = None
         self._ros_process: Optional[subprocess.Popen] = None

Backend/robot_controllers/franka_robot.py

@@ -17,7 +17,7 @@ class FrankaRobot:
     and forwards them to MoveIt. Has no knowledge of named poses or files.
     """
 
-    def __init__(self, arm_name: str, hand_name: str, moveit_commander):
+    def __init__(self, arm_name: str, hand_name: str, moveit_commander) -> None:
         self.arm     = moveit_commander.MoveGroupCommander(arm_name)
         self.gripper = moveit_commander.MoveGroupCommander(hand_name)
 

Backend/robot_controllers/mock_controller.py

@@ -10,7 +10,7 @@
 class MockRobotController(BaseRobotController):
     """Mock robot controller for testing without hardware."""
 
-    def __init__(self, poses_file: str = "Backend/poses/mock_poses.jsonl"):
+    def __init__(self, poses_file: str = "Backend/poses/mock_poses.jsonl") -> None:
         super().__init__(poses_file)
         self._joint_angles: list = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
         self._tcp_pose: list = [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]  # pos + identity quat
@@ -38,6 +38,7 @@ def move_joint(self, pose: dict, speed: Optional[float] = None, offset: Optional
         target_pos = list(pose["pos"])
         if offset:
             target_pos = [p + o for p, o in zip(target_pos, offset)]
+            logger.info("Offset used: '%s'", offset)
         time.sleep(2)
         self._joint_angles = list(pose["joints"])
         self._tcp_pose = target_pos + list(pose["quat"])
@@ -49,6 +50,7 @@ def move_linear(self, pose: dict, speed: Optional[float] = None, offset: Optiona
         target_pos = list(pose["pos"])
         if offset:
             target_pos = [p + o for p, o in zip(target_pos, offset)]
+            logger.info("Offset used: '%s'", offset)
         time.sleep(2)
         self._joint_angles = list(pose["joints"])
         self._tcp_pose = target_pos + list(pose["quat"])