Adding Agilex Piper node, PyOrbbeckSDK node, Agilex UGV node

node-hub/dora-piper/README.md

@@ -0,0 +1,9 @@
+# Dora Node (Experimental) to communicate with Agilex Piper SDK
+
+This node is used to communicate with piper.
+
+Make sure to follow both the setup script and installation script from https://github.com/agilexrobotics/piper_sdk
+
+# To setup the arm
+
+Make sure that the can bus is activated for both arm and leader arms are not connected.

node-hub/dora-piper/dora_piper/__init__.py

@@ -0,0 +1,11 @@
+import os
+
+# Define the path to the README file relative to the package directory
+readme_path = os.path.join(os.path.dirname(os.path.dirname(__file__)), "README.md")
+
+# Read the content of the README file
+try:
+    with open(readme_path, "r", encoding="utf-8") as f:
+        __doc__ = f.read()
+except FileNotFoundError:
+    __doc__ = "README file not found."

node-hub/dora-piper/dora_piper/main.py

@@ -0,0 +1,112 @@
+from piper_sdk import C_PiperInterface
+from dora import Node
+import pyarrow as pa
+import os
+import time
+
+TEACH_MODE = (
+    os.getenv("TEACH_MODE", "False") == "True"
+    or os.getenv("TEACH_MODE", "False") == "true"
+)
+
+
+def enable_fun(piper: C_PiperInterface):
+    """
+    使能机械臂并检测使能状态,尝试5s,如果使能超时则退出程序
+    """
+    enable_flag = False
+    # 设置超时时间（秒）
+    timeout = 5
+    # 记录进入循环前的时间
+    start_time = time.time()
+    elapsed_time_flag = False
+    while not (enable_flag):
+        elapsed_time = time.time() - start_time
+        print("--------------------")
+        enable_flag = (
+            piper.GetArmLowSpdInfoMsgs().motor_1.foc_status.driver_enable_status
+            and piper.GetArmLowSpdInfoMsgs().motor_2.foc_status.driver_enable_status
+            and piper.GetArmLowSpdInfoMsgs().motor_3.foc_status.driver_enable_status
+            and piper.GetArmLowSpdInfoMsgs().motor_4.foc_status.driver_enable_status
+            and piper.GetArmLowSpdInfoMsgs().motor_5.foc_status.driver_enable_status
+            and piper.GetArmLowSpdInfoMsgs().motor_6.foc_status.driver_enable_status
+        )
+        print("使能状态:", enable_flag)
+        piper.EnableArm(7)
+        piper.GripperCtrl(0, 1000, 0x01, 0)
+        print("--------------------")
+        # 检查是否超过超时时间
+        if elapsed_time > timeout:
+            print("超时....")
+            elapsed_time_flag = True
+            enable_flag = True
+            break
+        time.sleep(1)
+    if elapsed_time_flag:
+        print("程序自动使能超时,退出程序")
+        print("If you have this issue, you should probably restart your computer")
+        exit(0)
+
+
+def main():
+    elapsed_time = time.time()
+    CAN_BUS = os.getenv("CAN_BUS", "")
+    piper = C_PiperInterface(CAN_BUS)
+    piper.ConnectPort()
+
+    if TEACH_MODE is False:
+        # piper.MotionCtrl_3(0, 0, 0, 0x00)#位置速度模式
+        piper.EnableArm(7)
+        enable_fun(piper=piper)
+        piper.GripperCtrl(0, 1000, 0x01, 0)
+    factor = 57324.840764  # 1000*180/3.14
+    time.sleep(2)
+    node = Node()
+
+    for event in node:
+        if event["type"] == "INPUT":
+            if event["id"] != "action":
+                joint = piper.GetArmJointMsgs()
+                gripper = piper.GetArmGripperMsgs()
+
+                joint_value = []
+                joint_value += [joint.joint_state.joint_1.real / factor]
+                joint_value += [joint.joint_state.joint_2.real / factor]
+                joint_value += [joint.joint_state.joint_3.real / factor]
+                joint_value += [joint.joint_state.joint_4.real / factor]
+                joint_value += [joint.joint_state.joint_5.real / factor]
+                joint_value += [joint.joint_state.joint_6.real / factor]
+                joint_value += [gripper.gripper_state.grippers_angle / 1000 / 1000 / 4]
+
+                node.send_output("jointstate", pa.array(joint_value, type=pa.float32()))
+            else:
+
+                # Do not push to many commands to fast. Limiting it to 20Hz
+                # This is due to writing on a moving arm might fail the can bus.
+                if time.time() - elapsed_time > 0.05:
+                    elapsed_time = time.time()
+                else:
+                    continue
+
+                position = event["value"].to_numpy()
+                joint_0 = round(position[0] * factor)
+                joint_1 = round(position[1] * factor)
+                joint_2 = round(position[2] * factor)
+                joint_3 = round(position[3] * factor)
+                joint_4 = round(position[4] * factor)
+                joint_5 = round(position[5] * factor)
+                joint_6 = round(position[6] * 1000 * 1000 * 12)
+
+                piper.MotionCtrl_2(0x01, 0x01, 50, 0x00)
+                piper.JointCtrl(joint_0, joint_1, joint_2, joint_3, joint_4, joint_5)
+                piper.GripperCtrl(abs(joint_6), 1000, 0x01, 0)
+                piper.MotionCtrl_2(0x01, 0x01, 50, 0x00)
+        elif event["type"] == "STOP":
+
+            # Waiting for the arm to stop moving before stopping the node
+            time.sleep(5)
+            break
+
+
+if __name__ == "__main__":
+    main()

