Integrates a UDP bridge and changes to be compatible with it

README.md

@@ -36,6 +36,21 @@ emio.printStatus() # Print the status of the robot
 emio.disconnect()  # Close the connection to the robot
 ```
 
+### UDP Bridge
+To use the UDP bridge, you can directly call the tool: 
+
+```bash
+python -m emioapi startUDP
+```
+
+This will start a UDP bridge that will listen and send data according to the parameters in file [`emioapi/udp_bridge/udp_bridge_params.py`](./emioapi/udp_bridge/udp_bridge_params.py)
+
+You also have options that you can set at the call. List them using: 
+```bash
+python -m emioapi startUDP -h
+```
+
+
 ## For Developers
 The documentation is generated using [pydoc-markdown](https://pypi.org/project/pydoc-markdown/). To generate the documentation, you need to install `pydoc-markdown`:
 

emioapi/__main__.py

@@ -1,12 +1,37 @@
-import emioapi
-import sys
+"""
+## Emio API Tools
 
-from emioapi._logging_config import logger
+This module provides command-line tools for working with Emio devices, including camera 
+calibration and starting a UDP bridge for real-time communication between the Emio robot 
+and a remote host (e.g., Simulink).
+
+To use these tools, run the following command in your terminal:
+```bash
+python -m emioapi <command> [options]
+```
+### Available Commands
+- `calibrate`: Calibrate the Emio camera. This command will open the camera feed
+    and allow you to perform the calibration process. The camera will be automatically closed after calibration.
+- `startUDP`: Start a UDP bridge for motor/camera data. 
+    This command will launch a UDP bridge that sends the camera's markers and motors positions and receives motor commands from a remote host. 
+    The bridge can be configured with various options such as FPS, number of markers, remote IP/port, etc.
+
+You can run each command with the `--help` flag to see the specific options available for that command. For example:
+```bash
+python -m emioapi --help
+python -m emioapi calibrate --help
+python -m emioapi startUDP --help
+```
+
+"""
 
 def calibrate():
     """
-    Calibrate the camera of the first Emio camera found
+    Calibrate the camera of the first Emio camera found. For more informations about the calibration process, please refer to the EmioCamera.calibrate() method documentation.
     """
+    import emioapi
+    from emioapi._logging_config import logger 
+
     camera = emioapi.EmioCamera(show=True)
     print("Available cameras:", emioapi.EmioCamera.listCameras())
 
@@ -32,14 +57,108 @@ def calibrate():
     else:
         print("Failed to open camera.")
 
+def startUDP(args):
+    """
+    Start a UDP bridge configured with the parameters found in args or will default to `emioapi/udpbridge/udpbridge_params.py`.
+
+    A handshake is done at the beginning to ensure that the remote host is ready to receive data. It should follow the same protocol describded below with dummy data.
+
+    The sequence number is a simple counter that is incremented at each frame. It is used by the process_motors process to make sure that the remote is synchronized.
+
+    The protocol is as follows:
+    - The bridge sends a packet made of a sequence number, the four motors positions and followed by the marker(s) position(s) (x, y, z)
+    - The remote host should reply with a packet containing the four motors positions to send to the Emio robot.
+
+    """
+    import emioapi.udpbridge.udpbridge as udpBdrige
+    config = udpBdrige.UDPBridgeConfig(
+        fps = args.fps,
+        nb_markers = args.nb_markers,
+        side = args.side,
+        sort = args.sort,
+        remote_ip = args.remote_ip,
+        remote_port = args.remote_port,
+        local_port = args.local_port,
+        bind_port = args.bind_port,
+        recv_timeout = args.recv_timeout,
+        camera_only = args.camera_only,
+        motors_only = args.motors_only
+    )
+
+    print("-"*50)
+    print(f"Starting UDP bridge with config: {config}")
+    print("-"*50)
+
+    udpBdrige.startUDPbridge(config)
+
+
+def _parse_args():
+
+    import argparse
+    import emioapi.udpbridge.udpbridgeparams as prm
+
+    p = argparse.ArgumentParser(
+        description="Emio API tools for Emio",
+        prog="emioapi",
+        formatter_class=argparse.ArgumentDefaultsHelpFormatter
+    )
+
+    subparsers = p.add_subparsers(
+        title="Available Commands", 
+        dest="command", 
+        required=True
+    )
+
+    # --- Subparser for 'calibrate' command ---
+    parser_calibrate = subparsers.add_parser("calibrate", help="Calibrate the Emio camera.",
+        formatter_class=argparse.ArgumentDefaultsHelpFormatter)
+    parser_calibrate.description = calibrate.__doc__
+
+
+    # --- Subparser for 'startUDP' command ---
+    parser_udp = subparsers.add_parser("startUDP", help="Start a UDP bridge for motor/camera data.",
+        formatter_class=argparse.ArgumentDefaultsHelpFormatter)
+
+    parser_udp.description = startUDP.__doc__
+
+    # Add the specific arguments needed for startUDP here
+    parser_udp.add_argument("--fps",       type=int, default=prm.fps, help="Frames per second (e.g., 30)")
+    parser_udp.add_argument("--nb-markers",  type=int, default=prm.nb_markers, help="Number of markers to process")
+    parser_udp.add_argument("--side",       choices=["top", "front", "plane"], default=prm.side, help="Camera side view")
+    parser_udp.add_argument("--sort",        choices=["y", "z"], default=prm.sort, help="Sorting axis")
+    parser_udp.add_argument("--remote-ip",    type=str, default=prm.remote_ip, help="Remote IP address")
+    parser_udp.add_argument("--remote-port",  type=int, default=prm.remote_port, help="Remote Port")
+    parser_udp.add_argument("--local-port",    type=int, default=prm.local_port, help="Local Port")
+    parser_udp.add_argument("--bind-port",      type=int, default=prm.bind_port, help="Bind port for local communication")
+    parser_udp.add_argument("--recv-timeout",  type=float, default=prm.recv_timeout, help="Receive timeout in seconds")
+    parser_udp.add_argument("--camera-only",  action=argparse.BooleanOptionalAction, default=False, help="Only send the markers position without waiting for the motors command. The motors positions will be sent as 0. And no command will be applied to the motors.")
+    parser_udp.add_argument("--motors-only",  action=argparse.BooleanOptionalAction, default=False, help="Only send the motors position without waiting for the camera data")
+
+    try:
+        # Parse the arguments
+        args = p.parse_args()
+
+        print(f"Command chosen: {args.command}" + (f" with parameters: {vars(args)}" if args.command == "startUDP" else ""))
+
+        # Execute the function associated with the chosen command
+        if args.command == "calibrate":
+            calibrate()
+        elif args.command == "startUDP":
+            startUDP(args)
+        else:
+            print(f"Unknown command: {args.command}")
+    except:
+        p.print_help()
+        print()
+        parser_calibrate.print_help()
+        print()
+        parser_udp.print_help()
+        pass
+
 
 if __name__ == "__main__":
-    if len(sys.argv) > 1 and sys.argv[1] == 'calibrate':
-        calibrate()
-    else:
-        print("Available functions from emioapi tool:")
-        for name in dir(sys.modules[__name__]):
-            if not name.startswith("_"):
-                attr = getattr(sys.modules[__name__], name)
-                if callable(attr):
-                    print(f"  {name} - {attr.__doc__}")
+    try:
+        args = _parse_args()
+    except Exception as e:
+        import traceback
+        print(f"An error happened: {traceback.format_exc()}")

emioapi/_depthcamera.py

@@ -1,6 +1,4 @@
-import os
 import json
-from time import sleep
 import time
 from enum import Enum
 
@@ -9,7 +7,7 @@
 import pyrealsense2 as rs
 
 from ._camerafeedwindow import CameraFeedWindow
-from ._positionestimation import PositionEstimation, image_pixel_to_mm, CONFIG_FILENAME
+from ._positionestimation import PositionEstimation, CONFIG_FILENAME
 from emioapi._logging_config import logger
 
 DEFAULT_CAMERA_PARAMS = {"hue_h": 90, "hue_l": 36, "sat_h": 255, "sat_l": 138, "value_h": 255, "value_l": 35, "erosion_size": 0, "area": 100}
@@ -68,6 +66,7 @@ class DepthCamera:
     parameter = {}
     tracking = False
     trackers_pos = []
+    trackers_pos_image = []
     maskWindow = None
     frameWindow = None
     hsvWindow = None
@@ -77,6 +76,7 @@ class DepthCamera:
     maskFrame = None
     frame: np.ndarray = None
     depth_frame: np.ndarray = None
+    depth_rsframe: np.ndarray = None
     depth_max = 430
     depth_min = 2
     calibration_status = CalibrationStatusEnum.NOT_CALIBRATED
@@ -123,6 +123,7 @@ def __init__(self,
             return
 
         self.trackers_pos = []
+        self.trackers_pos_image = []
 
         if parameter:
             self.parameter = parameter
@@ -290,19 +291,24 @@ def get_frame(self):
         color_frame = frames.get_color_frame()
 
         if not depth_frame or not color_frame:
-            return False, color_frame, depth_frame
+            return False
 
         # Convert images to numpy arrays
-        depth_image = np.asanyarray(depth_frame.get_data())
-        color_image = np.asanyarray(color_frame.get_data())
-        return True, color_image, depth_image, depth_frame
+        self.depth_frame = np.asanyarray(depth_frame.get_data())
+        self.frame = np.asanyarray(color_frame.get_data())
+        self.depth_rsframe = depth_frame
 
+        return True
 
-    def update(self):
-        ret, self.frame, self.depth_frame, depth_rsframe = self.get_frame()
 
+    def update(self):
+        ret = self.get_frame()
         if ret is False:
             return
+        self.process_frame()
+
+    def process_frame(self):
+
         # if frame is read correctly ret is True
 
         self.hsvFrame = cv.cvtColor(self.frame, cv.COLOR_BGR2HSV)
@@ -333,6 +339,7 @@ def update(self):
                 areas = [cv.contourArea(cnt) for cnt in contours]
 
                 self.trackers_pos = []
+                self.trackers_pos_image = []
                 for i, a in enumerate(areas):
                     if a > self.parameter['area']:
                         x, y = compute_contour_center(contours[i])
@@ -341,6 +348,7 @@ def update(self):
                         depth = compute_median_depth(contours[i], self.depth_frame) if self.depth_frame[y, x] == 0 else self.depth_frame[y, x]
                         worldx, worldy, worldz = self.position_estimator.camera_image_to_simulation(x, y, depth)
                         self.trackers_pos.append([worldx, worldy, worldz])
+                        self.trackers_pos_image.append([x, y, depth])
 
                         cv.drawContours(marker_mask, [contours[i]], -1, color=255, thickness=-1)
                         for frame in [self.hsvFrame, self.frame]:
@@ -352,7 +360,7 @@ def update(self):
                             cv.drawContours(self.frame, [contours[i]], -1, (255, 255, 0), 3)
 
         if self.compute_point_cloud:
-            points = self.pc.calculate(depth_rsframe)
+            points = self.pc.calculate(self.depth_rsframe)
             v = points.get_vertices()
             self.point_cloud = np.asanyarray(v).view(np.float32).reshape(-1, 3)  # xyz
 
@@ -370,7 +378,7 @@ def update(self):
                 self.hsvWindow.set_frame(self.hsvFrame)
 
             if self.depthWindow is not None and self.depthWindow.running:
-                colorized = np.asanyarray(rs.colorizer().colorize(depth_rsframe).get_data())
+                colorized = np.asanyarray(rs.colorizer().colorize(self.depth_rsframe).get_data())
                 self.depthWindow.set_frame(colorized)
 
             self.rootWindow.update()
@@ -395,4 +403,4 @@ def run_loop(self):
                 self.rootWindow.update()
             self.update()
 
-        self.close()
+        self.close()

emioapi/_positionestimation.py

@@ -343,7 +343,36 @@ def calibrate(self, frame, depth_image, aggregate, window=None)-> bool:
             window.set_frame(frame)
 
         return True
+
+
+    def camera_image_to_simulation_plane_intersection(self, x: int, y: int, plane_n: np.ndarray, plane_d: float) -> list[float]:
+        """
+        Calculate the position of the object in our frame space by projecting the ray from the camera to the image point on a plane.
+
+        Args
+        x,y: int
+            The pixel coordinates
+
+        plane_n: np.ndarray
+            The normal vector of the plane
 
+        plane_d: float
+            The distance of the plane from the origin along its normal vector
+
+        Return:
+            position: numpy.ndarray
+                The real world coordinates of the object in the Emio frame space
+        """
+        ray_world = self.camera_image_to_simulation(x, y, 1.0) - self.t
+        camera_pos_world = self.t
+        denom = plane_n.dot(ray_world)
+
+        if abs(denom) < 1e-6:
+            raise ValueError("Ray is parallel to the plane")
+        s = - (plane_n.dot(camera_pos_world) + plane_d) / denom
+        result = camera_pos_world + s * ray_world
+        return [result[0], result[1], result[2]]
+
 
     def camera_image_to_simulation(self, x: int, y: int, depth: float) -> list[float]:
         """

emioapi/emioapi.py

@@ -11,12 +11,6 @@
 pip install git+https://github.com/SofaComplianceRobotics/Emio.API.git@main
 ```
 
-## Camera Calibration Tool
-Once emioapi installed, you can directly call the calibration tool to calibrate the camera of Emio.
-From a terminal with a Python having the emioapi moduules intalled, run:
-```bash
-python -m emioapi calibrate
-```
 """
 
 from dataclasses import field