Merge pull request #1 from agimus-project/feature/publishing-symmetries

Add symmetries publisher and publish empty messages on no detections

.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:

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.

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)

happypose_examples/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>happypose_examples</name>
-  <version>0.0.0</version>
+  <version>0.2.0</version>
   <description>Examples for happypose_ros package</description>
   <author email="[email protected]">Krzysztof Wojciechowski</author>
   <maintainer email="[email protected]">Guilhem Saurel</maintainer>

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()

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
+}

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]

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

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

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

happypose_msgs/package.xml

@@ -0,0 +1,37 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>happypose_msgs</name>
+  <version>0.2.0</version>
+  <description>Custom messages used by happypose_ros package.</description>
+  <author email="[email protected]">Krzysztof Wojciechowski</author>
+  <maintainer email="[email protected]">Guilhem Saurel</maintainer>
+  <license>BSD</license>
+
+  <url type="website">https://github.com/agimus-project/happypose_ros</url>
+  <url type="bugtracker">https://github.com/agimus-project/happypose_ros/issues</url>
+  <url type="repository">https://github.com/agimus-project/happypose_ros</url>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>ament_cmake_python</buildtool_depend>
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+
+  <build_depend>builtin_interfaces</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+
+  <exec_depend>builtin_interfaces</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>python3-numpy</exec_depend>
+  <exec_depend>python3-transforms3d</exec_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+
+  <test_depend>pinocchio</test_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>