node-hub/dora-piper/pyproject.toml

@@ -0,0 +1,21 @@
+[tool.poetry]
+name = "dora-piper"
+version = "0.3.6"
+authors = ["Haixuan Xavier Tao <[email protected]>"]
+description = "Dora Node for using Agilex piper"
+readme = "README.md"
+
+packages = [{ include = "dora_piper" }]
+
+[tool.poetry.dependencies]
+dora-rs = "^0.3.6"
+python = "^3.7"
+piper_sdk = "^0.0.7"
+
+
+[tool.poetry.scripts]
+dora-piper = "dora_piper.main:main"
+
+[build-system]
+requires = ["poetry-core>=1.8.0"]
+build-backend = "poetry.core.masonry.api"

node-hub/dora-piper/tests/test_piper.py

@@ -0,0 +1,5 @@
+def test_import_main():
+    from piper_sdk import C_PiperInterface
+
+    ## Test piper installation
+    assert C_PiperInterface

node-hub/dora-pyorbbecksdk/README.md

@@ -0,0 +1,33 @@
+# Dora Node to communicate with Pyorbbecksdk
+
+Please follow the installation instruction in: https://github.com/orbbec/pyorbbecsdk
+
+## Hardware Products Supported by Python Pyorbbecksdk SDK
+
+| **products list** | **firmware version**        |
+| ----------------- | --------------------------- |
+| Gemini 335        | 1.2.20                      |
+| Gemini 335L       | 1.2.20                      |
+| Gemini 336        | 1.2.20                      |
+| Gemini 336L       | 1.2.20                      |
+| Femto Bolt        | 1.0.6/1.0.9                 |
+| Femto Mega        | 1.1.7/1.2.7                 |
+| Gemini 2 XL       | Obox: V1.2.5 VL:1.4.54      |
+| Astra 2           | 2.8.20                      |
+| Gemini 2 L        | 1.4.32                      |
+| Gemini 2          | 1.4.60 /1.4.76              |
+| Astra+            | 1.0.22/1.0.21/1.0.20/1.0.19 |
+| Femto             | 1.6.7                       |
+| Femto W           | 1.1.8                       |
+| DaBai             | 2436                        |
+| DaBai DCW         | 2460                        |
+| DaBai DW          | 2606                        |
+| Astra Mini Pro    | 1007                        |
+| Gemini E          | 3460                        |
+| Gemini E Lite     | 3606                        |
+| Gemini            | 3.0.18                      |
+| Astra Mini S Pro  | 1.0.05                      |
+
+## Contribution
+
+Please contribute to this node if you see missing features.

