Merge pull request #3717 from JdeRobot/ros2-direct-e2e

Ros2 direct e2e

Author: javizqh

exercises/end_to_end_visual_control/python_template/WebGUI.py

@@ -2,45 +2,65 @@
 import base64
 import json
 import threading
+import sys
 import rclpy
+from rclpy.node import Node
+from rclpy.executors import MultiThreadedExecutor
+from sensor_msgs.msg import Image
+from cv_bridge import CvBridge
+
 from gui_interfaces.general.measuring_threading_gui import MeasuringThreadingGUI
 from console_interfaces.general.console import start_console
-from hal_interfaces.general.odometry import OdometryNode
 from lap import Lap
+from hal_interfaces.general.odometry import OdometryNode
 
-import sys
 
-sys.path.insert(0, "/RoboticsApplicationManager")
+class WebGUINode(Node):
+    def __init__(self, gui_instance):
+        super().__init__("webgui_end_to_end")
+        self.gui = gui_instance
+        self.bridge = CvBridge()
 
-from robotics_application_manager import LogManager
+        self.create_subscription(Image, "/webgui/image", self.img_cb, 10)
 
-# Graphical User Interface Class
+    def img_cb(self, msg):
+        try:
+            cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
+            with self.gui.image_show_lock:
+                self.gui.image_to_be_shown = cv_image
+                self.gui.image_to_be_shown_updated = True
+        except Exception:
+            pass
 
 
 class WebGUI(MeasuringThreadingGUI):
     def __init__(self, host="ws://127.0.0.1:2303"):
         super().__init__(host)
 
+        if not rclpy.ok():
+            rclpy.init()
+
         self.image_to_be_shown = None
         self.image_to_be_shown_updated = False
         self.image_show_lock = threading.Lock()
 
-        # Payload vars
         self.payload = {"image": "", "lap": "", "map": ""}
-        # TODO: maybe move this to HAL and have it be hybrid
-        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.lap = Lap(self.pose3d_object)
+
+        self.ros_node = WebGUINode(self)
+        self.odom_node = OdometryNode("/odom", "webgui_odometry")
+
+        self.executor = MultiThreadedExecutor()
+        self.executor.add_node(self.ros_node)
+        self.executor.add_node(self.odom_node)
+
+        self.executor_thread = threading.Thread(target=self.executor.spin, daemon=True)
+        self.executor_thread.start()
+
+        self.lap = Lap(self.odom_node)
 
         self.start()
 
-    # Process incoming messages to the GUI
     def gui_in_thread(self, ws, message):
-
-        # In this case, incoming msgs can only be acks
         if "ack" in message:
             with self.ack_lock:
                 self.ack = True
@@ -51,55 +71,46 @@ 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 is not None:
             self.payload["lap"] = str(lapped)
 
-        # Payload Map Message
-        pose = self.pose3d_object.getPose3d()
+        pose = self.odom_node.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:
             return payload
 
-        shape = image.shape
-        frame = cv2.imencode(".JPEG", image)[1]
-        encoded_image = base64.b64encode(frame)
+        if image_to_be_shown is not None:
+            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")
-        payload["shape"] = shape
+            payload["image"] = encoded_image.decode("utf-8")
+            payload["shape"] = shape
 
         with self.image_show_lock:
             self.image_to_be_shown_updated = False
 
         return payload
 
-    # Function for student to call
     def showImage(self, image):
         with self.image_show_lock:
             self.image_to_be_shown = image
@@ -108,11 +119,8 @@ def showImage(self, image):
 
 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)

exercises/end_to_end_visual_control/python_template/model.py

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