diff --git a/.github/workflows/happypose_ros_build_and_test.yaml b/.github/workflows/happypose_ros_build_and_test.yaml index a2c9150..12e8687 100644 --- a/.github/workflows/happypose_ros_build_and_test.yaml +++ b/.github/workflows/happypose_ros_build_and_test.yaml @@ -1,6 +1,12 @@ name: "Humble: Build and Test" -on: [ push, pull_request ] +on: + push: + branches: + - "main" + pull_request: + branches: + - "*" jobs: test_happypose_ros: diff --git a/README.md b/README.md index c2e5ce0..7f26d87 100644 --- a/README.md +++ b/README.md @@ -69,10 +69,16 @@ The node provides the following ROS topics: - Single-view: bounding box is populated. - Multi-view: no bounding box. Results are represented in the *leading camera* reference frame. Detections are the result of CosyPose multi-view algorithm. + Timestamp in the message's header is set to the moment it is published, after the pose etimation pipeline finished. Timestamps of each result are the same as the timestamp of the image used for the detection. In case of multiview see parameter **time_stamp_strategy** for more information. + - **happypose/vision_info** [vision_msgs/msg/VisionInfo] Information about the used pose estimator (currently only CosyPose is supported) and URL with object database location. +- **happypose/object_symmetries** [happypose_msgs/msg/ObjectSymmetriesArray] (*QOS: TRANSIENT_LOCAL*) + + Discrete and continuous symmetries of objects in the dataset. + - **happypose/markers** [visualization_msgs/msg/MarkerArray] Array of markers used to visualize detections with their meshes in software like RViz 2. diff --git a/happypose_examples/CMakeLists.txt b/happypose_examples/CMakeLists.txt index d8ebe49..b04d444 100644 --- a/happypose_examples/CMakeLists.txt +++ b/happypose_examples/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.5.0) +cmake_minimum_required(VERSION 3.10) project(happypose_examples) find_package(ament_cmake REQUIRED) diff --git a/happypose_examples/package.xml b/happypose_examples/package.xml index d17ba92..043f119 100644 --- a/happypose_examples/package.xml +++ b/happypose_examples/package.xml @@ -2,7 +2,7 @@ happypose_examples - 0.0.0 + 0.2.0 Examples for happypose_ros package Krzysztof Wojciechowski Guilhem Saurel diff --git a/happypose_msgs/CMakeLists.txt b/happypose_msgs/CMakeLists.txt new file mode 100644 index 0000000..1710b23 --- /dev/null +++ b/happypose_msgs/CMakeLists.txt @@ -0,0 +1,45 @@ +cmake_minimum_required(VERSION 3.10) +project(happypose_msgs) + +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(std_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) + +find_package(rclpy REQUIRED) + +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces( + ${PROJECT_NAME} + msg/ContinuousSymmetry.msg + msg/ObjectSymmetries.msg + msg/ObjectSymmetriesArray.msg + DEPENDENCIES + builtin_interfaces + std_msgs + geometry_msgs) + +# Install Python modules +ament_python_install_package(${PROJECT_NAME}_py) +install(FILES package.xml DESTINATION share/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_cmake_pytest REQUIRED) + set(_pytest_tests + test/test_discretize_symmetries.py + ) + foreach(_test_path ${_pytest_tests}) + get_filename_component(_test_name ${_test_path} NAME_WE) + ament_add_pytest_test(${_test_name} ${_test_path} + APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR} + TIMEOUT 60 + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + ) + endforeach() +endif() + + +ament_export_dependencies(rosidl_default_runtime) +ament_package() diff --git a/happypose_msgs/examples/meshcat_viz.ipynb b/happypose_msgs/examples/meshcat_viz.ipynb new file mode 100644 index 0000000..fc09f59 --- /dev/null +++ b/happypose_msgs/examples/meshcat_viz.ipynb @@ -0,0 +1,136 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Simple example of usage of symmetries discretization\n", + "\n", + "This example will walk you through the usage of the discretization function found in `happypose_msgs_py`.\n", + "Note, this example requires additional dependency in a form of [MeshCat](https://pypi.org/project/meshcat/) which has to be installed manually.\n", + "\n", + "Additionally, the user has to update `PYTHONPATH` variable used inside Jupyter notebook to account for dependencies found in their ROS 2 installation and build dependencies of `happypose_msgs` build in their Colcon workspace. Code cell will help to make those changes." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Add ROS 2 install path and your Colcon workspace to PYTHONPATH in Jupyter\n", + "import os\n", + "import sys\n", + "from pathlib import Path\n", + "\n", + "# Modify this path to mach you Colcon workspace. The path has to be global\n", + "my_colcon_ws_path = Path(\"/home/gepetto/ros2_ws\")\n", + "\n", + "python_version = f\"python{sys.version_info.major}.{sys.version_info.minor}\"\n", + "dist_package_path = Path(\"local\") / \"lib\" / python_version / \"dist-packages\"\n", + "ros_path = Path(\"/opt\") / \"ros\" / os.environ['ROS_DISTRO'] / dist_package_path\n", + "colson_ws_path = my_colcon_ws_path / \"install\" / \"happypose_msgs\" / dist_package_path\n", + "sys.path.append(ros_path.as_posix())\n", + "sys.path.append(colson_ws_path.as_posix())" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "import time\n", + "\n", + "import meshcat\n", + "import meshcat.geometry as g\n", + "\n", + "from geometry_msgs.msg import Vector3\n", + "from happypose_msgs_py.symmetries import discretize_symmetries\n", + "from happypose_msgs.msg import ContinuousSymmetry, ObjectSymmetries\n", + "\n", + "# Generate input ROS message with symmetries\n", + "input_msg = ObjectSymmetries(\n", + " symmetries_discrete=[],\n", + " symmetries_continuous=[\n", + " ContinuousSymmetry(\n", + " axis=Vector3(x=0.0, y=0.0, z=1.0),\n", + " offset=Vector3(x=0.0, y=0.0, z=0.0),\n", + " )\n", + " ],\n", + ")\n", + "\n", + "# Discretize symmetries from the message\n", + "res = discretize_symmetries(input_msg, n_symmetries_continuous=64)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Create MeshCat window to display simple mesh rotating around our symmetries" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "vis = meshcat.Visualizer()\n", + "vis.jupyter_cell()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Load mesh of Valkyrie robot head and spin it around our symmetry axis" + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": {}, + "outputs": [], + "source": [ + "assests_path = Path(meshcat.viewer_assets_path()) / \"data\"\n", + "\n", + "vis[\"robots/valkyrie/head\"].set_object(\n", + " g.ObjMeshGeometry.from_file(assests_path / \"head_multisense.obj\"),\n", + " g.MeshLambertMaterial(\n", + " map=g.ImageTexture(\n", + " image=g.PngImage.from_file(assests_path / \"HeadTextureMultisense.png\")\n", + " )\n", + " ),\n", + ")\n", + "\n", + "for r in res:\n", + " # Apply our symmetry transformation in a form of matrix\n", + " vis[\"robots/valkyrie/head\"].set_transform(r)\n", + " time.sleep(0.1)" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.10.12" + } + }, + "nbformat": 4, + "nbformat_minor": 2 +} diff --git a/happypose_ros/resource/happypose_ros b/happypose_msgs/happypose_msgs_py/__init__.py similarity index 100% rename from happypose_ros/resource/happypose_ros rename to happypose_msgs/happypose_msgs_py/__init__.py diff --git a/happypose_msgs/happypose_msgs_py/symmetries.py b/happypose_msgs/happypose_msgs_py/symmetries.py new file mode 100644 index 0000000..3268d52 --- /dev/null +++ b/happypose_msgs/happypose_msgs_py/symmetries.py @@ -0,0 +1,103 @@ +from copy import copy +import numpy as np +import numpy.typing as npt +import transforms3d +from typing import List, Union + +from geometry_msgs.msg import Transform, Vector3, Quaternion + +from happypose_msgs.msg import ObjectSymmetries + + +def discretize_symmetries( + object_symmetries: ObjectSymmetries, + n_symmetries_continuous: int = 8, + return_ros_msg: bool = False, +) -> Union[npt.NDArray[np.float64], List[Transform]]: + """Converts discrete and continuous symmetries to a list of discrete symmetries. + + :param object_symmetries: ROS message containing symmetries of a given object. + :type object_symmetries: happypose_msgs.msg.ObjectSymmetries + :param n_symmetries_continuous: Number of segments to discretize continuous symmetries. + :type n_symmetries_continuous: int + :param return_ros_msg: Whether to return ROS message or numpy array + with 4x4 matrices, defaults to False. + :type return_ros_msg: bool, optional + :return: If ``return_ros_msg`` is False returns array of a shape (n, 4, 4) with ``n`` + SE3 transformation matrices representing symmetries. + Otherwise list of ROS Transform messages. + :rtype: Union[npt.NDArray[np.float64], List[geometry_msgs.msg.Transform]] + """ + + # If there are no continuous symmetries and ROS message is expected skip computations + if return_ros_msg and len(object_symmetries.symmetries_continuous) == 0: + return copy(object_symmetries.symmetries_discrete) + + n_con = len(object_symmetries.symmetries_continuous) * n_symmetries_continuous + n_disc = len(object_symmetries.symmetries_discrete) + n_mix = n_con * n_disc + + # Preallocate memory for results + out = np.zeros((n_con + n_disc + n_mix, 4, 4)) + + # Precompute steps of rotations + angles = np.linspace(0.0, 2.0 * np.pi, n_symmetries_continuous, endpoint=False) + + # Discretize continuous symmetries + for i, sym_c in enumerate(object_symmetries.symmetries_continuous): + axis = np.array([sym_c.axis.x, sym_c.axis.y, sym_c.axis.z]) + if not np.isclose(np.linalg.norm(axis), 1.0): + raise ValueError( + f"Continuous symmetry at index {i} has non unitary rotation axis!" + ) + # Compute begin and end indices + begin = i * n_symmetries_continuous + end = (i + 1) * n_symmetries_continuous + + # Compute T @ R @ int(T) + # Discrete rotations around axis, generating matrices R + out[begin:end, :3, :3] = np.array( + [transforms3d.axangles.axangle2mat(axis, a) for a in angles] + ) + # Compute T @ R + offset = np.array([sym_c.offset.x, sym_c.offset.y, sym_c.offset.z, 1.0]) + out[begin:end, :, -1] = offset + + # Multiply by inv(T) + T = np.eye(4) + T[:3, 3] = -offset[:3] + out[begin:end, :, :] = out[begin:end, :, :] @ T + + # Convert discrete symmetries to matrix format + for i, sym_d in enumerate(object_symmetries.symmetries_discrete): + begin = n_con + i + out[begin, :3, :3] = transforms3d.quaternions.quat2mat( + [sym_d.rotation.w, sym_d.rotation.x, sym_d.rotation.y, sym_d.rotation.z] + ) + out[begin, :, -1] = np.array( + [sym_d.translation.x, sym_d.translation.y, sym_d.translation.z, 1.0] + ) + + sym_c_d_end = n_con + n_disc + symmetries_continuous = out[:n_con] + # Combine discrete symmetries with possible continuous rotations + # TODO @MedericFourmy we should ensure this operation is valid for all object + # and not only objects with offset being at the origin of the coordinate system. + for i in range(n_disc): + begin = sym_c_d_end + i * n_symmetries_continuous + end = sym_c_d_end + (i + 1) * n_symmetries_continuous + symmetry_discrete = out[n_con + i] + # Multiply batch of continuous symmetries onto single discrete symmetry + out[begin:end] = symmetry_discrete @ symmetries_continuous + + if not return_ros_msg: + return out + + def _mat_to_msg(M: npt.NDArray[np.float64]) -> Transform: + q = transforms3d.quaternions.mat2quat(M[:3, :3]) + return Transform( + translation=Vector3(**dict(zip("xyz", M[:, -1]))), + rotation=Quaternion(**dict(zip("wxyz", q))), + ) + + return [_mat_to_msg(M) for M in out] diff --git a/happypose_msgs/msg/ContinuousSymmetry.msg b/happypose_msgs/msg/ContinuousSymmetry.msg new file mode 100644 index 0000000..31f6971 --- /dev/null +++ b/happypose_msgs/msg/ContinuousSymmetry.msg @@ -0,0 +1,6 @@ +# Definition of continuous symmetry. +# Consists of rotation axis and offset. +# Symilarly to HappyPose object ContinuousSymmetry + +geometry_msgs/Vector3 offset +geometry_msgs/Vector3 axis diff --git a/happypose_msgs/msg/ObjectSymmetries.msg b/happypose_msgs/msg/ObjectSymmetries.msg new file mode 100644 index 0000000..a194d05 --- /dev/null +++ b/happypose_msgs/msg/ObjectSymmetries.msg @@ -0,0 +1,10 @@ +# Class id for which symmetries are considered +string class_id +# Lists discrete and continuous symmetries of considered object. +# If no symmetries of a given type, list is left empty. + +# In HappyPose discrete symmetries are represented as +# transformation matrices. Those matrices are directly +# converted to the Transform message. +geometry_msgs/Transform[] symmetries_discrete +ContinuousSymmetry[] symmetries_continuous diff --git a/happypose_msgs/msg/ObjectSymmetriesArray.msg b/happypose_msgs/msg/ObjectSymmetriesArray.msg new file mode 100644 index 0000000..984a64d --- /dev/null +++ b/happypose_msgs/msg/ObjectSymmetriesArray.msg @@ -0,0 +1,5 @@ +# Time stamp at which the message was sent +std_msgs/Header header + +# List of objects detected by the HappyPose node +ObjectSymmetries[] objects diff --git a/happypose_msgs/package.xml b/happypose_msgs/package.xml new file mode 100644 index 0000000..78aafac --- /dev/null +++ b/happypose_msgs/package.xml @@ -0,0 +1,37 @@ + + + happypose_msgs + 0.2.0 + Custom messages used by happypose_ros package. + Krzysztof Wojciechowski + Guilhem Saurel + BSD + + https://github.com/agimus-project/happypose_ros + https://github.com/agimus-project/happypose_ros/issues + https://github.com/agimus-project/happypose_ros + + ament_cmake + ament_cmake_python + rosidl_default_generators + + builtin_interfaces + std_msgs + geometry_msgs + + builtin_interfaces + std_msgs + geometry_msgs + rclpy + python3-numpy + python3-transforms3d + rosidl_default_runtime + + pinocchio + + rosidl_interface_packages + + + ament_cmake + + diff --git a/happypose_msgs/test/test_discretize_symmetries.py b/happypose_msgs/test/test_discretize_symmetries.py new file mode 100644 index 0000000..41dfe22 --- /dev/null +++ b/happypose_msgs/test/test_discretize_symmetries.py @@ -0,0 +1,359 @@ +#!/usr/bin/env python + +import numpy as np +import numpy.typing as npt +import pinocchio as pin +from typing import List + +from geometry_msgs.msg import Transform, Vector3, Quaternion + +from happypose_msgs_py.symmetries import discretize_symmetries + +from happypose_msgs.msg import ContinuousSymmetry, ObjectSymmetries + + +def are_transforms_close(t1: pin.SE3, t2: pin.SE3) -> bool: + """Checks if two SE3 transformations are close. + + :param t1: First transform to compare. + :type t1: pinocchio.SE3 + :param t2: Second transform to compare. + :type t2: pinocchio.SE3 + :return: If those transformations are close. + :rtype: bool + """ + diff = t1.inverse() * t2 + return np.linalg.norm(pin.log6(diff).vector) < 5e-3 + + +def are_transform_msgs_close(t1: Transform, t2: Transform) -> bool: + """Checks if two ROS transform messages are close. + + :param t1: First transform to compare. + :type t1: geometry_msgs.msg.Transform + :param t2: Second transform to compare. + :type t2: geometry_msgs.msg.Transform + :return: If those transformations are close. + :rtype: bool + """ + T1, T2 = [ + pin.XYZQUATToSE3( + [ + t.translation.x, + t.translation.y, + t.translation.z, + t.rotation.x, + t.rotation.y, + t.rotation.z, + t.rotation.w, + ] + ) + for t in (t1, t2) + ] + return are_transforms_close(T1, T2) + + +def is_transform_msg_in_list(t1: Transform, t_list: List[Transform]) -> bool: + """Checks if a transform is in the list of transformations. + + :param t1: Transform to check if in the list. + :type t1: Transform + :param t_list: List of transforms to check if ``t1`` exists in there. + :type t_list: List[Transform] + :return: If the transform in the list. + :rtype: bool + """ + return any(are_transform_msgs_close(t1, t2) for t2 in t_list) + + +def is_transform_in_se3_list( + t1: npt.NDArray[np.float64], t_list: List[pin.SE3] +) -> bool: + """Checks if a transform is in the tensor of transformations. + + :param t1: Transform to check if in the tensor. + :type t1: npt.NDArray[np.float64] + :param t_list: List of SE3 objects used to find ``t1``. + :type t_list: List[pinocchio.SE3] + :return: If the transform in the list. + :rtype: bool + """ + T1 = pin.SE3(t1) + return any(are_transforms_close(T1, t2) for t2 in t_list) + + +def pin_to_msg(transform: pin.SE3) -> Transform: + """Converts 4x4 transformation matrix to ROS Transform message. + + :param transform: Input transformation. + :type transform: pinocchio.SE3 + :return: Converted SE3 transformation into ROS Transform + message format. + :rtype: geometry_msgs.msg.Transform + """ + pose_vec = pin.SE3ToXYZQUAT(transform) + return Transform( + translation=Vector3(**dict(zip("xyz", pose_vec[:3]))), + rotation=Quaternion(**dict(zip("xyzw", pose_vec[3:]))), + ) + + +def test_empty_message_np() -> None: + msg = ObjectSymmetries( + symmetries_discrete=[], + symmetries_continuous=[], + ) + + res = discretize_symmetries(msg) + assert isinstance(res, np.ndarray), "Result is not an instance of Numpy array!" + assert res.shape == (0, 4, 4), "Result shape is incorrect!" + + +def test_empty_message_ros() -> None: + msg = ObjectSymmetries( + symmetries_discrete=[], + symmetries_continuous=[], + ) + + res = discretize_symmetries(msg, return_ros_msg=True) + assert isinstance(res, list), "Result is not an instance of a list!" + assert len(res) == 0, "Results list is not empty!" + + +def test_only_discrete_np() -> None: + t1 = pin.SE3( + pin.Quaternion(np.array([0.0, 0.0, 0.0, 1.0])), np.array([0.0, 0.0, 0.0]) + ) + t2 = pin.SE3( + pin.Quaternion(np.array([0.707, 0.0, 0.0, 0.707])), np.array([0.1, 0.1, 0.1]) + ) + msg = ObjectSymmetries( + symmetries_discrete=[pin_to_msg(t1), pin_to_msg(t2)], + symmetries_continuous=[], + ) + + res = discretize_symmetries(msg) + + assert res.shape == (2, 4, 4), "Result shape is incorrect!" + + for i, t in enumerate(res): + assert is_transform_in_se3_list( + t, [t1, t2] + ), f"Discrete symmetry at index {i} did not match any of the initial ones!" + + +def test_only_discrete_ros() -> None: + msg = ObjectSymmetries( + symmetries_discrete=[ + Transform( + translation=Vector3(x=0.0, y=0.0, z=0.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), + Transform( + translation=Vector3(x=0.1, y=0.1, z=0.1), + rotation=Quaternion(x=0.707, y=0.0, z=0.0, w=0.707), + ), + ], + symmetries_continuous=[], + ) + + res = discretize_symmetries(msg, return_ros_msg=True) + + assert len(res) == len( + msg.symmetries_discrete + ), "Results list does not have all discrete symmetries from message received!" + + assert all( + isinstance(r, Transform) for r in res + ), "Returned type of elements in the list is not geometry_msgs.msg.Transform!" + + for i, t in enumerate(res): + assert is_transform_msg_in_list( + t, msg.symmetries_discrete + ), f"Discrete symmetry at index {i} did not match any initial ones in the message!" + + +def test_only_continuous_np() -> None: + n_symmetries = 2 + axis = np.array([1.0, 0.0, -1.0]) + axis = axis / np.linalg.norm(axis) + offset = np.array([-0.1, 0.0, 0.1]) + + t_list = [ + pin.SE3(np.eye(3), offset) + * pin.SE3( + pin.Quaternion(pin.AngleAxis(2.0 * np.pi / n_symmetries * i, axis)), + np.array([0.0, 0.0, 0.0]), + ) + * pin.SE3(np.eye(3), offset).inverse() + for i in range(n_symmetries) + ] + msg = ObjectSymmetries( + symmetries_discrete=[], + symmetries_continuous=[ + ContinuousSymmetry( + axis=Vector3(**dict(zip("xyz", axis))), + offset=Vector3(**dict(zip("xyz", offset))), + ) + ], + ) + + res = discretize_symmetries(msg, n_symmetries_continuous=n_symmetries) + assert res.shape == (n_symmetries, 4, 4), "Result shape is incorrect!" + for i, t in enumerate(res): + assert is_transform_in_se3_list( + t, t_list + ), f"Discrete symmetry at index {i} did not match any symmetry from generated list!" + + +def test_only_continuous_ros() -> None: + n_symmetries = 9 + axis = np.array([1.0, 0.2, 1.0]) + axis = axis / np.linalg.norm(axis) + offset = np.array([0.0, 0.0, 0.1]) + + t_list = [ + pin.SE3(np.eye(3), offset) + * pin.SE3( + pin.Quaternion(pin.AngleAxis(2.0 * np.pi / n_symmetries * i, axis)), + np.array([0.0, 0.0, 0.0]), + ) + * pin.SE3(np.eye(3), offset).inverse() + for i in range(n_symmetries) + ] + msg = ObjectSymmetries( + symmetries_discrete=[], + symmetries_continuous=[ + ContinuousSymmetry( + axis=Vector3(**dict(zip("xyz", axis))), + offset=Vector3(**dict(zip("xyz", offset))), + ) + ], + ) + + res = discretize_symmetries( + msg, n_symmetries_continuous=n_symmetries, return_ros_msg=True + ) + + assert len(res) == len( + t_list + ), "Results list does not have all discrete symmetries from message received!" + + t_msgs = [pin_to_msg(t) for t in t_list] + + for i, t in enumerate(res): + assert is_transform_msg_in_list( + t, t_msgs + ), f"Discrete symmetry at index {i} did not match any symmetry from generated list!" + + +def test_mixed_np() -> None: + n_symmetries = 32 + axis = np.array([-0.1, 0.5, 0.5]) + axis = axis / np.linalg.norm(axis) + offset = np.array([-0.1, 0.6, 0.1]) + + t_c_list = [ + pin.SE3(np.eye(3), offset) + * pin.SE3( + pin.Quaternion(pin.AngleAxis(2.0 * np.pi / n_symmetries * i, axis)), + np.array([0.0, 0.0, 0.0]), + ) + * pin.SE3(np.eye(3), offset).inverse() + for i in range(n_symmetries) + ] + + t_d_list = [ + pin.SE3( + pin.Quaternion(np.array([0.0, 0.0, 0.0, 1.0])), np.array([0.0, 0.0, 0.0]) + ), + pin.SE3( + pin.Quaternion(np.array([0.707, 0.0, 0.0, 0.707])), + np.array([0.1, 0.1, 0.1]), + ), + ] + msg = ObjectSymmetries( + symmetries_discrete=[*[pin_to_msg(t) for t in t_d_list]], + symmetries_continuous=[ + ContinuousSymmetry( + axis=Vector3(**dict(zip("xyz", axis))), + offset=Vector3(**dict(zip("xyz", offset))), + ) + ], + ) + + res = discretize_symmetries(msg, n_symmetries_continuous=n_symmetries) + + assert res.shape == ( + len(t_c_list) + len(t_d_list) + len(t_d_list) * len(t_c_list), + 4, + 4, + ), "Result shape is incorrect!" + + t_test = [ + *t_c_list, + *t_d_list, + *[t_d * t_c for t_c in t_c_list for t_d in t_d_list], + ] + + print(res, flush=True) + + for i, t in enumerate(res): + assert is_transform_in_se3_list( + t, t_test + ), f"Discrete symmetry at index {i} did not match any symmetry from generated list!" + + +def test_mixed_ros() -> None: + n_symmetries = 31 + axis = np.array([-0.9, 0.2, -0.5]) + axis = axis / np.linalg.norm(axis) + offset = np.array([-1.1, 0.6, 0.1]) + + t_c_list = [ + pin.SE3(np.eye(3), offset) + * pin.SE3( + pin.Quaternion(pin.AngleAxis(2.0 * np.pi / n_symmetries * i, axis)), + np.array([0.0, 0.0, 0.0]), + ) + * pin.SE3(np.eye(3), offset).inverse() + for i in range(n_symmetries) + ] + + t_d_list = [ + pin.SE3( + pin.Quaternion(np.array([0.0, 0.0, 0.0, 1.0])), np.array([0.0, 0.0, 0.0]) + ), + pin.SE3( + pin.Quaternion(np.array([0.707, 0.0, 0.0, 0.707])), + np.array([0.1, 0.1, 0.1]), + ), + ] + msg = ObjectSymmetries( + symmetries_discrete=[*[pin_to_msg(t) for t in t_d_list]], + symmetries_continuous=[ + ContinuousSymmetry( + axis=Vector3(**dict(zip("xyz", axis))), + offset=Vector3(**dict(zip("xyz", offset))), + ) + ], + ) + + res = discretize_symmetries( + msg, n_symmetries_continuous=n_symmetries, return_ros_msg=True + ) + + assert len(res) == ( + len(t_c_list) + len(t_d_list) + len(t_d_list) * len(t_c_list) + ), "Size od the results is incorrect!" + + t_test = [ + *[pin_to_msg(t) for t in t_c_list], + *[pin_to_msg(t) for t in t_d_list], + *[pin_to_msg(t_d * t_c) for t_c in t_c_list for t_d in t_d_list], + ] + + for i, t in enumerate(res): + assert is_transform_msg_in_list( + t, t_test + ), f"Discrete symmetry at index {i} did not match any symmetry from generated list!" diff --git a/happypose_ros/.gitignore b/happypose_ros/.gitignore index b6a8238..2be329e 100644 --- a/happypose_ros/.gitignore +++ b/happypose_ros/.gitignore @@ -1,5 +1,5 @@ # Byte-compiled / optimized / DLL files -__pycache__/ +**/__pycache__/ *.py[cod] *$py.class @@ -7,7 +7,7 @@ __pycache__/ *.so # Linter cache -.ruff_cache +*.ruff_cache # Colcon custom files COLCON_IGNORE @@ -17,6 +17,4 @@ AMENT_IGNORE .vscode # Generated ROS parameters -# The file has to be manually modified as the code generations has a bug in __map happypose_ros/happypose_ros_parameters.py -happypose_ros/test/__pycache__ diff --git a/happypose_ros/happypose_ros/happypose_node.py b/happypose_ros/happypose_ros/happypose_node.py index 64a7257..eba2d7b 100644 --- a/happypose_ros/happypose_ros/happypose_node.py +++ b/happypose_ros/happypose_ros/happypose_node.py @@ -7,13 +7,13 @@ import torch.multiprocessing as mp import queue - import rclpy from rclpy.duration import Duration from rclpy.exceptions import ParameterException import rclpy.logging from rclpy.node import Node from rclpy.time import Time +from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile from tf2_ros import TransformBroadcaster @@ -34,8 +34,11 @@ get_camera_transform, get_detection_array_msg, get_marker_array_msg, + get_object_symmetries_msg, ) +from happypose_msgs.msg import ObjectSymmetriesArray # noqa: E402 + # Automatically generated file from happypose_ros.happypose_ros_parameters import happypose_ros # noqa: E402 @@ -44,6 +47,7 @@ def happypose_worker_proc( worker_free: mp.Value, observation_tensor_queue: mp.Queue, results_queue: mp.Queue, + symmetries_queue: mp.Queue, params_queue: mp.Queue, ) -> None: """Function used to trigger worker process. @@ -59,6 +63,8 @@ def happypose_worker_proc( """ # Initialize the pipeline pipeline = HappyPosePipeline(params_queue.get()) + # Inform ROS node about the dataset + symmetries_queue.put(pipeline.get_dataset()) # Notify parent that initialization has finished with worker_free.get_lock(): @@ -68,11 +74,14 @@ def happypose_worker_proc( while True: # Await any data on all the input queues observation = observation_tensor_queue.get(block=True, timeout=None) + if observation is None: + break # Update inference args if available try: params = params_queue.get_nowait() pipeline.update_params(params) + symmetries_queue.put(pipeline.get_dataset()) except queue.Empty: pass @@ -121,6 +130,7 @@ def __init__(self, node_name: str = "happypose_node", **kwargs) -> None: self._worker_free = ctx.Value(c_bool, False) self._observation_tensor_queue = ctx.Queue(1) self._results_queue = ctx.Queue(1) + self._symmetries_queue = ctx.Queue(1) self._params_queue = ctx.Queue(1) self._update_dynamic_params(True) @@ -133,12 +143,13 @@ def __init__(self, node_name: str = "happypose_node", **kwargs) -> None: self._worker_free, self._observation_tensor_queue, self._results_queue, + self._symmetries_queue, self._params_queue, ), ) self._await_results_task = None - # TODO once Megapose is available initialization + # TODO once MegaPose is available initialization # should be handled better self._vision_info_msg = VisionInfo( method=self._params.pose_estimator_type, @@ -164,6 +175,16 @@ def __init__(self, node_name: str = "happypose_node", **kwargs) -> None: VisionInfo, "happypose/vision_info", 10 ) + # Set the message to be "latched" + qos = QoSProfile( + depth=1, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + history=HistoryPolicy.KEEP_LAST, + ) + self._symmetries_publisher = self.create_publisher( + ObjectSymmetriesArray, "happypose/object_symmetries", qos + ) + if self._params.cameras.n_min_cameras > len(self._cameras): e = ParameterException( "Minimum number of cameras to trigger the pipeline is" @@ -242,21 +263,40 @@ def __init__(self, node_name: str = "happypose_node", **kwargs) -> None: # Start the worker when all possible errors are handled self._happypose_worker.start() + # Start spinner waiting for updates in symmetries + self._symmetries_queue_task = Thread(target=self._symmetries_queue_spinner) + self._symmetries_queue_task.start() + self.get_logger().info( "Node initialized. Waiting for HappyPose worker to initialize...", ) def destroy_node(self) -> None: """Destroys the node and closes all queues.""" + # Signal closing of queues + if self._symmetries_queue is not None: + self._symmetries_queue.put(None) if self._observation_tensor_queue is not None: - self._observation_tensor_queue.close() + self._observation_tensor_queue.put(None) + + # Close receiving queue if self._results_queue is not None: self._results_queue.close() + + # Stop threads if self._await_results_task is not None: self._await_results_task.join() + if self._symmetries_queue_task is not None: + self._symmetries_queue_task.join() + if self._symmetries_queue is not None: + self._symmetries_queue.close() + + # Stop worker process if self._happypose_worker is not None: self._happypose_worker.join() self._happypose_worker.terminate() + if self._observation_tensor_queue is not None: + self._observation_tensor_queue.close() super().destroy_node() def _update_dynamic_params(self, on_init: bool = False) -> None: @@ -278,6 +318,24 @@ def _update_dynamic_params(self, on_init: bool = False) -> None: if self._params.verbose_info_logs and not on_init: self.get_logger().info("Parameter change occurred.") + def _symmetries_queue_spinner(self) -> None: + """Awaits new data in a queue with symmetries and publishes them on a ROS topic""" + try: + while True: + symmetries = self._symmetries_queue.get(block=True, timeout=None) + if symmetries is None: + break + + header = Header( + frame_id="", + stamp=self.get_clock().now().to_msg(), + ) + symmetries_msg = get_object_symmetries_msg(symmetries, header) + self._symmetries_publisher.publish(symmetries_msg) + # Queue is closed + except ValueError: + pass + def _on_image_cb(self) -> None: """Callback function used to synchronize incoming images from different cameras. Performs basic checks if worker process is available and can start HappyPose pipeline. @@ -327,7 +385,7 @@ def _trigger_pipeline(self) -> None: if len(processed_cameras) < self._params.cameras.n_min_cameras: # Throttle logs to either 5 times the timeout or 10 seconds - timeout = max(5 * self._params.cameras.timeout, 10.0) + timeout = max(5.0 * self._params.cameras.timeout, 10.0) if (now - self._last_pipeline_trigger) > (Duration(seconds=timeout)): self.get_logger().warn( "Unable to start pipeline! Not enough camera" @@ -401,51 +459,6 @@ def _await_results(self, cam_data: dict) -> None: results = self._results_queue.get(block=True, timeout=None) - if results is None: - if self._params.verbose_info_logs: - self.get_logger().info("No objects detected.") - return - - if self._params.verbose_info_logs: - self.get_logger().info(f"Detected {len(results['infos'])} objects.") - - if self._multiview: - missing_cameras = len(cam_data) - len(results["camera_infos"]) - if missing_cameras > 0: - # Keep only the cameras that were not discarded in multiview - cam_data = { - name: cam - for i, (name, cam) in enumerate(cam_data.items()) - if i in results["camera_infos"].view_id.values - } - - if self._leading_camera not in cam_data.keys(): - self.get_logger().error( - f"Leading camera '{self._leading_camera}'" - " was discarded when performing multi-view!" - ) - return - - if self._params.verbose_info_logs: - self.get_logger().warn( - f"{missing_cameras} camera views were discarded in multi-view." - ) - - lead_cam_idx = list(cam_data.keys()).index(self._leading_camera) - # Transform objects into leading camera's reference frame - leading_cam_pose_inv = results["camera_poses"][lead_cam_idx].inverse() - # Transform detected objects' poses - results["poses"] = leading_cam_pose_inv @ results["poses"] - # Create mask of cameras that expect to have TF published - cams_to_tf = [ - self._params.cameras.get_entry(name).publish_tf - for name in cam_data.keys() - ] - # Transform cameras' poses - results["camera_poses"][cams_to_tf] = ( - leading_cam_pose_inv @ results["camera_poses"][cams_to_tf] - ) - header = Header( frame_id=cam_data[self._leading_camera]["frame_id"], # Choose sorted time stamp @@ -456,10 +469,63 @@ def _await_results(self, cam_data: dict) -> None: ).to_msg(), ) - # In case of multi-view, do not use bounding boxes - detections = get_detection_array_msg( - results, header, has_bbox=not self._multiview - ) + if results is not None: + if self._params.verbose_info_logs: + self.get_logger().info(f"Detected {len(results['infos'])} objects.") + + if self._multiview: + missing_cameras = len(cam_data) - len(results["camera_infos"]) + if missing_cameras > 0: + # Keep only the cameras that were not discarded in multiview + cam_data = { + name: cam + for i, (name, cam) in enumerate(cam_data.items()) + if i in results["camera_infos"].view_id.values + } + + if self._leading_camera not in cam_data.keys(): + self.get_logger().error( + f"Leading camera '{self._leading_camera}'" + " was discarded when performing multi-view!" + ) + return + + if self._params.verbose_info_logs: + self.get_logger().warn( + f"{missing_cameras} camera views were discarded in multi-view." + ) + + lead_cam_idx = list(cam_data.keys()).index(self._leading_camera) + # Transform objects into leading camera's reference frame + leading_cam_pose_inv = results["camera_poses"][ + lead_cam_idx + ].inverse() + # Transform detected objects' poses + results["poses"] = leading_cam_pose_inv @ results["poses"] + # Create mask of cameras that expect to have TF published + cams_to_tf = [ + self._params.cameras.get_entry(name).publish_tf + for name in cam_data.keys() + ] + # Transform cameras' poses + results["camera_poses"][cams_to_tf] = ( + leading_cam_pose_inv @ results["camera_poses"][cams_to_tf] + ) + + # In case of multi-view, do not use bounding boxes + detections = get_detection_array_msg( + results, header, has_bbox=not self._multiview + ) + + else: + if self._params.verbose_info_logs: + self.get_logger().info("No objects detected.") + + detections = Detection2DArray( + header=header, + detections=[], + ) + self._detections_publisher.publish(detections) self._vision_info_msg.header = header diff --git a/happypose_ros/happypose_ros/inference_pipeline.py b/happypose_ros/happypose_ros/inference_pipeline.py index c64c549..8098ee6 100644 --- a/happypose_ros/happypose_ros/inference_pipeline.py +++ b/happypose_ros/happypose_ros/inference_pipeline.py @@ -4,6 +4,7 @@ from happypose.toolbox.inference.types import ObservationTensor from happypose.toolbox.inference.utils import filter_detections +from happypose.toolbox.datasets.object_dataset import RigidObjectDataset from happypose.pose_estimators.cosypose.cosypose.utils.cosypose_wrapper import ( CosyPoseWrapper, @@ -65,6 +66,17 @@ def update_params(self, params: dict) -> None: else None ) + def get_dataset(self) -> RigidObjectDataset: + """Returns rigid object dataset used by HappyPose pose estimator + + :return: Dataset used by HappyPose pose estimator + :rtype: RigidObjectDataset + """ + dataset = self._wrapper.object_dataset + if self._inference_args["labels_to_keep"] is None: + return dataset + return dataset.filter_objects(self._inference_args["labels_to_keep"]) + def __call__(self, observation: ObservationTensor) -> Union[None, dict]: """Performs sequence of actions to estimate pose and optionally merge multiview results. diff --git a/happypose_ros/happypose_ros/utils.py b/happypose_ros/happypose_ros/utils.py index 79abadc..93e2713 100644 --- a/happypose_ros/happypose_ros/utils.py +++ b/happypose_ros/happypose_ros/utils.py @@ -25,6 +25,14 @@ ObjectHypothesis, ObjectHypothesisWithPose, ) +from happypose_msgs.msg import ( + ContinuousSymmetry, + ObjectSymmetries, + ObjectSymmetriesArray, +) + +import happypose.toolbox.lib3d.symmetries as happypose_symmetries +from happypose.toolbox.datasets.object_dataset import RigidObject, RigidObjectDataset # Automatically generated file from happypose_ros.happypose_ros_parameters import happypose_ros @@ -201,6 +209,22 @@ def generate_marker_msg(i: int) -> Marker: ) +def transform_mat_to_msg(transform: npt.NDArray[np.float64]) -> Transform: + """Converts 4x4 transformation matrix to ROS Transform message. + + :param transform: 4x4 transformation array. + :type transform: npt.NDArray[np.float64] + :return: Converted SE3 transformation into ROS Transform + message format. + :rtype: geometry_msgs.msg.Transform + """ + pose_vec = pin.SE3ToXYZQUAT(pin.SE3(transform)) + return Transform( + translation=Vector3(**dict(zip("xyz", pose_vec[:3]))), + rotation=Quaternion(**dict(zip("xyzw", pose_vec[3:]))), + ) + + def get_camera_transform( camera_pose: Tensor, header: Header, child_frame_id: str ) -> TransformStamped: @@ -218,12 +242,73 @@ def get_camera_transform( :rtype: geometry_msgs.msg.TransformStamped """ - pose_vec = pin.SE3ToXYZQUAT(pin.SE3(camera_pose.numpy())) return TransformStamped( header=header, child_frame_id=child_frame_id, - transform=Transform( - translation=Vector3(**dict(zip("xyz", pose_vec[:3]))), - rotation=Quaternion(**dict(zip("xyzw", pose_vec[3:]))), - ), + transform=transform_mat_to_msg(camera_pose.numpy()), + ) + + +def continuous_symmetry_to_msg( + symmetry: happypose_symmetries.ContinuousSymmetry, +) -> ContinuousSymmetry: + """Converts HappyPose ContinuousSymmetry object into ContinuousSymmetry ROS message + + :param symmetry: HappyPose object storing definition of continuous symmetry. + :type symmetry: happypose_symmetries.ContinuousSymmetry + :return: ROS message representing continuous symmetry. + :rtype: happypose_msgs.msg.ContinuousSymmetry + """ + return ContinuousSymmetry( + # Explicit conversion to float is required in this case. + # Some values in json files defining object properties are written without decimal + # precision and are stored as integers. ROS 2 generated messages do not cast + # integers to floats resulting in an error. + offset=Vector3(**dict(zip("xyz", [float(i) for i in symmetry.offset]))), + axis=Vector3(**dict(zip("xyz", [float(i) for i in symmetry.axis]))), + ) + + +def discrete_symmetry_to_msg( + symmetry: happypose_symmetries.DiscreteSymmetry, +) -> Transform: + """Converts HappyPose DiscreteSymmetry object into Transform ROS message. + + :param symmetry: HappyPose object storing definition of discrete symmetry. + :type symmetry: happypose_symmetries.DiscreteSymmetry + :return: ROS message with transformation corresponding to given symmetry. + :rtype: geometry_msgs.msg.Transform + """ + return transform_mat_to_msg(symmetry.pose) + + +def get_object_symmetries_msg( + dataset: RigidObjectDataset, header: Header +) -> ObjectSymmetriesArray: + """Converts HappyPose RigidObjectDataset object into ROS message representing symmetries + of all objects in that dataset. + + :param dataset: Dataset of rigid objects detected by HappyPose. + :type dataset: RigidObjectDataset + :param header: Header used to populate the message. + Contains timestamp at which message was published. + :type header: std_msgs.msg.Header + :return: Message containing symmetries of objects detected by HappyPose. + :rtype: happypose_msgs.msg.ObjectSymmetriesArray + """ + + def generate_symmetry_msg(object: RigidObject) -> ObjectSymmetries: + return ObjectSymmetries( + class_id=object.label, + symmetries_discrete=[ + discrete_symmetry_to_msg(sym) for sym in object.symmetries_discrete + ], + symmetries_continuous=[ + continuous_symmetry_to_msg(sym) for sym in object.symmetries_continuous + ], + ) + + return ObjectSymmetriesArray( + header=header, + objects=[generate_symmetry_msg(object) for object in dataset.list_objects], ) diff --git a/happypose_ros/package.xml b/happypose_ros/package.xml index d593d72..0d69a3a 100644 --- a/happypose_ros/package.xml +++ b/happypose_ros/package.xml @@ -2,7 +2,7 @@ happypose_ros - 0.0.0 + 0.2.0 ROS 2 wrapper around HappyPose python library for 6D pose estimation Krzysztof Wojciechowski Guilhem Saurel @@ -21,6 +21,7 @@ rclpy tf2_ros geometry_msgs + happypose_msgs sensor_msgs std_msgs vision_msgs diff --git a/happypose_ros/resource/happypose_node b/happypose_ros/resource/happypose_node new file mode 100644 index 0000000..e69de29 diff --git a/happypose_ros/setup.py b/happypose_ros/setup.py index 647d240..f64daec 100644 --- a/happypose_ros/setup.py +++ b/happypose_ros/setup.py @@ -1,26 +1,42 @@ -import os -from glob import glob +from pathlib import Path +from typing import List + from setuptools import find_packages, setup from generate_parameter_library_py.setup_helper import generate_parameter_module package_name = "happypose_ros" +project_source_dir = Path(__file__).parent module_name = "happypose_ros_parameters" yaml_file = "happypose_ros/happypose_ros_parameters.yaml" validation_module = "happypose_ros.custom_validation" generate_parameter_module(module_name, yaml_file, validation_module=validation_module) + +def get_files(dir: Path, pattern: str) -> List[str]: + return [x.as_posix() for x in (dir).glob(pattern) if x.is_file()] + + setup( name=package_name, - version="0.0.0", + version="0.0.2", packages=find_packages(exclude=["test"]), data_files=[ - ("share/ament_index/resource_index/packages", ["resource/" + package_name]), - ("share/" + package_name, ["package.xml"]), - (os.path.join("share", package_name, "test"), glob("test/*.py")), - (os.path.join("share", package_name, "test"), glob("test/*.yaml")), - (os.path.join("share", package_name, "test"), glob("test/*.png")), + ("share/ament_index/resource_index/packages", ["resource/happypose_node"]), + ("share/happypose_ros", ["package.xml"]), + ( + f"share/{package_name}/test", + get_files(project_source_dir / "test", "*.py"), + ), + ( + f"share/{package_name}/test", + get_files(project_source_dir / "test", "*.png"), + ), + ( + f"share/{package_name}/test", + get_files(project_source_dir / "test", "*.yaml"), + ), ], install_requires=["setuptools"], zip_safe=True, diff --git a/happypose_ros/test/happypose_testing_utils.py b/happypose_ros/test/happypose_testing_utils.py index 974e9ed..f343f78 100644 --- a/happypose_ros/test/happypose_testing_utils.py +++ b/happypose_ros/test/happypose_testing_utils.py @@ -11,6 +11,7 @@ from rclpy.node import Node from rclpy.parameter import Parameter from rclpy.time import Time +from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile from tf2_ros.buffer import Buffer from tf2_ros.transform_listener import TransformListener @@ -29,6 +30,8 @@ from rcl_interfaces.msg import Parameter as RCL_Parameter from rcl_interfaces.srv import GetParameters, SetParametersAtomically +from happypose_msgs.msg import ObjectSymmetriesArray # noqa: E402 + class HappyPoseTestCase(unittest.TestCase): """Generic test case for HappyPose""" @@ -115,6 +118,7 @@ def __init__( "happypose/markers": [], "happypose/detections": [], "happypose/vision_info": [], + "happypose/object_symmetries": [], } self._markers_sub = self.create_subscription( MarkerArray, "happypose/markers", self._markers_cb, 5 @@ -126,6 +130,18 @@ def __init__( VisionInfo, "happypose/vision_info", self._vision_info_cb, 5 ) + qos = QoSProfile( + depth=1, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + history=HistoryPolicy.KEEP_LAST, + ) + self._object_symmetries_sub = self.create_subscription( + ObjectSymmetriesArray, + "happypose/object_symmetries", + self._object_symmetries_cb, + qos, + ) + # Initialize service clients self._set_param_cli = self.create_client( SetParametersAtomically, @@ -165,6 +181,14 @@ def _vision_info_cb(self, msg: VisionInfo) -> None: """ self._sub_topic["happypose/vision_info"].append(msg) + def _object_symmetries_cb(self, msg: ObjectSymmetriesArray) -> None: + """Callback of the object symmetries message topic + + :param msg: Message containing object symmetries + :type msg: happypose_msgs.msg.ObjectSymmetriesArray + """ + self._sub_topic["happypose/object_symmetries"].append(msg) + def get_transform( self, target_frame: str, source_frame: str, timeout: float = 5.0 ) -> Transform: diff --git a/happypose_ros/test/single_view_base.py b/happypose_ros/test/single_view_base.py index 20ebfbf..fd7551b 100644 --- a/happypose_ros/test/single_view_base.py +++ b/happypose_ros/test/single_view_base.py @@ -83,8 +83,31 @@ def test_02_check_topics(self) -> None: self.node.assert_node_is_publisher("happypose/detections", timeout=3.0) self.node.assert_node_is_publisher("happypose/markers", timeout=3.0) self.node.assert_node_is_publisher("happypose/vision_info", timeout=3.0) + self.node.assert_node_is_publisher("happypose/object_symmetries", timeout=3.0) - def test_03_trigger_pipeline(self, proc_output: ActiveIoHandler) -> None: + def test_03_receive_object_symmetries(self) -> None: + self.node.assert_message_received("happypose/object_symmetries", timeout=120.0) + + def test_04_check_object_symmetries(self) -> None: + object_symmetries = self.node.get_received_message( + "happypose/object_symmetries" + ) + self.assertEqual(len(object_symmetries.objects), 21) + # Check if all object names follow the scheme + self.assertTrue( + all(["ycbv-obj_0000" in obj.class_id for obj in object_symmetries.objects]) + ) + + self.assertEqual(len(object_symmetries.objects[0].symmetries_discrete), 1) + self.assertEqual(len(object_symmetries.objects[0].symmetries_continuous), 0) + + self.assertEqual(len(object_symmetries.objects[4].symmetries_discrete), 0) + self.assertEqual(len(object_symmetries.objects[4].symmetries_continuous), 0) + + self.assertEqual(len(object_symmetries.objects[12].symmetries_discrete), 0) + self.assertEqual(len(object_symmetries.objects[12].symmetries_continuous), 1) + + def test_05_trigger_pipeline(self, proc_output: ActiveIoHandler) -> None: # Clear buffer before expecting any messages self.node.clear_msg_buffer() @@ -98,17 +121,17 @@ def test_03_trigger_pipeline(self, proc_output: ActiveIoHandler) -> None: ready = proc_output.waitFor("HappyPose initialized", timeout=0.5) assert ready, "Failed to trigger the pipeline!" - def test_04_receive_messages(self) -> None: + def test_06_receive_messages(self) -> None: self.node.assert_message_received("happypose/detections", timeout=180.0) self.node.assert_message_received("happypose/markers", timeout=6.0) self.node.assert_message_received("happypose/vision_info", timeout=6.0) - def test_05_check_vision_info(self) -> None: + def test_07_check_vision_info(self) -> None: vision_info = self.node.get_received_message("happypose/vision_info") self.assertEqual(vision_info.method, "cosypose") self.assertTrue("ycbv" in vision_info.database_location) - def test_06_check_detection(self) -> None: + def test_08_check_detection(self) -> None: detections = self.node.get_received_message("happypose/detections") self.assertEqual( detections.header.frame_id, "cam_1", "Incorrect frame_id in the header!" @@ -174,7 +197,7 @@ def test_06_check_detection(self) -> None: assert_bbox(ycbv_05.bbox, [394, 107, 77, 248], percent_error=50.0) assert_bbox(ycbv_15.bbox, [64, 204, 267, 151]) - def test_07_check_markers(self) -> None: + def test_09_check_markers(self) -> None: detections = self.node.get_received_message("happypose/detections") ycbv_02 = assert_and_find_detection(detections, "ycbv-obj_000002") ycbv_05 = assert_and_find_detection(detections, "ycbv-obj_000005") @@ -251,7 +274,7 @@ def test_07_check_markers(self) -> None: "Mesh expected to use 'mesh_use_embedded_materials'!", ) - def test_08_dynamic_params_labels_to_keep( + def test_10_dynamic_params_labels_to_keep( self, proc_output: ActiveIoHandler ) -> None: label_to_keep = "ycbv-obj_000002" @@ -285,7 +308,18 @@ def test_08_dynamic_params_labels_to_keep( msg="Filtered label is not the same as the expected one!", ) - def test_09_dynamic_params_markers(self) -> None: + self.node.assert_message_received("happypose/object_symmetries", timeout=20.0) + object_symmetries = self.node.get_received_message( + "happypose/object_symmetries" + ) + self.assertEqual( + len(object_symmetries.objects), + 1, + msg="Number of objects with symmetries does not reflect changes after filtering labels!", + ) + self.assertEqual(object_symmetries.objects[0].class_id, label_to_keep) + + def test_11_dynamic_params_markers(self) -> None: lifetime = 2137.0 self.node.set_params( [ @@ -328,8 +362,8 @@ def test_09_dynamic_params_markers(self) -> None: msg="Marker lifetime did not change!", ) - def test_10_dynamic_params_labels_to_keep_reset(self: ActiveIoHandler) -> None: - # Undoo filtering of the labels + def test_12_dynamic_params_labels_to_keep_reset(self: ActiveIoHandler) -> None: + # Undo filtering of the labels self.node.set_params( [ Parameter( @@ -353,3 +387,41 @@ def test_10_dynamic_params_labels_to_keep_reset(self: ActiveIoHandler) -> None: 3, msg="Detections were not brought back to being unfiltered!", ) + + self.node.assert_message_received("happypose/object_symmetries", timeout=5.0) + object_symmetries = self.node.get_received_message( + "happypose/object_symmetries" + ) + self.assertEqual( + len(object_symmetries.objects), + 21, + msg="Number of objects with symmetries is not equal to full YCBV dataset!", + ) + + def test_13_dynamic_params_labels_to_keep_empty_message( + self: ActiveIoHandler, + ) -> None: + # Undo filtering of the labels + self.node.set_params( + [ + Parameter( + "cosypose.inference.labels_to_keep", + Parameter.Type.STRING_ARRAY, + ["ycbv-obj_000020"], + ) + ] + ) + + # Clear old messages + self.node.clear_msg_buffer() + + # Publish new to trigger parameter change + self.node.publish_image("cam_1", self.rgb, self.K) + self.node.assert_message_received("happypose/detections", timeout=20.0) + detections = self.node.get_received_message("happypose/detections") + + self.assertGreaterEqual( + len(detections.detections), + 0, + "After filtering labels published message is not empty!", + ) diff --git a/happypose_ros/test/test_single_view.yaml b/happypose_ros/test/test_single_view.yaml index 05434d7..71d14f3 100644 --- a/happypose_ros/test/test_single_view.yaml +++ b/happypose_ros/test/test_single_view.yaml @@ -1,6 +1,6 @@ /**: ros__parameters: - # device is set from launchfile + # device is set from launch file verbose_info_logs: true pose_estimator_type: cosypose visualization.publish_markers: true @@ -17,4 +17,4 @@ cam_1: leading: true publish_tf: false - # compressed is set from launchfile + # compressed is set from launch file diff --git a/resources/happypose_ros_diagram.drawio.svg b/resources/happypose_ros_diagram.drawio.svg index 1c512f6..4cd6d92 100644 --- a/resources/happypose_ros_diagram.drawio.svg +++ b/resources/happypose_ros_diagram.drawio.svg @@ -1,4 +1,682 @@ - - - -
/happypose_node/set_parameters
ObservationTensor
Dict
with results
Dict with
detection results
happypose_worker
Process
/cam_1/image_color
/cam_1/camera_info
happypose_ros
Node
cam_1
CameraWrapper
/cam_2/image_color/compressed
/cam_2/camera_info
cam_2
CameraWrapper
/cam_n/image_color
/cam_n/camera_info
cam_n
CameraWrapper
/happypose/detections
main thread
_trigger_pipeline
function
/happypose/vision_info
/happypose/markers
/tf
Function call
ROS topic
ROS service
Information via queue
Spawns thread
_await_results
Thread
 + + + + + + + + + + +