node-hub/dora-pyorbbecksdk/dora_pyorbbecksdk/__init__.py

@@ -0,0 +1,11 @@
+import os
+
+# Define the path to the README file relative to the package directory
+readme_path = os.path.join(os.path.dirname(os.path.dirname(__file__)), "README.md")
+
+# Read the content of the README file
+try:
+    with open(readme_path, "r", encoding="utf-8") as f:
+        __doc__ = f.read()
+except FileNotFoundError:
+    __doc__ = "README file not found."

node-hub/dora-pyorbbecksdk/dora_pyorbbecksdk/main.py

@@ -0,0 +1,155 @@
+# ******************************************************************************
+#  Copyright (c) 2023 Orbbec 3D Technology, Inc
+#
+#  Licensed under the Apache License, Version 2.0 (the "License");
+#  you may not use this file except in compliance with the License.
+#  You may obtain a copy of the License at
+#
+#      http:# www.apache.org/licenses/LICENSE-2.0
+#
+#  Unless required by applicable law or agreed to in writing, software
+#  distributed under the License is distributed on an "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+#  See the License for the specific language governing permissions and
+#  limitations under the License.
+# ******************************************************************************
+import cv2
+import os
+import numpy as np
+
+try:
+    from pyorbbecsdk import Context
+    from pyorbbecsdk import Config
+    from pyorbbecsdk import OBError
+    from pyorbbecsdk import OBSensorType, OBFormat
+    from pyorbbecsdk import Pipeline, FrameSet
+    from pyorbbecsdk import VideoStreamProfile
+    from pyorbbecsdk import VideoFrame
+except ImportError as err:
+    print(
+        "Please install pyorbbecsdk first by following the instruction at: https://github.com/orbbec/pyorbbecsdk"
+    )
+    raise err
+
+
+def yuyv_to_bgr(frame: np.ndarray, width: int, height: int) -> np.ndarray:
+    yuyv = frame.reshape((height, width, 2))
+    bgr_image = cv2.cvtColor(yuyv, cv2.COLOR_YUV2BGR_YUY2)
+    return bgr_image
+
+
+def uyvy_to_bgr(frame: np.ndarray, width: int, height: int) -> np.ndarray:
+    uyvy = frame.reshape((height, width, 2))
+    bgr_image = cv2.cvtColor(uyvy, cv2.COLOR_YUV2BGR_UYVY)
+    return bgr_image
+
+
+def i420_to_bgr(frame: np.ndarray, width: int, height: int) -> np.ndarray:
+    y = frame[0:height, :]
+    u = frame[height : height + height // 4].reshape(height // 2, width // 2)
+    v = frame[height + height // 4 :].reshape(height // 2, width // 2)
+    yuv_image = cv2.merge([y, u, v])
+    bgr_image = cv2.cvtColor(yuv_image, cv2.COLOR_YUV2BGR_I420)
+    return bgr_image
+
+
+def nv21_to_bgr(frame: np.ndarray, width: int, height: int) -> np.ndarray:
+    y = frame[0:height, :]
+    uv = frame[height : height + height // 2].reshape(height // 2, width)
+    yuv_image = cv2.merge([y, uv])
+    bgr_image = cv2.cvtColor(yuv_image, cv2.COLOR_YUV2BGR_NV21)
+    return bgr_image
+
+
+def nv12_to_bgr(frame: np.ndarray, width: int, height: int) -> np.ndarray:
+    y = frame[0:height, :]
+    uv = frame[height : height + height // 2].reshape(height // 2, width)
+    yuv_image = cv2.merge([y, uv])
+    bgr_image = cv2.cvtColor(yuv_image, cv2.COLOR_YUV2BGR_NV12)
+    return bgr_image
+
+
+def frame_to_bgr_image(frame: VideoFrame):
+    width = frame.get_width()
+    height = frame.get_height()
+    color_format = frame.get_format()
+    data = np.asanyarray(frame.get_data())
+    image = np.zeros((height, width, 3), dtype=np.uint8)
+    if color_format == OBFormat.RGB:
+        image = np.resize(data, (height, width, 3))
+        image = cv2.cvtColor(image, cv2.COLOR_RGB2BGR)
+    elif color_format == OBFormat.BGR:
+        image = np.resize(data, (height, width, 3))
+        image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
+    elif color_format == OBFormat.YUYV:
+        image = np.resize(data, (height, width, 2))
+        image = cv2.cvtColor(image, cv2.COLOR_YUV2BGR_YUYV)
+    elif color_format == OBFormat.MJPG:
+        image = cv2.imdecode(data, cv2.IMREAD_COLOR)
+    elif color_format == OBFormat.I420:
+        image = i420_to_bgr(data, width, height)
+        return image
+    elif color_format == OBFormat.NV12:
+        image = nv12_to_bgr(data, width, height)
+        return image
+    elif color_format == OBFormat.NV21:
+        image = nv21_to_bgr(data, width, height)
+        return image
+    elif color_format == OBFormat.UYVY:
+        image = np.resize(data, (height, width, 2))
+        image = cv2.cvtColor(image, cv2.COLOR_YUV2BGR_UYVY)
+    else:
+        print("Unsupported color format: {}".format(color_format))
+        return None
+    return image
+
+
+from dora import Node
+import pyarrow as pa
+
+ESC_KEY = 27
+
+DEVICE_INDEX = int(os.getenv("DEVICE_INDEX", "0"))
+
+
+def main():
+    node = Node()
+    config = Config()
+    ctx = Context()
+    device_list = ctx.query_devices()
+    device = device_list.get_device_by_index(int(DEVICE_INDEX))
+    pipeline = Pipeline(device)
+    profile_list = pipeline.get_stream_profile_list(OBSensorType.COLOR_SENSOR)
+    try:
+        color_profile: VideoStreamProfile = profile_list.get_video_stream_profile(
+            640, 0, OBFormat.RGB, 30
+        )
+    except OBError as e:
+        print(e)
+        color_profile = profile_list.get_default_video_stream_profile()
+        print("color profile: ", color_profile)
+    config.enable_stream(color_profile)
+    pipeline.start(config)
+    for _event in node:
+        try:
+            frames: FrameSet = pipeline.wait_for_frames(100)
+            if frames is None:
+                continue
+            color_frame = frames.get_color_frame()
+            if color_frame is None:
+                continue
+            # covert to RGB format
+            color_image = frame_to_bgr_image(color_frame)
+            if color_image is None:
+                print("failed to convert frame to image")
+                continue
+            ret, frame = cv2.imencode("." + "jpeg", color_image)
+            if ret:
+                node.send_output("image", pa.array(frame), {"encoding": "jpeg"})
+        except KeyboardInterrupt:
+            break
+    pipeline.stop()
+
+
+if __name__ == "__main__":
+    main()

node-hub/dora-pyorbbecksdk/pyproject.toml

@@ -0,0 +1,20 @@
+[tool.poetry]
+name = "dora-pyorbbecksdk"
+version = "0.3.6"
+authors = ["Haixuan Xavier Tao <[email protected]>"]
+description = "Dora Node for capturing video with PyOrbbeck SDK"
+readme = "README.md"
+
+packages = [{ include = "dora_pyorbbecksdk" }]
+
+[tool.poetry.dependencies]
+dora-rs = "^0.3.6"
+python = "^3.7"
+
+
+[tool.poetry.scripts]
+dora-pyorbbecksdk = "dora_pyorbbecksdk.main:main"
+
+[build-system]
+requires = ["poetry-core>=1.8.0"]
+build-backend = "poetry.core.masonry.api"

node-hub/dora-pyorbbecksdk/tests/test_pyorbbecksdk.py

@@ -0,0 +1,10 @@
+def test_import_main():
+    return  # Remove this if you want to test pyorbbecksdk installation
+    # import pyorbbecksdk
+
+    # from opencv_video_capture.main import main
+    # skip test as pyorbbecksdk installation is a bit complicated
+
+    # Check that everything is working, and catch dora Runtime Exception as we're not running in a dora dataflow.
+    # with pytest.raises(RuntimeError):
+    #    main()