Merge pull request #3209 from JdeRobot/onnxruntime-gpu

update end_to_end visual control

Author: codezerro

compose_cfg/dev_humble_nvidia.yaml

@@ -1,53 +1,56 @@
 services:
-  my-postgres:
-    image: jderobot/robotics-database:latest
-    container_name: universe_db
-    environment:
-      POSTGRES_DB: academy_db 
-      POSTGRES_USER: user-dev 
-      POSTGRES_PASSWORD: robotics-academy-dev
-    ports:
-      - "5432:5432"
-    volumes:
-      - ./RoboticsInfrastructure/database/universes.sql:/docker-entrypoint-initdb.d/1.sql
-      - ./database/exercises/db.sql:/docker-entrypoint-initdb.d/2.sql
-      - ./database/django_auth.sql:/docker-entrypoint-initdb.d/3.sql
-      - ./scripts:/scripts
-    healthcheck:
-      test: pg_isready -U user-dev -d academy_db
-      interval: 3s
-      timeout: 1s
-      retries: 20
+    my-postgres:
+        image: jderobot/robotics-database:latest
+        container_name: universe_db
+        environment:
+            POSTGRES_DB: academy_db
+            POSTGRES_USER: user-dev
+            POSTGRES_PASSWORD: robotics-academy-dev
+        ports:
+            - "5432:5432"
+        volumes:
+            - ./RoboticsInfrastructure/database/universes.sql:/docker-entrypoint-initdb.d/1.sql
+            - ./database/exercises/db.sql:/docker-entrypoint-initdb.d/2.sql
+            - ./database/django_auth.sql:/docker-entrypoint-initdb.d/3.sql
+            - ./scripts:/scripts
+        healthcheck:
+            test: pg_isready -U user-dev -d academy_db
+            interval: 3s
+            timeout: 1s
+            retries: 20
 
-  robotics-academy:
-    image: jderobot/robotics-academy:latest
-    container_name: developer-container
-    command:  "-s"
-    deploy:
-      resources:
-        reservations:
-          devices:
-            - driver: nvidia
-              count: all
-              capabilities: [gpu]
-    runtime: nvidia
-    environment:
-      - NVIDIA_VISIBLE_DEVICES=all
-      - NVIDIA_DRIVER_CAPABILITIES=all
-    ports:
-      - "7164:7164"
-      - "7163:7163"
-      - "6080:6080"
-      - "1108:1108"
-    volumes:
-      - type: bind
-        source: ./
-        target: /RoboticsAcademy
-      - type: bind
-        source: ./src
-        target: /RoboticsApplicationManager
-    tty: true
-    stdin_open: true
-    depends_on: 
-      my-postgres:
-        condition: service_healthy
+    robotics-academy:
+        image: jderobot/robotics-academy:latest
+        container_name: developer-container
+        command: "-s"
+        deploy:
+            resources:
+                reservations:
+                    devices:
+                        - driver: nvidia
+                          count: all
+                          capabilities: [gpu]
+        environment:
+            - NVIDIA_VISIBLE_DEVICES=all
+            - NVIDIA_DRIVER_CAPABILITIES=all
+        devices:
+            - "/dev/dri:/dev/dri"
+        ports:
+            - "7164:7164"
+            - "7163:7163"
+            - "6080:6080"
+            - "1108:1108"
+        volumes:
+            - type: bind
+              source: ./
+              target: /RoboticsAcademy
+            - type: bind
+              source: ./src
+              target: /RoboticsApplicationManager
+        tty: true
+        stdin_open: true
+        depends_on:
+            my-postgres:
+                condition: service_healthy
+        links:
+            - my-postgres

exercises/static/exercises/assets/img/end_to_end_visual_control_teaser.png

@@ -0,0 +1,9 @@
+model_dir_path = "/workspace/code/model.onnx"
+
+
+# This file is part of the Human Detection ROS2 package.
+def model_path_func() -> str:
+    return model_dir_path
+
+
+model_path = model_path_func()

exercises/static/exercises/end_to_end_visual_control/python_template/ros2_humble/model.py

@@ -5,16 +5,27 @@
 import rclpy
 from gui_interfaces.general.measuring_threading_gui import MeasuringThreadingGUI
 from console_interfaces.general.console import start_console
+
+from hal_interfaces.general.camera import CameraNode
+from cv_bridge import CvBridge
 from hal_interfaces.general.odometry import OdometryNode
 from lap import Lap
+from sensor_msgs.msg import Image
+from rclpy.node import Node
 
 import sys
 
 sys.path.insert(0, "/RoboticsApplicationManager")
 
 from manager.ram_logging.log_manager import LogManager
 
-# Graphical User Interface Class
+
+class WebGUIImagePublisher(Node):
+    """Internal publisher to create /webgui_image topic"""
+
+    def __init__(self):
+        super().__init__("webgui_image_publisher_internal")
+        self.publisher = self.create_publisher(Image, "/webgui_image", 10)
 
 
 class WebGUI(MeasuringThreadingGUI):
@@ -25,22 +36,67 @@ def __init__(self, host="ws://127.0.0.1:2303"):
         self.image_to_be_shown_updated = False
         self.image_show_lock = threading.Lock()
 
