SahilCodevally/codevally-vision-language-action

1"""2Action Planning Module — Generates structured action steps from3detected objects and the interpreted command.4 5Uses the ObjectMapper for fuzzy label matching between the command6interpretation and the actual YOLO detections.7"""8 9import logging10from typing import Any11 12from src.utils.object_mapper import find_best_match13 14logger = logging.getLogger(__name__)15 16 17class ActionPlanner:18    """19    Generates a step-by-step action plan by matching the interpreted20    command to detected objects in the scene.21    """22 23    def plan(24        self,25        detected_objects: list[dict[str, Any]],26        command_interpretation: dict[str, Any],27    ) -> dict[str, Any]:28        """29        Create an action plan from detections and the interpreted command.30 31        Args:32            detected_objects: Objects detected by the vision module.33            command_interpretation: Output from the language reasoner34                (keys: ``action``, ``source``, ``destination``).35 36        Returns:37            Dict containing:38                - ``steps`` (list[str]): Human-readable action steps.39                - ``source_object`` (dict | None): Matched source detection.40                - ``dest_object`` (dict | None): Matched destination detection.41                - ``source_bbox`` (list | None): Source bounding box.42                - ``dest_bbox`` (list | None): Destination bounding box.43                - ``action`` (str): The action verb.44                - ``status`` (str): ``"success"`` or ``"partial"`` or ``"failed"``.45        """46        if not detected_objects:47            logger.warning("No detected objects provided to planner.")48            return self._empty_plan("No objects detected in the scene.")49 50        action = command_interpretation.get("action", "unknown")51        source_query = command_interpretation.get("source", "")52        dest_query = command_interpretation.get("destination")53 54        detected_labels = [obj["label"] for obj in detected_objects]55 56        # Match source object57        source_obj = self._match_object(source_query, detected_labels, detected_objects)58        # Match destination object (optional)59        dest_obj = (60            self._match_object(dest_query, detected_labels, detected_objects)61            if dest_query62            else None63        )64 65        # Build the step list66        steps = self._build_steps(action, source_query, dest_query, source_obj, dest_obj)67 68        # Determine status69        status = self._determine_status(source_obj, dest_obj, dest_query)70 71        plan = {72            "action": action,73            "steps": steps,74            "source_object": source_obj,75            "dest_object": dest_obj,76            "source_bbox": source_obj["bbox"] if source_obj else None,77            "dest_bbox": dest_obj["bbox"] if dest_obj else None,78            "status": status,79        }80 81        logger.info(82            "Action plan generated: %d steps, status=%s", len(steps), status83        )84        return plan85 86    # ------------------------------------------------------------------87    # Internal helpers88    # ------------------------------------------------------------------89 90    @staticmethod91    def _match_object(92        query: str | None,93        detected_labels: list[str],94        detected_objects: list[dict[str, Any]],95    ) -> dict[str, Any] | None:96        """97        Match a query string to a detected object using fuzzy matching.98 99        Returns the full detection dict or None.100        """101        if not query:102            return None103 104        matched_label = find_best_match(query, detected_labels)105        if matched_label is None:106            logger.warning("Could not match '%s' to any detected object.", query)107            return None108 109        # Return the first detection with this label110        for obj in detected_objects:111            if obj["label"] == matched_label:112                return obj113 114        return None115 116    @staticmethod117    def _build_steps(118        action: str,119        source_query: str,120        dest_query: str | None,121        source_obj: dict | None,122        dest_obj: dict | None,123    ) -> list[str]:124        """Build human-readable action steps."""125        steps: list[str] = []126 127        source_label = source_obj["label"] if source_obj else source_query128        dest_label = dest_obj["label"] if dest_obj else dest_query129 130        # Step 1: Locate source131        if source_obj:132            bbox = source_obj["bbox"]133            steps.append(134                f"Step 1: Locate '{source_label}' at position "135                f"[{bbox[0]}, {bbox[1]}, {bbox[2]}, {bbox[3]}]"136            )137        else:138            steps.append(139                f"Step 1: Locate '{source_label}' — ⚠ Not detected in scene"140            )141 142        # Step 2: Locate destination (if applicable)143        if dest_query:144            if dest_obj:145                bbox = dest_obj["bbox"]146                steps.append(147                    f"Step 2: Locate destination '{dest_label}' at position "148                    f"[{bbox[0]}, {bbox[1]}, {bbox[2]}, {bbox[3]}]"149                )150            else:151                steps.append(152                    f"Step 2: Locate destination '{dest_label}' "153                    f"— ⚠ Not detected in scene"154                )155 156        # Step 3: Execute action157        action_desc = action.replace("_", " ").capitalize()158        if dest_label:159            steps.append(160                f"Step {len(steps) + 1}: {action_desc} '{source_label}' "161                f"→ '{dest_label}'"162            )163        else:164            steps.append(165                f"Step {len(steps) + 1}: {action_desc} '{source_label}'"166            )167 168        # Step 4: Confirmation169        steps.append(f"Step {len(steps) + 1}: Verify action completion")170 171        return steps172 173    @staticmethod174    def _determine_status(175        source_obj: dict | None,176        dest_obj: dict | None,177        dest_query: str | None,178    ) -> str:179        """Determine planning status based on matching results."""180        if source_obj is None:181            return "failed"182        if dest_query and dest_obj is None:183            return "partial"184        return "success"185 186    @staticmethod187    def _empty_plan(reason: str) -> dict[str, Any]:188        """Return an empty plan with a failure reason."""189        return {190            "action": "none",191            "steps": [f"Planning failed: {reason}"],192            "source_object": None,193            "dest_object": None,194            "source_bbox": None,195            "dest_bbox": None,196            "status": "failed",197        }198