From e8d705b265b4d7eb093006339354dbfc4a53bfbf Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski Date: Tue, 16 Jul 2024 20:37:42 +0000 Subject: [PATCH 01/32] Untested symmetries publisher --- README.md | 4 + happypose_examples/CMakeLists.txt | 2 +- happypose_examples/package.xml | 2 +- happypose_msgs/CMakeLists.txt | 21 + happypose_msgs/msg/ContinuousSymmetry.msg | 6 + happypose_msgs/msg/ObjectSymmetries.msg | 10 + happypose_msgs/msg/ObjectSymmetriesArray.msg | 5 + happypose_msgs/package.xml | 31 + happypose_ros/happypose_ros/happypose_node.py | 153 ++-- .../happypose_ros/inference_pipeline.py | 12 + happypose_ros/happypose_ros/utils.py | 92 ++- happypose_ros/package.xml | 3 +- resources/happypose_ros_diagram.drawio.svg | 686 +++++++++++++++++- 13 files changed, 963 insertions(+), 64 deletions(-) create mode 100644 happypose_msgs/CMakeLists.txt create mode 100644 happypose_msgs/msg/ContinuousSymmetry.msg create mode 100644 happypose_msgs/msg/ObjectSymmetries.msg create mode 100644 happypose_msgs/msg/ObjectSymmetriesArray.msg create mode 100644 happypose_msgs/package.xml diff --git a/README.md b/README.md index f643648..32fccbe 100644 --- a/README.md +++ b/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. 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 1efaf92..81612ea 100644 --- a/happypose_examples/package.xml +++ b/happypose_examples/package.xml @@ -2,7 +2,7 @@ happypose_examples - 0.0.0 + 0.0.2 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..4bacce4 --- /dev/null +++ b/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() 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..9981f59 --- /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 directyl +# 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..d107608 --- /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 be HappyPose node +ObjectSymmetries[] objects diff --git a/happypose_msgs/package.xml b/happypose_msgs/package.xml new file mode 100644 index 0000000..6ce14dd --- /dev/null +++ b/happypose_msgs/package.xml @@ -0,0 +1,31 @@ + + + happypose_msgs + 0.0.2 + Custom messages used by happypose_ros package. + Krzysztof Wojciechowski + Guilhem Saurel + BSD + + https://gitlab.laas.fr/kwojciecho/happypose_ros + https://gitlab.laas.fr/kwojciecho/happypose_ros/-/issues + https://gitlab.laas.fr/kwojciecho/happypose_ros + + ament_cmake + rosidl_default_generators + + builtin_interfaces + std_msgs + geometry_msgs + + builtin_interfaces + std_msgs + geometry_msgs + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + + diff --git a/happypose_ros/happypose_ros/happypose_node.py b/happypose_ros/happypose_ros/happypose_node.py index 64a7257..ac6a82a 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(): @@ -73,6 +79,7 @@ def happypose_worker_proc( try: params = params_queue.get_nowait() pipeline.update_params(params) + symmetries_queue.put(pipeline.get_dataset()) except queue.Empty: pass @@ -121,6 +128,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 +141,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 +173,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,6 +261,10 @@ 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...", ) @@ -252,8 +275,12 @@ def destroy_node(self) -> None: self._observation_tensor_queue.close() if self._results_queue is not None: self._results_queue.close() + if self._symmetries_queue is not None: + self._symmetries_queue.close() 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._happypose_worker is not None: self._happypose_worker.join() self._happypose_worker.terminate() @@ -278,6 +305,20 @@ 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""" + while True: + try: + symmetries = self._symmetries_queue.get() + 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) + except ValueError: + break + 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 +368,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 +442,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 +452,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..32b9cc2 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 detector. + + :return: Dataset used by detector. + :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..8b000b7 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,70 @@ 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 + 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 corresponding to + + :param symmetry: HappyPose object storing definition of continuous 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 9694fa0..8773d7f 100644 --- a/happypose_ros/package.xml +++ b/happypose_ros/package.xml @@ -2,7 +2,7 @@ happypose_ros - 0.0.0 + 0.0.2 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/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 + + + + From d956f071efc3b2f0ab1223e0f446b6cf1cdf5278 Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski Date: Wed, 17 Jul 2024 15:00:02 +0000 Subject: [PATCH 02/32] Add unittest basis --- happypose_ros/test/happypose_testing_utils.py | 17 ++++ happypose_ros/test/single_view_base.py | 90 +++++++++++++++++-- 2 files changed, 98 insertions(+), 9 deletions(-) diff --git a/happypose_ros/test/happypose_testing_utils.py b/happypose_ros/test/happypose_testing_utils.py index 974e9ed..a9474b5 100644 --- a/happypose_ros/test/happypose_testing_utils.py +++ b/happypose_ros/test/happypose_testing_utils.py @@ -29,6 +29,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 +117,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 @@ -125,6 +128,12 @@ def __init__( self._vision_info_sub = self.create_subscription( VisionInfo, "happypose/vision_info", self._vision_info_cb, 5 ) + self._object_symmetries_sub = self.create_subscription( + ObjectSymmetriesArray, + "happypose/object_symmetries", + self._object_symmetries_cb, + 5, + ) # Initialize service clients self._set_param_cli = self.create_client( @@ -165,6 +174,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/vision_info"].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 4351a30..9232c75 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=20.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=20.0) self.node.assert_message_received("happypose/markers", timeout=2.0) self.node.assert_message_received("happypose/vision_info", timeout=2.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=5.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!", + ) From f840ba688fafab3fa84cc00cfbc03312b7c1dca5 Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski Date: Thu, 18 Jul 2024 08:52:59 +0000 Subject: [PATCH 03/32] Update setup.py --- .../{happypose_ros => happypose_node} | 0 happypose_ros/setup.py | 32 ++++++++++++++----- 2 files changed, 24 insertions(+), 8 deletions(-) rename happypose_ros/resource/{happypose_ros => happypose_node} (100%) diff --git a/happypose_ros/resource/happypose_ros b/happypose_ros/resource/happypose_node similarity index 100% rename from happypose_ros/resource/happypose_ros rename to happypose_ros/resource/happypose_node diff --git a/happypose_ros/setup.py b/happypose_ros/setup.py index 647d240..2dd2b8b 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}/config", + get_files(project_source_dir / "config", "*.yaml"), + ), + ( + f"share/{package_name}/test", + get_files(project_source_dir / "test", "*.py"), + ), + ( + f"share/{package_name}/test", + get_files(project_source_dir / "test", "*.png"), + ), ], install_requires=["setuptools"], zip_safe=True, From 58a8288b85e7b15bc2c8f00f7695eb8d934b69c3 Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski Date: Thu, 18 Jul 2024 08:54:06 +0000 Subject: [PATCH 04/32] Fix thread cleanup --- happypose_ros/happypose_ros/happypose_node.py | 33 ++++++++++++++----- 1 file changed, 25 insertions(+), 8 deletions(-) diff --git a/happypose_ros/happypose_ros/happypose_node.py b/happypose_ros/happypose_ros/happypose_node.py index ac6a82a..eba2d7b 100644 --- a/happypose_ros/happypose_ros/happypose_node.py +++ b/happypose_ros/happypose_ros/happypose_node.py @@ -74,6 +74,8 @@ 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: @@ -271,19 +273,30 @@ def __init__(self, node_name: str = "happypose_node", **kwargs) -> None: 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() - if self._symmetries_queue is not None: - self._symmetries_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: @@ -307,17 +320,21 @@ def _update_dynamic_params(self, on_init: bool = False) -> None: def _symmetries_queue_spinner(self) -> None: """Awaits new data in a queue with symmetries and publishes them on a ROS topic""" - while True: - try: - symmetries = self._symmetries_queue.get() + 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) - except ValueError: - break + # Queue is closed + except ValueError: + pass def _on_image_cb(self) -> None: """Callback function used to synchronize incoming images from different cameras. From f6832ba8f72479611206077a23fa4bc18832afd5 Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski Date: Thu, 18 Jul 2024 09:43:22 +0000 Subject: [PATCH 05/32] Fix typos | Change QOS in tests --- happypose_ros/test/happypose_testing_utils.py | 11 +++++++++-- happypose_ros/test/test_single_view.yaml | 4 ++-- 2 files changed, 11 insertions(+), 4 deletions(-) diff --git a/happypose_ros/test/happypose_testing_utils.py b/happypose_ros/test/happypose_testing_utils.py index a9474b5..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 @@ -128,11 +129,17 @@ def __init__( self._vision_info_sub = self.create_subscription( 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, - 5, + qos, ) # Initialize service clients @@ -180,7 +187,7 @@ def _object_symmetries_cb(self, msg: ObjectSymmetriesArray) -> None: :param msg: Message containing object symmetries :type msg: happypose_msgs.msg.ObjectSymmetriesArray """ - self._sub_topic["happypose/vision_info"].append(msg) + self._sub_topic["happypose/object_symmetries"].append(msg) def get_transform( self, target_frame: str, source_frame: str, timeout: float = 5.0 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 From 46933c0b7d08e147decbfddffc39dc9472b2c10a Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski Date: Thu, 18 Jul 2024 09:51:53 +0000 Subject: [PATCH 06/32] Fix typos in setup.py --- happypose_ros/setup.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/happypose_ros/setup.py b/happypose_ros/setup.py index 2dd2b8b..f64daec 100644 --- a/happypose_ros/setup.py +++ b/happypose_ros/setup.py @@ -25,10 +25,6 @@ def get_files(dir: Path, pattern: str) -> List[str]: data_files=[ ("share/ament_index/resource_index/packages", ["resource/happypose_node"]), ("share/happypose_ros", ["package.xml"]), - ( - f"share/{package_name}/config", - get_files(project_source_dir / "config", "*.yaml"), - ), ( f"share/{package_name}/test", get_files(project_source_dir / "test", "*.py"), @@ -37,6 +33,10 @@ def get_files(dir: Path, pattern: str) -> List[str]: 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, From d79df1caaeee629880511dc68487c8dd1e27c347 Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski Date: Tue, 23 Jul 2024 13:06:25 +0000 Subject: [PATCH 07/32] Fix typos pointed during review --- happypose_ros/happypose_ros/inference_pipeline.py | 4 ++-- happypose_ros/happypose_ros/utils.py | 9 ++++++--- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/happypose_ros/happypose_ros/inference_pipeline.py b/happypose_ros/happypose_ros/inference_pipeline.py index 32b9cc2..8098ee6 100644 --- a/happypose_ros/happypose_ros/inference_pipeline.py +++ b/happypose_ros/happypose_ros/inference_pipeline.py @@ -67,9 +67,9 @@ def update_params(self, params: dict) -> None: ) def get_dataset(self) -> RigidObjectDataset: - """Returns rigid object dataset used by detector. + """Returns rigid object dataset used by HappyPose pose estimator - :return: Dataset used by detector. + :return: Dataset used by HappyPose pose estimator :rtype: RigidObjectDataset """ dataset = self._wrapper.object_dataset diff --git a/happypose_ros/happypose_ros/utils.py b/happypose_ros/happypose_ros/utils.py index 8b000b7..93e2713 100644 --- a/happypose_ros/happypose_ros/utils.py +++ b/happypose_ros/happypose_ros/utils.py @@ -260,7 +260,10 @@ def continuous_symmetry_to_msg( :rtype: happypose_msgs.msg.ContinuousSymmetry """ return ContinuousSymmetry( - # Explicit conversion to float is required in this case + # 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]))), ) @@ -269,9 +272,9 @@ def continuous_symmetry_to_msg( def discrete_symmetry_to_msg( symmetry: happypose_symmetries.DiscreteSymmetry, ) -> Transform: - """Converts HappyPose DiscreteSymmetry object into Transform ROS message corresponding to + """Converts HappyPose DiscreteSymmetry object into Transform ROS message. - :param symmetry: HappyPose object storing definition of continuous symmetry. + :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 From 0ba5392ee84826d485c31285b3d681aac6a57783 Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski Date: Tue, 23 Jul 2024 13:07:10 +0000 Subject: [PATCH 08/32] Add draft for continuous to discrete symmetries converter --- happypose_msgs/CMakeLists.txt | 8 +++ happypose_msgs/happypose_msgs_py/__init__.py | 0 .../happypose_msgs_py/symmetries.py | 72 +++++++++++++++++++ happypose_msgs/package.xml | 3 + 4 files changed, 83 insertions(+) create mode 100644 happypose_msgs/happypose_msgs_py/__init__.py create mode 100644 happypose_msgs/happypose_msgs_py/symmetries.py diff --git a/happypose_msgs/CMakeLists.txt b/happypose_msgs/CMakeLists.txt index 4bacce4..71d9671 100644 --- a/happypose_msgs/CMakeLists.txt +++ b/happypose_msgs/CMakeLists.txt @@ -2,9 +2,13 @@ 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( @@ -17,5 +21,9 @@ rosidl_generate_interfaces( std_msgs geometry_msgs) +# Install Python modules +ament_python_install_package(${PROJECT_NAME}_py) +install(FILES package.xml DESTINATION share/${PROJECT_NAME}) + ament_export_dependencies(rosidl_default_runtime) ament_package() diff --git a/happypose_msgs/happypose_msgs_py/__init__.py b/happypose_msgs/happypose_msgs_py/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/happypose_msgs/happypose_msgs_py/symmetries.py b/happypose_msgs/happypose_msgs_py/symmetries.py new file mode 100644 index 0000000..22bf4e5 --- /dev/null +++ b/happypose_msgs/happypose_msgs_py/symmetries.py @@ -0,0 +1,72 @@ +import numpy as np +import numpy.typing as npt +import transforms3d + +from geometry_msgs.msg import Transform + +from happypose_msgs.msg import ContinuousSymmetry, ObjectSymmetries + + +def discretize_poses( + object_symmetries: ObjectSymmetries, n_symmetries_continuous: int = 8 +) -> npt.NDArray[np.float64]: + """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 + :return: List of a shape (n, 4, 4) with ``n`` SE3 transformation matrices representing symmetries. + :rtype: npt.NDArray[np.float64] + """ + + def _discretize_continuous( + sym: ContinuousSymmetry, idx: int + ) -> npt.NDArray[np.float64]: + axis = np.array([sym.axis.x, sym.axis.y, sym.axis.z]) + if not np.isclose(axis.sum(), 1.0): + raise ValueError( + f"Continuous symmetry at index {idx} has non unitary rotation axis!" + ) + symmetries = np.zeros((n_symmetries_continuous, 4, 4)) + + # Pre compute steps of rotations + rot_base = 2.0 * axis * np.pi / n_symmetries_continuous + for i in range(n_symmetries_continuous): + symmetries[i, :3, :3] = transforms3d.euler.euler2mat(*(rot_base * i)) + + symmetries[:, -1, -1] = 1.0 + symmetries[:, :3, -1] = np.array([sym.offset.x, sym.offset.y, sym.offset.z]) + + return symmetries + + symmetries_continuous = np.array( + [ + _discretize_continuous(sym_c, idx) + for idx, sym_c in enumerate(object_symmetries.symmetries_continuous) + ] + ).reshape((-1, 4, 4)) + + def _transform_msg_to_mat(sym: Transform) -> npt.NDArray[np.float64]: + M = np.eye(4) + M[:3, :3] = transforms3d.quaternions.quat2mat( + (sym.rotation.w, sym.rotation.x, sym.rotation.y, sym.rotation.z) + ) + M[0, -1] = sym.translation.x + M[1, -1] = sym.translation.y + M[2, -1] = sym.translation.z + return M + + symmetries_discrete = np.array( + [ + _transform_msg_to_mat(sym_d) + for sym_d in object_symmetries.symmetries_discrete + ] + ).reshape((-1, 4, 4)) + + symmetries_mixed = [ + (symmetries_discrete[i] @ symmetries_continuous).reshape((-1, 4, 4)) + for i in range(len(object_symmetries.symmetries_discrete)) + ] + + return np.vstack([symmetries_continuous, symmetries_discrete, *symmetries_mixed]) diff --git a/happypose_msgs/package.xml b/happypose_msgs/package.xml index 6ce14dd..53b6f16 100644 --- a/happypose_msgs/package.xml +++ b/happypose_msgs/package.xml @@ -12,15 +12,18 @@ https://gitlab.laas.fr/kwojciecho/happypose_ros ament_cmake + ament_cmake_python rosidl_default_generators builtin_interfaces std_msgs geometry_msgs + rclpy builtin_interfaces std_msgs geometry_msgs + rclpy rosidl_default_runtime rosidl_interface_packages From e9e8e03980a98dc80d8fc85f9a616c74963d84b5 Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski Date: Tue, 23 Jul 2024 13:17:19 +0000 Subject: [PATCH 09/32] Add package dependencies --- happypose_msgs/package.xml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/happypose_msgs/package.xml b/happypose_msgs/package.xml index 53b6f16..110ce57 100644 --- a/happypose_msgs/package.xml +++ b/happypose_msgs/package.xml @@ -18,12 +18,13 @@ builtin_interfaces std_msgs geometry_msgs - rclpy builtin_interfaces std_msgs geometry_msgs rclpy + python3-numpy + python3-transforms3d rosidl_default_runtime rosidl_interface_packages From 278047af78b84a595d2c50453e74bca607cccf4c Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski Date: Tue, 23 Jul 2024 13:23:36 +0000 Subject: [PATCH 10/32] Fix formatting of package.xml --- happypose_msgs/package.xml | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/happypose_msgs/package.xml b/happypose_msgs/package.xml index 110ce57..5123574 100644 --- a/happypose_msgs/package.xml +++ b/happypose_msgs/package.xml @@ -1,15 +1,15 @@ - happypose_msgs - 0.0.2 - Custom messages used by happypose_ros package. - Krzysztof Wojciechowski - Guilhem Saurel - BSD + happypose_msgs + 0.0.2 + Custom messages used by happypose_ros package. + Krzysztof Wojciechowski + Guilhem Saurel + BSD - https://gitlab.laas.fr/kwojciecho/happypose_ros - https://gitlab.laas.fr/kwojciecho/happypose_ros/-/issues - https://gitlab.laas.fr/kwojciecho/happypose_ros + https://gitlab.laas.fr/kwojciecho/happypose_ros + https://gitlab.laas.fr/kwojciecho/happypose_ros/-/issues + https://gitlab.laas.fr/kwojciecho/happypose_ros ament_cmake ament_cmake_python From b4fc4e63ca5f4abdc51b2c8a90f0cafc8f1f248b Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski Date: Tue, 23 Jul 2024 13:55:03 +0000 Subject: [PATCH 11/32] Add option to return ROS messages --- .../happypose_msgs_py/symmetries.py | 37 ++++++++++++++++--- 1 file changed, 31 insertions(+), 6 deletions(-) diff --git a/happypose_msgs/happypose_msgs_py/symmetries.py b/happypose_msgs/happypose_msgs_py/symmetries.py index 22bf4e5..ec7f86d 100644 --- a/happypose_msgs/happypose_msgs_py/symmetries.py +++ b/happypose_msgs/happypose_msgs_py/symmetries.py @@ -1,25 +1,37 @@ import numpy as np import numpy.typing as npt import transforms3d +from typing import List, Union -from geometry_msgs.msg import Transform +from geometry_msgs.msg import Transform, Vector3, Quaternion from happypose_msgs.msg import ContinuousSymmetry, ObjectSymmetries def discretize_poses( - object_symmetries: ObjectSymmetries, n_symmetries_continuous: int = 8 -) -> npt.NDArray[np.float64]: + 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 - :return: List of a shape (n, 4, 4) with ``n`` SE3 transformation matrices representing symmetries. - :rtype: npt.NDArray[np.float64] + :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 not continuous symmetries and ROS message is expected skip computations + if return_ros_msg and len(object_symmetries.symmetries_continuous) == 0: + return object_symmetries.symmetries_discrete + def _discretize_continuous( sym: ContinuousSymmetry, idx: int ) -> npt.NDArray[np.float64]: @@ -69,4 +81,17 @@ def _transform_msg_to_mat(sym: Transform) -> npt.NDArray[np.float64]: for i in range(len(object_symmetries.symmetries_discrete)) ] - return np.vstack([symmetries_continuous, symmetries_discrete, *symmetries_mixed]) + symmetries = np.vstack( + [symmetries_continuous, symmetries_discrete, *symmetries_mixed] + ) + if not return_ros_msg: + return symmetries + + def _mat_to_msg(M: npt.NDArray[np.float64]): + 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 symmetries] From 31b280fc07315eb85fb32376809c7487aa509c88 Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski Date: Tue, 23 Jul 2024 13:55:52 +0000 Subject: [PATCH 12/32] Rename function --- happypose_msgs/happypose_msgs_py/symmetries.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/happypose_msgs/happypose_msgs_py/symmetries.py b/happypose_msgs/happypose_msgs_py/symmetries.py index ec7f86d..31e6b64 100644 --- a/happypose_msgs/happypose_msgs_py/symmetries.py +++ b/happypose_msgs/happypose_msgs_py/symmetries.py @@ -8,7 +8,7 @@ from happypose_msgs.msg import ContinuousSymmetry, ObjectSymmetries -def discretize_poses( +def discretize_symmetries( object_symmetries: ObjectSymmetries, n_symmetries_continuous: int = 8, return_ros_msg: bool = False, From 1c35c1800dfb38e455fdcf47ed3fead389264a8a Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski Date: Tue, 23 Jul 2024 13:57:24 +0000 Subject: [PATCH 13/32] Fix comment --- happypose_msgs/happypose_msgs_py/symmetries.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/happypose_msgs/happypose_msgs_py/symmetries.py b/happypose_msgs/happypose_msgs_py/symmetries.py index 31e6b64..38ed714 100644 --- a/happypose_msgs/happypose_msgs_py/symmetries.py +++ b/happypose_msgs/happypose_msgs_py/symmetries.py @@ -42,7 +42,7 @@ def _discretize_continuous( ) symmetries = np.zeros((n_symmetries_continuous, 4, 4)) - # Pre compute steps of rotations + # Precompute steps of rotations rot_base = 2.0 * axis * np.pi / n_symmetries_continuous for i in range(n_symmetries_continuous): symmetries[i, :3, :3] = transforms3d.euler.euler2mat(*(rot_base * i)) From 69709fbe28c12677a696915abe269cb8bbd6d50a Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski Date: Tue, 23 Jul 2024 15:34:26 +0000 Subject: [PATCH 14/32] Add base for unittests --- happypose_msgs/CMakeLists.txt | 16 +++ happypose_msgs/package.xml | 2 + .../test/test_discretize_symmetries.py | 104 ++++++++++++++++++ happypose_ros/.gitignore | 2 + 4 files changed, 124 insertions(+) create mode 100644 happypose_msgs/test/test_discretize_symmetries.py diff --git a/happypose_msgs/CMakeLists.txt b/happypose_msgs/CMakeLists.txt index 71d9671..1710b23 100644 --- a/happypose_msgs/CMakeLists.txt +++ b/happypose_msgs/CMakeLists.txt @@ -25,5 +25,21 @@ rosidl_generate_interfaces( 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/package.xml b/happypose_msgs/package.xml index 5123574..f384f27 100644 --- a/happypose_msgs/package.xml +++ b/happypose_msgs/package.xml @@ -27,6 +27,8 @@ python3-transforms3d rosidl_default_runtime + pinocchio + rosidl_interface_packages diff --git a/happypose_msgs/test/test_discretize_symmetries.py b/happypose_msgs/test/test_discretize_symmetries.py new file mode 100644 index 0000000..9219bf8 --- /dev/null +++ b/happypose_msgs/test/test_discretize_symmetries.py @@ -0,0 +1,104 @@ +#!/usr/bin/env python + +import numpy as np +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 ObjectSymmetries + + +def are_transforms_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) + ] + diff = T1.inverse() * T2 + return np.linalg.norm(pin.log6(diff).vector) < 1e-6 + + +def is_transform_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_transforms_close(t1, t2) for t2 in t_list) + + +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_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.0, y=0.0, z=0.0, w=1.0), + ), + ], + 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!" + + assert all( + is_transform_in_list(t, msg.symmetries_discrete) for t in res + ), "Resulted discrete symmetries are not close to the initial ones int the message!" diff --git a/happypose_ros/.gitignore b/happypose_ros/.gitignore index b6a8238..ab3b224 100644 --- a/happypose_ros/.gitignore +++ b/happypose_ros/.gitignore @@ -20,3 +20,5 @@ AMENT_IGNORE # 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__ +happypose_msgs/test/__pycache__ +happypose_msgs_py/test/__pycache__ From 604184c0ab572911cd93ffb4fc13eceacc548b7b Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> Date: Wed, 24 Jul 2024 10:23:03 +0200 Subject: [PATCH 15/32] Smarter way to aviod `__pycache__` Co-authored-by: Naveau --- happypose_ros/.gitignore | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/happypose_ros/.gitignore b/happypose_ros/.gitignore index ab3b224..8f79cb6 100644 --- a/happypose_ros/.gitignore +++ b/happypose_ros/.gitignore @@ -19,6 +19,6 @@ AMENT_IGNORE # 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__ +*__pycache__* happypose_msgs/test/__pycache__ happypose_msgs_py/test/__pycache__ From 5162234b9690c1f8456fcd77043b6e3bc2d40c64 Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> Date: Wed, 24 Jul 2024 10:23:14 +0200 Subject: [PATCH 16/32] Update happypose_msgs/msg/ObjectSymmetriesArray.msg Co-authored-by: Naveau --- happypose_msgs/msg/ObjectSymmetriesArray.msg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/happypose_msgs/msg/ObjectSymmetriesArray.msg b/happypose_msgs/msg/ObjectSymmetriesArray.msg index d107608..984a64d 100644 --- a/happypose_msgs/msg/ObjectSymmetriesArray.msg +++ b/happypose_msgs/msg/ObjectSymmetriesArray.msg @@ -1,5 +1,5 @@ # Time stamp at which the message was sent std_msgs/Header header -# List of objects detected be HappyPose node +# List of objects detected by the HappyPose node ObjectSymmetries[] objects From c15e79485e722152a02e1ec640a71bd10c6d1662 Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> Date: Wed, 24 Jul 2024 10:23:24 +0200 Subject: [PATCH 17/32] Update happypose_msgs/msg/ObjectSymmetries.msg Co-authored-by: Naveau --- happypose_msgs/msg/ObjectSymmetries.msg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/happypose_msgs/msg/ObjectSymmetries.msg b/happypose_msgs/msg/ObjectSymmetries.msg index 9981f59..a194d05 100644 --- a/happypose_msgs/msg/ObjectSymmetries.msg +++ b/happypose_msgs/msg/ObjectSymmetries.msg @@ -4,7 +4,7 @@ string class_id # If no symmetries of a given type, list is left empty. # In HappyPose discrete symmetries are represented as -# transformation matrices. Those matrices are directyl +# transformation matrices. Those matrices are directly # converted to the Transform message. geometry_msgs/Transform[] symmetries_discrete ContinuousSymmetry[] symmetries_continuous From 89c0140fab3603d880b5f03e4d358b1781688f64 Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> Date: Wed, 24 Jul 2024 10:23:36 +0200 Subject: [PATCH 18/32] Update happypose_ros/.gitignore Co-authored-by: Naveau --- happypose_ros/.gitignore | 1 - 1 file changed, 1 deletion(-) diff --git a/happypose_ros/.gitignore b/happypose_ros/.gitignore index 8f79cb6..6cb3687 100644 --- a/happypose_ros/.gitignore +++ b/happypose_ros/.gitignore @@ -20,5 +20,4 @@ AMENT_IGNORE # The file has to be manually modified as the code generations has a bug in __map happypose_ros/happypose_ros_parameters.py *__pycache__* -happypose_msgs/test/__pycache__ happypose_msgs_py/test/__pycache__ From b4eb1e00043878bbda428533ae7dc8cc26979382 Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> Date: Wed, 24 Jul 2024 10:23:46 +0200 Subject: [PATCH 19/32] Update happypose_ros/.gitignore Co-authored-by: Naveau --- happypose_ros/.gitignore | 1 - 1 file changed, 1 deletion(-) diff --git a/happypose_ros/.gitignore b/happypose_ros/.gitignore index 6cb3687..995f9b1 100644 --- a/happypose_ros/.gitignore +++ b/happypose_ros/.gitignore @@ -20,4 +20,3 @@ AMENT_IGNORE # The file has to be manually modified as the code generations has a bug in __map happypose_ros/happypose_ros_parameters.py *__pycache__* -happypose_msgs_py/test/__pycache__ From ee3f145ba7d7ef5caee4e394d4575683cb2fd9f9 Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski Date: Wed, 24 Jul 2024 13:58:51 +0000 Subject: [PATCH 20/32] Cover all test cases | Fix bugs --- .../happypose_msgs_py/symmetries.py | 11 +- .../test/test_discretize_symmetries.py | 267 +++++++++++++++++- 2 files changed, 264 insertions(+), 14 deletions(-) diff --git a/happypose_msgs/happypose_msgs_py/symmetries.py b/happypose_msgs/happypose_msgs_py/symmetries.py index 38ed714..08b6fe2 100644 --- a/happypose_msgs/happypose_msgs_py/symmetries.py +++ b/happypose_msgs/happypose_msgs_py/symmetries.py @@ -36,16 +36,18 @@ def _discretize_continuous( sym: ContinuousSymmetry, idx: int ) -> npt.NDArray[np.float64]: axis = np.array([sym.axis.x, sym.axis.y, sym.axis.z]) - if not np.isclose(axis.sum(), 1.0): + if not np.isclose(np.linalg.norm(axis), 1.0): raise ValueError( f"Continuous symmetry at index {idx} has non unitary rotation axis!" ) symmetries = np.zeros((n_symmetries_continuous, 4, 4)) # Precompute steps of rotations - rot_base = 2.0 * axis * np.pi / n_symmetries_continuous + rot_base = 2.0 * np.pi / n_symmetries_continuous for i in range(n_symmetries_continuous): - symmetries[i, :3, :3] = transforms3d.euler.euler2mat(*(rot_base * i)) + symmetries[i, :3, :3] = transforms3d.axangles.axangle2mat( + axis, rot_base * i + ) symmetries[:, -1, -1] = 1.0 symmetries[:, :3, -1] = np.array([sym.offset.x, sym.offset.y, sym.offset.z]) @@ -61,8 +63,9 @@ def _discretize_continuous( def _transform_msg_to_mat(sym: Transform) -> npt.NDArray[np.float64]: M = np.eye(4) + M[:3, :3] = transforms3d.quaternions.quat2mat( - (sym.rotation.w, sym.rotation.x, sym.rotation.y, sym.rotation.z) + [sym.rotation.w, sym.rotation.x, sym.rotation.y, sym.rotation.z] ) M[0, -1] = sym.translation.x M[1, -1] = sym.translation.y diff --git a/happypose_msgs/test/test_discretize_symmetries.py b/happypose_msgs/test/test_discretize_symmetries.py index 9219bf8..664f039 100644 --- a/happypose_msgs/test/test_discretize_symmetries.py +++ b/happypose_msgs/test/test_discretize_symmetries.py @@ -1,6 +1,7 @@ #!/usr/bin/env python import numpy as np +import numpy.typing as npt import pinocchio as pin from typing import List @@ -8,10 +9,24 @@ from happypose_msgs_py.symmetries import discretize_symmetries -from happypose_msgs.msg import ObjectSymmetries +from happypose_msgs.msg import ContinuousSymmetry, ObjectSymmetries -def are_transforms_close(t1: Transform, t2: Transform) -> bool: +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. @@ -35,11 +50,10 @@ def are_transforms_close(t1: Transform, t2: Transform) -> bool: ) for t in (t1, t2) ] - diff = T1.inverse() * T2 - return np.linalg.norm(pin.log6(diff).vector) < 1e-6 + return are_transforms_close(T1, T2) -def is_transform_in_list(t1: Transform, t_list: List[Transform]) -> bool: +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. @@ -49,7 +63,39 @@ def is_transform_in_list(t1: Transform, t_list: List[Transform]) -> bool: :return: If the transform in the list. :rtype: bool """ - return any(are_transforms_close(t1, t2) for t2 in t_list) + 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: @@ -74,6 +120,28 @@ def test_empty_message_ros() -> None: 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=[ @@ -83,7 +151,7 @@ def test_only_discrete_ros() -> None: ), Transform( translation=Vector3(x=0.1, y=0.1, z=0.1), - rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + rotation=Quaternion(x=0.707, y=0.0, z=0.0, w=0.707), ), ], symmetries_continuous=[], @@ -99,6 +167,185 @@ def test_only_discrete_ros() -> None: isinstance(r, Transform) for r in res ), "Returned type of elements in the list is not geometry_msgs.msg.Transform!" - assert all( - is_transform_in_list(t, msg.symmetries_discrete) for t in res - ), "Resulted discrete symmetries are not close to the initial ones int the message!" + 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( + pin.Quaternion(pin.AngleAxis(2.0 * np.pi / n_symmetries * i, axis)), + offset, + ) + 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( + pin.Quaternion(pin.AngleAxis(2.0 * np.pi / n_symmetries * i, axis)), + offset, + ) + 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( + pin.Quaternion(pin.AngleAxis(2.0 * np.pi / n_symmetries * i, axis)), + offset, + ) + 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( + pin.Quaternion(pin.AngleAxis(2.0 * np.pi / n_symmetries * i, axis)), + offset, + ) + 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!" From df8a165194216d44e892a2e79e4dcd50723964ff Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski Date: Wed, 24 Jul 2024 14:06:55 +0000 Subject: [PATCH 21/32] Update package.xml --- happypose_msgs/package.xml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/happypose_msgs/package.xml b/happypose_msgs/package.xml index f384f27..6623571 100644 --- a/happypose_msgs/package.xml +++ b/happypose_msgs/package.xml @@ -7,9 +7,9 @@ Guilhem Saurel BSD - https://gitlab.laas.fr/kwojciecho/happypose_ros - https://gitlab.laas.fr/kwojciecho/happypose_ros/-/issues - https://gitlab.laas.fr/kwojciecho/happypose_ros + 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 From 81be23282cfd754ac07ba65febc80b98097a716e Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski Date: Wed, 24 Jul 2024 14:09:40 +0000 Subject: [PATCH 22/32] Extend time for unit tests --- happypose_ros/test/single_view_base.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/happypose_ros/test/single_view_base.py b/happypose_ros/test/single_view_base.py index a0779dd..5b00779 100644 --- a/happypose_ros/test/single_view_base.py +++ b/happypose_ros/test/single_view_base.py @@ -86,7 +86,7 @@ def test_02_check_topics(self) -> None: self.node.assert_node_is_publisher("happypose/object_symmetries", timeout=3.0) def test_03_receive_object_symmetries(self) -> None: - self.node.assert_message_received("happypose/object_symmetries", timeout=20.0) + 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( @@ -308,7 +308,7 @@ def test_10_dynamic_params_labels_to_keep( msg="Filtered label is not the same as the expected one!", ) - self.node.assert_message_received("happypose/object_symmetries", timeout=5.0) + self.node.assert_message_received("happypose/object_symmetries", timeout=20.0) object_symmetries = self.node.get_received_message( "happypose/object_symmetries" ) From c6eefb539ecafd9964706b8c00accfa381c4cc4d Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski Date: Wed, 24 Jul 2024 14:11:13 +0000 Subject: [PATCH 23/32] Update .gitignore --- happypose_ros/.gitignore | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/happypose_ros/.gitignore b/happypose_ros/.gitignore index 995f9b1..d635094 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 @@ -19,4 +19,3 @@ AMENT_IGNORE # 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 -*__pycache__* From 052c273f32fc6b1ac55c5b9022b1c1f27f360757 Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski Date: Wed, 24 Jul 2024 14:13:58 +0000 Subject: [PATCH 24/32] FIx gitignore comment --- happypose_ros/.gitignore | 1 - 1 file changed, 1 deletion(-) diff --git a/happypose_ros/.gitignore b/happypose_ros/.gitignore index d635094..ffc6220 100644 --- a/happypose_ros/.gitignore +++ b/happypose_ros/.gitignore @@ -17,5 +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 From bd5de3214aab35b3f561e3df0e14261308a8b679 Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski Date: Wed, 24 Jul 2024 14:47:28 +0000 Subject: [PATCH 25/32] Reduce workload on actions | Fix test order --- .github/workflows/happypose_ros_build_and_test.yaml | 8 +++++++- happypose_ros/test/single_view_base.py | 2 +- 2 files changed, 8 insertions(+), 2 deletions(-) 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/happypose_ros/test/single_view_base.py b/happypose_ros/test/single_view_base.py index 5b00779..fd7551b 100644 --- a/happypose_ros/test/single_view_base.py +++ b/happypose_ros/test/single_view_base.py @@ -121,7 +121,7 @@ def test_05_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) From 11621b5568e3827ea69bf57f80603a0163c7cf8b Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski Date: Thu, 25 Jul 2024 08:47:23 +0000 Subject: [PATCH 26/32] Hopefully reduce number of memory allocations --- .../happypose_msgs_py/symmetries.py | 104 +++++++++--------- 1 file changed, 51 insertions(+), 53 deletions(-) diff --git a/happypose_msgs/happypose_msgs_py/symmetries.py b/happypose_msgs/happypose_msgs_py/symmetries.py index 08b6fe2..de7ac98 100644 --- a/happypose_msgs/happypose_msgs_py/symmetries.py +++ b/happypose_msgs/happypose_msgs_py/symmetries.py @@ -1,3 +1,4 @@ +from copy import copy import numpy as np import numpy.typing as npt import transforms3d @@ -5,7 +6,7 @@ from geometry_msgs.msg import Transform, Vector3, Quaternion -from happypose_msgs.msg import ContinuousSymmetry, ObjectSymmetries +from happypose_msgs.msg import ObjectSymmetries def discretize_symmetries( @@ -30,71 +31,68 @@ def discretize_symmetries( # If there are not continuous symmetries and ROS message is expected skip computations if return_ros_msg and len(object_symmetries.symmetries_continuous) == 0: - return object_symmetries.symmetries_discrete + return copy(object_symmetries.symmetries_discrete) - def _discretize_continuous( - sym: ContinuousSymmetry, idx: int - ) -> npt.NDArray[np.float64]: - axis = np.array([sym.axis.x, sym.axis.y, sym.axis.z]) - if not np.isclose(np.linalg.norm(axis), 1.0): - raise ValueError( - f"Continuous symmetry at index {idx} has non unitary rotation axis!" - ) - symmetries = np.zeros((n_symmetries_continuous, 4, 4)) - - # Precompute steps of rotations - rot_base = 2.0 * np.pi / n_symmetries_continuous - for i in range(n_symmetries_continuous): - symmetries[i, :3, :3] = transforms3d.axangles.axangle2mat( - axis, rot_base * i - ) - - symmetries[:, -1, -1] = 1.0 - symmetries[:, :3, -1] = np.array([sym.offset.x, sym.offset.y, sym.offset.z]) - - return symmetries + n_con = len(object_symmetries.symmetries_continuous) * n_symmetries_continuous + n_disc = len(object_symmetries.symmetries_discrete) + n_mix = n_con * n_disc - symmetries_continuous = np.array( - [ - _discretize_continuous(sym_c, idx) - for idx, sym_c in enumerate(object_symmetries.symmetries_continuous) - ] - ).reshape((-1, 4, 4)) + # Preallocate memory for results + out = np.zeros((n_con + n_disc + n_mix, 4, 4)) - def _transform_msg_to_mat(sym: Transform) -> npt.NDArray[np.float64]: - M = np.eye(4) + # Precompute steps of rotations + rot_base = 2.0 * np.pi / n_symmetries_continuous - M[:3, :3] = transforms3d.quaternions.quat2mat( - [sym.rotation.w, sym.rotation.x, sym.rotation.y, sym.rotation.z] + # 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 + out[begin:end, :3, :3] = np.array( + [ + transforms3d.axangles.axangle2mat(axis, rot_base * j) + for j in range(n_symmetries_continuous) + ] + ) + out[begin:end, :, -1] = np.array( + [sym_c.offset.x, sym_c.offset.y, sym_c.offset.z, 1.0] ) - M[0, -1] = sym.translation.x - M[1, -1] = sym.translation.y - M[2, -1] = sym.translation.z - return M - symmetries_discrete = np.array( - [ - _transform_msg_to_mat(sym_d) - for sym_d in object_symmetries.symmetries_discrete - ] - ).reshape((-1, 4, 4)) + # 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] + ) - symmetries_mixed = [ - (symmetries_discrete[i] @ symmetries_continuous).reshape((-1, 4, 4)) - for i in range(len(object_symmetries.symmetries_discrete)) - ] + sym_c_d_end = n_con + n_disc + symmetries_continuous = out[:n_con] + # Combine discrete symmetries with possible continuous rotations + # TODO @MedericFourmy 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 - symmetries = np.vstack( - [symmetries_continuous, symmetries_discrete, *symmetries_mixed] - ) if not return_ros_msg: - return symmetries + return out - def _mat_to_msg(M: npt.NDArray[np.float64]): + 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 symmetries] + return [_mat_to_msg(M) for M in out] From bf62c59f775590382b7a87111baaa92159cb0793 Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski Date: Thu, 25 Jul 2024 08:48:45 +0000 Subject: [PATCH 27/32] Update comment --- happypose_msgs/happypose_msgs_py/symmetries.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/happypose_msgs/happypose_msgs_py/symmetries.py b/happypose_msgs/happypose_msgs_py/symmetries.py index de7ac98..78357a3 100644 --- a/happypose_msgs/happypose_msgs_py/symmetries.py +++ b/happypose_msgs/happypose_msgs_py/symmetries.py @@ -76,8 +76,8 @@ def discretize_symmetries( sym_c_d_end = n_con + n_disc symmetries_continuous = out[:n_con] # Combine discrete symmetries with possible continuous rotations - # TODO @MedericFourmy ensure this operation is valid for all object and not only objects with offset - # being at the origin of the coordinate system. + # 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 From 0be9c5e375f481d1adbbf8b1b0ecc968614f80ba Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski Date: Thu, 25 Jul 2024 08:51:31 +0000 Subject: [PATCH 28/32] Fix pacakge version from 0.0.2 to 0.2.0 --- happypose_examples/package.xml | 2 +- happypose_msgs/package.xml | 2 +- happypose_ros/package.xml | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/happypose_examples/package.xml b/happypose_examples/package.xml index 4cc4b12..043f119 100644 --- a/happypose_examples/package.xml +++ b/happypose_examples/package.xml @@ -2,7 +2,7 @@ happypose_examples - 0.0.2 + 0.2.0 Examples for happypose_ros package Krzysztof Wojciechowski Guilhem Saurel diff --git a/happypose_msgs/package.xml b/happypose_msgs/package.xml index 6623571..78aafac 100644 --- a/happypose_msgs/package.xml +++ b/happypose_msgs/package.xml @@ -1,7 +1,7 @@ happypose_msgs - 0.0.2 + 0.2.0 Custom messages used by happypose_ros package. Krzysztof Wojciechowski Guilhem Saurel diff --git a/happypose_ros/package.xml b/happypose_ros/package.xml index 07815a2..0d69a3a 100644 --- a/happypose_ros/package.xml +++ b/happypose_ros/package.xml @@ -2,7 +2,7 @@ happypose_ros - 0.0.2 + 0.2.0 ROS 2 wrapper around HappyPose python library for 6D pose estimation Krzysztof Wojciechowski Guilhem Saurel From c1964f5dec45d3b547fcdbac6b08663c4fed45b7 Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski Date: Thu, 25 Jul 2024 11:54:23 +0000 Subject: [PATCH 29/32] FIx comment typo --- happypose_msgs/happypose_msgs_py/symmetries.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/happypose_msgs/happypose_msgs_py/symmetries.py b/happypose_msgs/happypose_msgs_py/symmetries.py index 78357a3..7d0fe7e 100644 --- a/happypose_msgs/happypose_msgs_py/symmetries.py +++ b/happypose_msgs/happypose_msgs_py/symmetries.py @@ -29,7 +29,7 @@ def discretize_symmetries( :rtype: Union[npt.NDArray[np.float64], List[geometry_msgs.msg.Transform]] """ - # If there are not continuous symmetries and ROS message is expected skip computations + # 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) From 234126079503942fda2514d9390928fa5e1e88ce Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski Date: Fri, 2 Aug 2024 15:26:03 +0000 Subject: [PATCH 30/32] Fix rotations around symmetry axis --- .../happypose_msgs_py/symmetries.py | 14 ++++++++--- .../test/test_discretize_symmetries.py | 24 ++++++++++++------- 2 files changed, 27 insertions(+), 11 deletions(-) diff --git a/happypose_msgs/happypose_msgs_py/symmetries.py b/happypose_msgs/happypose_msgs_py/symmetries.py index 7d0fe7e..4f5920b 100644 --- a/happypose_msgs/happypose_msgs_py/symmetries.py +++ b/happypose_msgs/happypose_msgs_py/symmetries.py @@ -53,15 +53,23 @@ def discretize_symmetries( # 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, rot_base * j) for j in range(n_symmetries_continuous) ] ) - out[begin:end, :, -1] = np.array( - [sym_c.offset.x, sym_c.offset.y, sym_c.offset.z, 1.0] - ) + # 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): diff --git a/happypose_msgs/test/test_discretize_symmetries.py b/happypose_msgs/test/test_discretize_symmetries.py index 664f039..41dfe22 100644 --- a/happypose_msgs/test/test_discretize_symmetries.py +++ b/happypose_msgs/test/test_discretize_symmetries.py @@ -180,10 +180,12 @@ def test_only_continuous_np() -> None: offset = np.array([-0.1, 0.0, 0.1]) t_list = [ - pin.SE3( + pin.SE3(np.eye(3), offset) + * pin.SE3( pin.Quaternion(pin.AngleAxis(2.0 * np.pi / n_symmetries * i, axis)), - offset, + np.array([0.0, 0.0, 0.0]), ) + * pin.SE3(np.eye(3), offset).inverse() for i in range(n_symmetries) ] msg = ObjectSymmetries( @@ -211,10 +213,12 @@ def test_only_continuous_ros() -> None: offset = np.array([0.0, 0.0, 0.1]) t_list = [ - pin.SE3( + pin.SE3(np.eye(3), offset) + * pin.SE3( pin.Quaternion(pin.AngleAxis(2.0 * np.pi / n_symmetries * i, axis)), - offset, + np.array([0.0, 0.0, 0.0]), ) + * pin.SE3(np.eye(3), offset).inverse() for i in range(n_symmetries) ] msg = ObjectSymmetries( @@ -250,10 +254,12 @@ def test_mixed_np() -> None: offset = np.array([-0.1, 0.6, 0.1]) t_c_list = [ - pin.SE3( + pin.SE3(np.eye(3), offset) + * pin.SE3( pin.Quaternion(pin.AngleAxis(2.0 * np.pi / n_symmetries * i, axis)), - offset, + np.array([0.0, 0.0, 0.0]), ) + * pin.SE3(np.eye(3), offset).inverse() for i in range(n_symmetries) ] @@ -305,10 +311,12 @@ def test_mixed_ros() -> None: offset = np.array([-1.1, 0.6, 0.1]) t_c_list = [ - pin.SE3( + pin.SE3(np.eye(3), offset) + * pin.SE3( pin.Quaternion(pin.AngleAxis(2.0 * np.pi / n_symmetries * i, axis)), - offset, + np.array([0.0, 0.0, 0.0]), ) + * pin.SE3(np.eye(3), offset).inverse() for i in range(n_symmetries) ] From 055cfd626ce541cc46c6e3b732a1aafa7bc3e93e Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski Date: Thu, 8 Aug 2024 15:13:02 +0000 Subject: [PATCH 31/32] Use `np.linspace` to generate rotation steps | Add notebook with an example --- happypose_msgs/examples/meshcat_viz.ipynb | 136 ++++++++++++++++++ .../happypose_msgs_py/symmetries.py | 7 +- happypose_ros/.gitignore | 2 +- 3 files changed, 139 insertions(+), 6 deletions(-) create mode 100644 happypose_msgs/examples/meshcat_viz.ipynb 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_msgs/happypose_msgs_py/symmetries.py b/happypose_msgs/happypose_msgs_py/symmetries.py index 4f5920b..3268d52 100644 --- a/happypose_msgs/happypose_msgs_py/symmetries.py +++ b/happypose_msgs/happypose_msgs_py/symmetries.py @@ -41,7 +41,7 @@ def discretize_symmetries( out = np.zeros((n_con + n_disc + n_mix, 4, 4)) # Precompute steps of rotations - rot_base = 2.0 * np.pi / n_symmetries_continuous + 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): @@ -57,10 +57,7 @@ def discretize_symmetries( # Compute T @ R @ int(T) # Discrete rotations around axis, generating matrices R out[begin:end, :3, :3] = np.array( - [ - transforms3d.axangles.axangle2mat(axis, rot_base * j) - for j in range(n_symmetries_continuous) - ] + [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]) diff --git a/happypose_ros/.gitignore b/happypose_ros/.gitignore index ffc6220..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 From b3e48958e3ed7ff732d43a6ba209b269f63b378d Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski Date: Tue, 3 Sep 2024 13:30:38 +0000 Subject: [PATCH 32/32] Update documentation --- README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/README.md b/README.md index e1a456a..7f26d87 100644 --- a/README.md +++ b/README.md @@ -69,6 +69,8 @@ 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.