API source snapshot · 2026-09-24
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 | """Async MTC Python Ophyd Device for Bluesky Integration """ from threading import Thread, Lock, current_thread import rclpy from rclpy.node import Node from rclpy.action import ActionClient from rosidl_runtime_py.utilities import get_action from ophyd.status import DeviceStatus from bluesky.protocols import Movable class ActionStatus(DeviceStatus): """Track the status of a ROS Action for Bluesky""" def __init__(self, device, **kwargs): super().__init__(device, **kwargs) def _handle_failure(self): """Called when status is marked as failed - attempt cancellation""" self.log.debug("Trying to stop %s", repr(self.device)) self.device.cancel_goal() class MTCExecutionDeviceAsync(Node, Movable): """Async Ophyd device for MTC task execution via Bluesky """ def __init__(self, name="mtc_executor", **kwargs): super().__init__(name, **kwargs) # Set name attribute for Ophyd self.name = name self.parent = None self.action_type = get_action('beambot_interfaces/MTCExecution') self._action_client = ActionClient(self, self.action_type, 'beambot_execution') self._goal_handle = None self._bluesky_status = None self._send_goal_future = None self._get_result_future = None # Background spinning thread self._spin_thread = None self._spinning = False self._spin_lock = Lock() def construct_goal_message(self, json_path_or_string): """Construct goal from JSON file path or JSON string Args: json_path_or_string: Either a path to a JSON file or a JSON string Returns: MTCExecution.Goal: The constructed goal message """ goal = self.action_type.Goal() # If it's a file path, read the file if isinstance(json_path_or_string, str) and json_path_or_string.endswith('.json'): with open(json_path_or_string, 'r') as f: goal.full_json = f.read() else: # Assume it's already a JSON string goal.full_json = json_path_or_string return goal def _spin_in_background(self): """Spin ROS in background thread to handle action callbacks This allows the set() method to return immediately while still processing ROS callbacks for the action. """ while self._spinning: if self._bluesky_status and self._bluesky_status.done: break try: rclpy.spin_once(self, timeout_sec=0.1) except Exception as e: self.get_logger().error(f"Error in spin thread: {e}") break with self._spin_lock: self._spinning = False self._spin_thread = None def set(self, json_path_or_string): """Execute MTC task (async-capable) This method returns immediately after sending the goal, allowing Bluesky to manage the waiting behavior based on the wait parameter. Args: json_path_or_string: Path to JSON task file or JSON string Returns: ActionStatus: Status object that Bluesky can monitor """ # Stop any previous spin thread self._stop_spinning() # Create new status self._bluesky_status = ActionStatus(self) # Construct and send goal goal_msg = self.construct_goal_message(json_path_or_string) self.get_logger().info("Waiting for action server...") if not self._action_client.wait_for_server(timeout_sec=10.0): self.get_logger().error("Action server not available!") self._bluesky_status.set_exception( Exception("Action server not available after 10 seconds") ) return self._bluesky_status self.get_logger().info("Sending goal...") self._send_goal_future = self._action_client.send_goal_async( goal_msg, feedback_callback=self._feedback_callback ) self._send_goal_future.add_done_callback(self._goal_response_callback) # Start background spinning to handle callbacks with self._spin_lock: self._spinning = True self._spin_thread = Thread(target=self._spin_in_background, daemon=True) self._spin_thread.start() # Return immediately return self._bluesky_status def _stop_spinning(self): """Stop the background spin thread""" with self._spin_lock: self._spinning = False # Only join if we're NOT being called from within the spin thread itself # (ROS callbacks execute in the spin thread, so _result_callback can trigger this) if self._spin_thread and self._spin_thread.is_alive(): if current_thread() != self._spin_thread: self._spin_thread.join(timeout=1.0) self._spin_thread = None def _feedback_callback(self, feedback_msg): """Handle execution feedback from the action server This provides real-time progress updates during task execution. """ fb = feedback_msg.feedback self.get_logger().info( f"Step {fb.current_step}: {fb.current_action} " f"({fb.progress_percentage:.1f}%) - {fb.status_message}" ) def _goal_response_callback(self, future): """Handle goal acceptance/rejection from action server""" try: goal_handle = future.result() except Exception as e: self.get_logger().error(f"Goal send failed: {e}") if self._bluesky_status: self._bluesky_status.set_exception(e) self._stop_spinning() return if not goal_handle.accepted: self.get_logger().error("Goal rejected by action server") if self._bluesky_status: self._bluesky_status.set_exception(Exception("Goal rejected")) self._stop_spinning() return self.get_logger().info("Goal accepted by action server") self._goal_handle = goal_handle self._get_result_future = goal_handle.get_result_async() self._get_result_future.add_done_callback(self._result_callback) def _result_callback(self, future): """Handle final result from action server This is called when the action completes (success, failure, or cancel). """ try: result = future.result() except Exception as e: self.get_logger().error(f"Error getting result: {e}") if self._bluesky_status: self._bluesky_status.set_exception(e) self._stop_spinning() return # Check result status if result.status == 4: # SUCCEEDED self.get_logger().info( f"✓ Task completed successfully: " f"{result.result.completed_steps}/{result.result.total_steps} steps" ) if self._bluesky_status: self._bluesky_status.set_finished() elif result.status == 5: # ABORTED error_msg = result.result.error_message if hasattr(result.result, 'error_message') else "Unknown error" self.get_logger().error(f"✗ Task aborted: {error_msg}") if self._bluesky_status: self._bluesky_status.set_exception(Exception(f"Task aborted: {error_msg}")) elif result.status == 6: # CANCELED self.get_logger().warning("⊗ Task canceled") if self._bluesky_status: self._bluesky_status.set_exception(Exception("Task canceled")) else: self.get_logger().error(f"✗ Unknown result status: {result.status}") if self._bluesky_status: self._bluesky_status.set_exception(Exception(f"Unknown status: {result.status}")) self._stop_spinning() def cancel_goal(self): """Cancel the currently executing goal This sends a cancel request to the action server. The cancellation is asynchronous - the status will be updated when the server responds. Returns: bool: True if cancel was requested, False if no active goal """ if self._goal_handle is None: self.get_logger().warning("No active goal to cancel") return False self.get_logger().info("Requesting goal cancellation...") cancel_future = self._goal_handle.cancel_goal_async() def cancel_done(future): try: cancel_response = future.result() if len(cancel_response.goals_canceling) > 0: self.get_logger().info("Goal cancellation accepted") else: self.get_logger().warning("Goal cancellation rejected or already complete") except Exception as e: self.get_logger().error(f"Error during cancellation: {e}") cancel_future.add_done_callback(cancel_done) return True def stop(self, *, success=False): """Stop the device (called by Bluesky on pause/abort) This is part of the Bluesky protocol for handling interruptions. Args: success: Whether this is a successful stop or an error condition """ self.get_logger().info(f"Stop requested (success={success})") # Only cancel if this is NOT a successful completion # When success=True, the plan finished normally and we should # let the background task continue if not success: self.get_logger().info("Canceling goal due to error/pause...") self.cancel_goal() else: self.get_logger().info("Plan completed, task continues in background") # Don't stop spinning - let the background thread continue # It will stop automatically when the task completes def __del__(self): """Cleanup on deletion""" self._stop_spinning() |