Modified files for visual odom v2

Author: jmjimenez2020

exercises/visual_odom/frontend/WebGUI.tsx

@@ -8,12 +8,14 @@ import { states } from "jderobot-commsmanager";
 
 function WebGUI() {
   const exerciseContext = useExercise();
+
   const [image, setImage] = useState<string | undefined>(undefined);
 
   // =========================
-  // 🧠 NUEVO: POSE STATE
+  // POSE + TRAJECTORY
   // =========================
   const [pose, setPose] = useState<any>(null);
+  const [trajectory, setTrajectory] = useState<any[]>([]);
 
   const [manager, setManager] = useState(exerciseContext.manager);
 
@@ -26,25 +28,33 @@ function WebGUI() {
     const update = data.update;
 
     // =========================
-    // IMAGE (igual que antes)
+    // IMAGE
     // =========================
     if (update.image) {
-      const image = JSON.parse(update.image);
-      setImage(`data:image/png;base64,${image.image}`);
-
-      // =========================
-      // 🧠 NUEVO: POSE
-      // =========================
-      if (image.pose) {
-        setPose(image.pose);
-      }
+      const img = JSON.parse(update.image);
+      setImage(`data:image/png;base64,${img.image}`);
+    }
+
+    // =========================
+    // POSE
+    // =========================
+    if (update.pose) {
+      setPose(update.pose);
+    }
+
+    // =========================
+    // TRAJECTORY
+    // =========================
+    if (update.trajectory) {
+      setTrajectory(update.trajectory);
     }
   };
 
   const stateCallback = (state: string) => {
     if (state === states.TOOLS_READY) {
       setImage(undefined);
       setPose(null);
+      setTrajectory([]);
     }
   };
 
@@ -53,11 +63,16 @@ function WebGUI() {
   return (
     <WebGUIContainer>
 
-      {/* Imagen principal (igual que antes) */}
-      <WebGUIImage id="gui_canvas" src={image} style={{ width: "100%" }} fit />
+      {/* IMAGE */}
+      <WebGUIImage
+        id="gui_canvas"
+        src={image}
+        style={{ width: "100%" }}
+        fit
+      />
 
       {/* =========================
-          🧠 NUEVO: VISOR POSE DEBUG
+          POSE OVERLAY
           ========================= */}
       {pose && (
         <div style={{
@@ -78,6 +93,39 @@ function WebGUI() {
         </div>
       )}
 
+      {/* =========================
+          TRAJECTORY VISUAL (BÁSICO)
+          ========================= */}
+      {trajectory.length > 0 && (
+        <svg
+          style={{
+            position: "absolute",
+            top: 0,
+            left: 0,
+            width: "100%",
+            height: "100%",
+            pointerEvents: "none",
+          }}
+        >
+          {trajectory.map((p, i) => {
+            if (i === 0) return null;
+            const prev = trajectory[i - 1];
+
+            return (
+              <line
+                key={i}
+                x1={prev.x * 50 + 200}
+                y1={prev.y * 50 + 200}
+                x2={p.x * 50 + 200}
+                y2={p.y * 50 + 200}
+                stroke="red"
+                strokeWidth={2}
+              />
+            );
+          })}
+        </svg>
+      )}
+
     </WebGUIContainer>
   );
 }

exercises/visual_odom/python_template/HAL.py

@@ -21,6 +21,11 @@ def __init__(self):
         self.bridge = CvBridge()
         self.get_logger().info("Input publisher initialized on /input/image_raw")
 
+        # =========================================================
+        # VISUAL ODOMETRY + TRAJECTORY STORAGE
+        # =========================================================
+        self.trajectory = []  # list of poses (x, y, z, yaw)
+
     def publish_image(self, cv_image):
         """
         Publish a CV image to the ROS2 topic
@@ -49,35 +54,47 @@ def publish_image(self, cv_image):
         except Exception as e:
             self.get_logger().error(f"Failed to publish image: {e}")
 
+    # =========================================================
+    # VISUAL ODOMETRY API
+    # =========================================================
 
-# =========================================================
-# POSE ESTIMATION (VISUAL ODOMETRY STORAGE)
-# =========================================================
-
-_current_pose = {
-    "x": 0.0,
-    "y": 0.0,
-    "z": 0.0,
-    "roll": 0.0,
-    "pitch": 0.0,
-    "yaw": 0.0,
-}
-
-
-def setEstimatedPose(x, y, z=0.0, roll=0.0, pitch=0.0, yaw=0.0):
-    global _current_pose
-    _current_pose = {
-        "x": float(x),
-        "y": float(y),
-        "z": float(z),
-        "roll": float(roll),
-        "pitch": float(pitch),
-        "yaw": float(yaw),
-    }
-
-
-def getEstimatedPose():
-    return _current_pose
+    def setEstimatedPose(self, x, y, z=0.0, roll=0.0, pitch=0.0, yaw=0.0):
+        """
+        Store estimated pose and append to trajectory
+        """
+        pose = {
+            "x": float(x),
+            "y": float(y),
+            "z": float(z),
+            "roll": float(roll),
+            "pitch": float(pitch),
+            "yaw": float(yaw),
+        }
+
+        self._current_pose = pose
+        self.trajectory.append(pose)
+
+        # debug print (NO BORRADO como pediste)
+        print(f"[HAL] Pose updated: {pose}")
+
+    def getEstimatedPose(self):
+        """
+        Return current estimated pose
+        """
+        return getattr(self, "_current_pose", {
+            "x": 0.0,
+            "y": 0.0,
+            "z": 0.0,
+            "roll": 0.0,
+            "pitch": 0.0,
+            "yaw": 0.0,
+        })
+
+    def getTrajectory(self):
+        """
+        Return full trajectory for visualization
+        """
+        return self.trajectory
 
 
 # =========================================================