+
+
+ /happypose_node/set_parameters +
+
+
+ + + /happypose_node/set_parameters + + + + + + + + +
+
+
+ ObservationTensor +
+
+
+ + + ObservationTensor + + + + + + + + + + +
+
+
+
+ Dict with +
+ detection +
+
+ results +
+
+
+
+
+ + + Dict with... + + + + + + + +
+
+
+ + happypose_worker +
+ + + Process + +
+
+
+ + + happypose_worker... + + + + + + + + +
+
+
+
+ /cam_1/image_color +
+
+
+
+ + + /cam_1/image_color + + + + + + + + +
+
+
+ /cam_1/camera_info +
+
+
+ + + /cam_1/camera_info + + + + + + + + + + +
+
+
+
+ happypose_ros +
+
+ + + Node + + +
+
+
+
+
+ + + happypose_ros... + + + + + + + + + +
+
+
+
+ + cam_1 + +
+
+ + + + CameraWrapper + +
+ + +
+
+
+
+ + + cam_1... + + + + + + + + +
+
+
+
+ /cam_2/image_color/compressed +
+
+
+
+ + + /cam_2/image_color/compressed + + + + + + + + +
+
+
+ /cam_2/camera_info +
+
+
+ + + /cam_2/camera_info + + + + + + + + + +
+
+
+
+ + cam_2 + +
+
+ + + + CameraWrapper + +
+ + +
+
+
+
+ + + cam_2... + + + + + + + + +
+
+
+
+ /cam_n/image_color +
+
+
+
+ + + /cam_n/image_color + + + + + + + + +
+
+
+ /cam_n/camera_info +
+
+
+ + + /cam_n/camera_info + + + + + + + + + +
+
+
+
+ + cam_n + +
+
+ + + + CameraWrapper + +
+ + +
+
+
+
+ + + cam_n... + + + + + + + + +
+
+
+
+ /happypose/detections +
+
+
+
+
+ + + /happypose/detections + + + + + + + + +
+
+
+
+ + main + + thread + + +
+
+
+
+ + + main thread + + + + + + + +
+
+
+
+ + _trigger_pipeline +
+ + + function + +
+ + +
+
+
+
+ + + _trigger_pipe... + + + + + + + + +
+
+
+
+ /happypose/vision_info +
+
+
+
+
+ + + /happypose/vision_info + + + + + + + + +
+
+
+
+ /happypose/markers +
+
+
+
+
+ + + /happypose/markers + + + + + + + + +
+
+
+
+ /tf +
+
+
+
+
+ + + /tf + + + + + + + + +
+
+
+
+ + Function call + +
+
+
+
+ + + Function call + + + + + + + + +
+
+
+ + ROS topic + +
+
+
+ + + ROS topic + + + + + + + + +
+
+
+
+ + ROS service + +
+
+
+
+ + + ROS service + + + + + + + + +
+
+
+ + Information via queue + +
+
+
+ + + Information via queue + + + + + + + + +
+
+
+ + Spawns thread + +
+
+
+ + + Spawns thread + + + + + + + +
+
+
+ + _await_results +
+ + + Thread + +
+
+
+ + + _await_results... + + + + + + + + +
+
+
+
+ Aux +
+
+ data +
+
+
+
+
+ + + Aux... + + + + + + + + +
+
+
+
+ /happypose/object_symmetries +
+
+
+
+
+ + + /happypose/object_symmetries + + + + + + + +
+
+
+
+ + status spinner +
+ + + function + +
+ + +
+
+
+
+ + + status... + + + + + + + + + Text is not SVG - cannot display + + + +