| """ |
| Action Planning Module — Generates structured action steps from |
| detected objects and the interpreted command. |
| |
| Uses the ObjectMapper for fuzzy label matching between the command |
| interpretation and the actual YOLO detections. |
| """ |
|
|
| import logging |
| from typing import Any |
|
|
| from src.utils.object_mapper import find_best_match |
|
|
| logger = logging.getLogger(__name__) |
|
|
|
|
| class ActionPlanner: |
| """ |
| Generates a step-by-step action plan by matching the interpreted |
| command to detected objects in the scene. |
| """ |
|
|
| def plan( |
| self, |
| detected_objects: list[dict[str, Any]], |
| command_interpretation: dict[str, Any], |
| ) -> dict[str, Any]: |
| """ |
| Create an action plan from detections and the interpreted command. |
| |
| Args: |
| detected_objects: Objects detected by the vision module. |
| command_interpretation: Output from the language reasoner |
| (keys: ``action``, ``source``, ``destination``). |
| |
| Returns: |
| Dict containing: |
| - ``steps`` (list[str]): Human-readable action steps. |
| - ``source_object`` (dict | None): Matched source detection. |
| - ``dest_object`` (dict | None): Matched destination detection. |
| - ``source_bbox`` (list | None): Source bounding box. |
| - ``dest_bbox`` (list | None): Destination bounding box. |
| - ``action`` (str): The action verb. |
| - ``status`` (str): ``"success"`` or ``"partial"`` or ``"failed"``. |
| """ |
| if not detected_objects: |
| logger.warning("No detected objects provided to planner.") |
| return self._empty_plan("No objects detected in the scene.") |
|
|
| action = command_interpretation.get("action", "unknown") |
| source_query = command_interpretation.get("source", "") |
| dest_query = command_interpretation.get("destination") |
|
|
| detected_labels = [obj["label"] for obj in detected_objects] |
|
|
| |
| source_obj = self._match_object(source_query, detected_labels, detected_objects) |
| |
| dest_obj = ( |
| self._match_object(dest_query, detected_labels, detected_objects) |
| if dest_query |
| else None |
| ) |
|
|
| |
| steps = self._build_steps(action, source_query, dest_query, source_obj, dest_obj) |
|
|
| |
| status = self._determine_status(source_obj, dest_obj, dest_query) |
|
|
| plan = { |
| "action": action, |
| "steps": steps, |
| "source_object": source_obj, |
| "dest_object": dest_obj, |
| "source_bbox": source_obj["bbox"] if source_obj else None, |
| "dest_bbox": dest_obj["bbox"] if dest_obj else None, |
| "status": status, |
| } |
|
|
| logger.info( |
| "Action plan generated: %d steps, status=%s", len(steps), status |
| ) |
| return plan |
|
|
| |
| |
| |
|
|
| @staticmethod |
| def _match_object( |
| query: str | None, |
| detected_labels: list[str], |
| detected_objects: list[dict[str, Any]], |
| ) -> dict[str, Any] | None: |
| """ |
| Match a query string to a detected object using fuzzy matching. |
| |
| Returns the full detection dict or None. |
| """ |
| if not query: |
| return None |
|
|
| matched_label = find_best_match(query, detected_labels) |
| if matched_label is None: |
| logger.warning("Could not match '%s' to any detected object.", query) |
| return None |
|
|
| |
| for obj in detected_objects: |
| if obj["label"] == matched_label: |
| return obj |
|
|
| return None |
|
|
| @staticmethod |
| def _build_steps( |
| action: str, |
| source_query: str, |
| dest_query: str | None, |
| source_obj: dict | None, |
| dest_obj: dict | None, |
| ) -> list[str]: |
| """Build human-readable action steps.""" |
| steps: list[str] = [] |
|
|
| source_label = source_obj["label"] if source_obj else source_query |
| dest_label = dest_obj["label"] if dest_obj else dest_query |
|
|
| |
| if source_obj: |
| bbox = source_obj["bbox"] |
| steps.append( |
| f"Step 1: Locate '{source_label}' at position " |
| f"[{bbox[0]}, {bbox[1]}, {bbox[2]}, {bbox[3]}]" |
| ) |
| else: |
| steps.append( |
| f"Step 1: Locate '{source_label}' — ⚠ Not detected in scene" |
| ) |
|
|
| |
| if dest_query: |
| if dest_obj: |
| bbox = dest_obj["bbox"] |
| steps.append( |
| f"Step 2: Locate destination '{dest_label}' at position " |
| f"[{bbox[0]}, {bbox[1]}, {bbox[2]}, {bbox[3]}]" |
| ) |
| else: |
| steps.append( |
| f"Step 2: Locate destination '{dest_label}' " |
| f"— ⚠ Not detected in scene" |
| ) |
|
|
| |
| action_desc = action.replace("_", " ").capitalize() |
| if dest_label: |
| steps.append( |
| f"Step {len(steps) + 1}: {action_desc} '{source_label}' " |
| f"→ '{dest_label}'" |
| ) |
| else: |
| steps.append( |
| f"Step {len(steps) + 1}: {action_desc} '{source_label}'" |
| ) |
|
|
| |
| steps.append(f"Step {len(steps) + 1}: Verify action completion") |
|
|
| return steps |
|
|
| @staticmethod |
| def _determine_status( |
| source_obj: dict | None, |
| dest_obj: dict | None, |
| dest_query: str | None, |
| ) -> str: |
| """Determine planning status based on matching results.""" |
| if source_obj is None: |
| return "failed" |
| if dest_query and dest_obj is None: |
| return "partial" |
| return "success" |
|
|
| @staticmethod |
| def _empty_plan(reason: str) -> dict[str, Any]: |
| """Return an empty plan with a failure reason.""" |
| return { |
| "action": "none", |
| "steps": [f"Planning failed: {reason}"], |
| "source_object": None, |
| "dest_object": None, |
| "source_bbox": None, |
| "dest_bbox": None, |
| "status": "failed", |
| } |
|
|
