Add symmetries publisher and publish empty messages on no detections

README.md

@@ -61,6 +61,10 @@ The node provides the following ROS topics:
 
     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.0.2</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,21 @@
+cmake_minimum_required(VERSION 3.10)
+project(happypose_msgs)
+
+find_package(ament_cmake REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(geometry_msgs 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)
+
+ament_export_dependencies(rosidl_default_runtime)
+ament_package()

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 directyl
+# 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 be HappyPose node
+ObjectSymmetries[] objects

happypose_msgs/package.xml

@@ -0,0 +1,31 @@
+<?xml version="1.0"?>
+<package format="3">
+	<name>happypose_msgs</name>
+	<version>0.0.2</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://gitlab.laas.fr/kwojciecho/happypose_ros</url>
+	<url type="bugtracker">https://gitlab.laas.fr/kwojciecho/happypose_ros/-/issues</url>
+	<url type="repository">https://gitlab.laas.fr/kwojciecho/happypose_ros</url>
+
+  <buildtool_depend>ament_cmake</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>rosidl_default_runtime</exec_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

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