-        # Payload vars
+        if not rclpy.ok():
+            rclpy.init()
+
+        self.webgui_publisher = WebGUIImagePublisher()
+        self.camera_node = None
+        self.auto_image_mode = False
+        self._setup_auto_mode()
+
         self.payload = {"image": "", "lap": "", "map": ""}
-        # TODO: maybe move this to HAL and have it be hybrid
+        self.bridge = CvBridge()
+
         self.pose3d_object = OdometryNode("/odom")
-        executor = rclpy.executors.MultiThreadedExecutor()
-        executor.add_node(self.pose3d_object)
-        executor_thread = threading.Thread(target=executor.spin, daemon=True)
-        executor_thread.start()
+
+        self.executor = rclpy.executors.MultiThreadedExecutor()
+        self.executor.add_node(self.webgui_publisher)
+        if self.camera_node:
+            self.executor.add_node(self.camera_node)
+        self.executor.add_node(self.pose3d_object)
+        self.executor_thread = threading.Thread(target=self.executor.spin, daemon=True)
+        self.executor_thread.start()
+
         self.lap = Lap(self.pose3d_object)
 
+        if self.auto_image_mode:
+            self.auto_image_thread = threading.Thread(
+                target=self._unified_image_loop, daemon=True
+            )
+            self.auto_image_thread.start()
+
         self.start()
 
-    # Process incoming messages to the GUI
-    def gui_in_thread(self, ws, message):
+    def _setup_auto_mode(self):
+        """Set up automatic image subscription"""
+        try:
+            temp_node = rclpy.create_node("topic_checker_temp")
+            topic_names_and_types = temp_node.get_topic_names_and_types()
+            topic_names = [topic_name for topic_name, _ in topic_names_and_types]
+
+            if "/webgui_image" in topic_names:
+                self.camera_node = CameraNode("/webgui_image")
+                self.auto_image_mode = True
+
+            temp_node.destroy_node()
+
+        except Exception:
+            pass
 
-        # In this case, incoming msgs can only be acks
+    def _unified_image_loop(self):
+        """Unified image handling loop"""
+        while True:
+            try:
+                if self.camera_node:
+                    image = self.camera_node.getImage()
+                    if image is not None:
+                        self.showImage(image.data)
+
+                threading.Event().wait(0.033)  # ~30 FPS
+            except Exception:
+                threading.Event().wait(1.0)
+
+    def gui_in_thread(self, ws, message):
         if "ack" in message:
             with self.ack_lock:
                 self.ack = True
@@ -49,44 +105,35 @@ def gui_in_thread(self, ws, message):
             self.lap.unpause()
         elif "pause" in message:
             self.lap.pause()
-        else:
-            LogManager.logger.error("Unsupported msg")
 
-    # Prepares and sends a map to the websocket server
     def update_gui(self):
-
         payload = self.payloadImage()
         self.payload["image"] = json.dumps(payload)
 
-        # Payload Lap Message
         lapped = self.lap.check_threshold()
         self.payload["lap"] = ""
-        if lapped != None:
+        if lapped is not None:
             self.payload["lap"] = str(lapped)
 
-        # Payload Map Message
         pose = self.pose3d_object.getPose3d()
         pos_message = str((pose.x, pose.y))
         self.payload["map"] = pos_message
 
         message = json.dumps(self.payload)
         self.send_to_client(message)
 
-    # Function to prepare image payload
-    # Encodes the image as a JSON string and sends through the WS
     def payloadImage(self):
         with self.image_show_lock:
             image_to_be_shown_updated = self.image_to_be_shown_updated
             image_to_be_shown = self.image_to_be_shown
 
-        image = image_to_be_shown
         payload = {"image": "", "shape": ""}
 
-        if not image_to_be_shown_updated:
+        if not image_to_be_shown_updated or image_to_be_shown is None:
             return payload
 
-        shape = image.shape
-        frame = cv2.imencode(".JPEG", image)[1]
+        shape = image_to_be_shown.shape
+        frame = cv2.imencode(".JPEG", image_to_be_shown)[1]
         encoded_image = base64.b64encode(frame)
 
         payload["image"] = encoded_image.decode("utf-8")
@@ -97,20 +144,42 @@ def payloadImage(self):
 
         return payload
 
-    # Function for student to call
     def showImage(self, image):
+        """Single point of entry for all images"""
         with self.image_show_lock:
             self.image_to_be_shown = image
             self.image_to_be_shown_updated = True
 
+    def get_image_mode(self):
+        return {
+            "auto_mode": self.auto_image_mode,
+            "topic_subscribed": "/webgui_image" if self.auto_image_mode else None,
+            "manual_mode_available": True,
+        }
+
 
+# Create GUI instance directly
 host = "ws://127.0.0.1:2303"
 gui = WebGUI(host)
-
-# Redirect the console
 start_console()
 
 
-# Expose to the user
 def showImage(image):
-    gui.showImage(image)
+    """Display an image in the GUI"""
+    if gui is not None:
+        gui.showImage(image)
+
+
+def get_image_mode():
+    if gui is not None:
+        return gui.get_image_mode()
+    return {"auto_mode": False, "topic_subscribed": None, "manual_mode_available": True}
+
+
+_gui = gui
+_gui_lock = threading.Lock()
+
+
+def get_gui():
+    """Backward compatibility function"""
+    return gui