diff --git a/.gitignore b/.gitignore index ccb9b18b1..c4473eea4 100755 --- a/.gitignore +++ b/.gitignore @@ -143,3 +143,5 @@ mypy.ini # Pycharm extension setup files .idea/* +/common/helpers/ros2_numpy +/common/helpers/ament_virtualenv \ No newline at end of file diff --git a/common/helpers/tf_pcl/CMakeLists.txt b/common/helpers/tf_pcl/CMakeLists.txt deleted file mode 100644 index cf02c6748..000000000 --- a/common/helpers/tf_pcl/CMakeLists.txt +++ /dev/null @@ -1,24 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(tf_pcl) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(std_msgs REQUIRED) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -ament_package() diff --git a/common/helpers/tf_pcl/LICENSE b/common/helpers/tf_pcl/LICENSE deleted file mode 100644 index d64569567..000000000 --- a/common/helpers/tf_pcl/LICENSE +++ /dev/null @@ -1,202 +0,0 @@ - - Apache License - Version 2.0, January 2004 - http://www.apache.org/licenses/ - - TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION - - 1. Definitions. - - "License" shall mean the terms and conditions for use, reproduction, - and distribution as defined by Sections 1 through 9 of this document. - - "Licensor" shall mean the copyright owner or entity authorized by - the copyright owner that is granting the License. - - "Legal Entity" shall mean the union of the acting entity and all - other entities that control, are controlled by, or are under common - control with that entity. For the purposes of this definition, - "control" means (i) the power, direct or indirect, to cause the - direction or management of such entity, whether by contract or - otherwise, or (ii) ownership of fifty percent (50%) or more of the - outstanding shares, or (iii) beneficial ownership of such entity. - - "You" (or "Your") shall mean an individual or Legal Entity - exercising permissions granted by this License. - - "Source" form shall mean the preferred form for making modifications, - including but not limited to software source code, documentation - source, and configuration files. - - "Object" form shall mean any form resulting from mechanical - transformation or translation of a Source form, including but - not limited to compiled object code, generated documentation, - and conversions to other media types. - - "Work" shall mean the work of authorship, whether in Source or - Object form, made available under the License, as indicated by a - copyright notice that is included in or attached to the work - (an example is provided in the Appendix below). - - "Derivative Works" shall mean any work, whether in Source or Object - form, that is based on (or derived from) the Work and for which the - editorial revisions, annotations, elaborations, or other modifications - represent, as a whole, an original work of authorship. For the purposes - of this License, Derivative Works shall not include works that remain - separable from, or merely link (or bind by name) to the interfaces of, - the Work and Derivative Works thereof. - - "Contribution" shall mean any work of authorship, including - the original version of the Work and any modifications or additions - to that Work or Derivative Works thereof, that is intentionally - submitted to Licensor for inclusion in the Work by the copyright owner - or by an individual or Legal Entity authorized to submit on behalf of - the copyright owner. For the purposes of this definition, "submitted" - means any form of electronic, verbal, or written communication sent - to the Licensor or its representatives, including but not limited to - communication on electronic mailing lists, source code control systems, - and issue tracking systems that are managed by, or on behalf of, the - Licensor for the purpose of discussing and improving the Work, but - excluding communication that is conspicuously marked or otherwise - designated in writing by the copyright owner as "Not a Contribution." - - "Contributor" shall mean Licensor and any individual or Legal Entity - on behalf of whom a Contribution has been received by Licensor and - subsequently incorporated within the Work. - - 2. Grant of Copyright License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - copyright license to reproduce, prepare Derivative Works of, - publicly display, publicly perform, sublicense, and distribute the - Work and such Derivative Works in Source or Object form. - - 3. Grant of Patent License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - (except as stated in this section) patent license to make, have made, - use, offer to sell, sell, import, and otherwise transfer the Work, - where such license applies only to those patent claims licensable - by such Contributor that are necessarily infringed by their - Contribution(s) alone or by combination of their Contribution(s) - with the Work to which such Contribution(s) was submitted. If You - institute patent litigation against any entity (including a - cross-claim or counterclaim in a lawsuit) alleging that the Work - or a Contribution incorporated within the Work constitutes direct - or contributory patent infringement, then any patent licenses - granted to You under this License for that Work shall terminate - as of the date such litigation is filed. - - 4. Redistribution. You may reproduce and distribute copies of the - Work or Derivative Works thereof in any medium, with or without - modifications, and in Source or Object form, provided that You - meet the following conditions: - - (a) You must give any other recipients of the Work or - Derivative Works a copy of this License; and - - (b) You must cause any modified files to carry prominent notices - stating that You changed the files; and - - (c) You must retain, in the Source form of any Derivative Works - that You distribute, all copyright, patent, trademark, and - attribution notices from the Source form of the Work, - excluding those notices that do not pertain to any part of - the Derivative Works; and - - (d) If the Work includes a "NOTICE" text file as part of its - distribution, then any Derivative Works that You distribute must - include a readable copy of the attribution notices contained - within such NOTICE file, excluding those notices that do not - pertain to any part of the Derivative Works, in at least one - of the following places: within a NOTICE text file distributed - as part of the Derivative Works; within the Source form or - documentation, if provided along with the Derivative Works; or, - within a display generated by the Derivative Works, if and - wherever such third-party notices normally appear. The contents - of the NOTICE file are for informational purposes only and - do not modify the License. You may add Your own attribution - notices within Derivative Works that You distribute, alongside - or as an addendum to the NOTICE text from the Work, provided - that such additional attribution notices cannot be construed - as modifying the License. - - You may add Your own copyright statement to Your modifications and - may provide additional or different license terms and conditions - for use, reproduction, or distribution of Your modifications, or - for any such Derivative Works as a whole, provided Your use, - reproduction, and distribution of the Work otherwise complies with - the conditions stated in this License. - - 5. Submission of Contributions. Unless You explicitly state otherwise, - any Contribution intentionally submitted for inclusion in the Work - by You to the Licensor shall be under the terms and conditions of - this License, without any additional terms or conditions. - Notwithstanding the above, nothing herein shall supersede or modify - the terms of any separate license agreement you may have executed - with Licensor regarding such Contributions. - - 6. Trademarks. This License does not grant permission to use the trade - names, trademarks, service marks, or product names of the Licensor, - except as required for reasonable and customary use in describing the - origin of the Work and reproducing the content of the NOTICE file. - - 7. Disclaimer of Warranty. Unless required by applicable law or - agreed to in writing, Licensor provides the Work (and each - Contributor provides its Contributions) on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or - implied, including, without limitation, any warranties or conditions - of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A - PARTICULAR PURPOSE. You are solely responsible for determining the - appropriateness of using or redistributing the Work and assume any - risks associated with Your exercise of permissions under this License. - - 8. Limitation of Liability. In no event and under no legal theory, - whether in tort (including negligence), contract, or otherwise, - unless required by applicable law (such as deliberate and grossly - negligent acts) or agreed to in writing, shall any Contributor be - liable to You for damages, including any direct, indirect, special, - incidental, or consequential damages of any character arising as a - result of this License or out of the use or inability to use the - Work (including but not limited to damages for loss of goodwill, - work stoppage, computer failure or malfunction, or any and all - other commercial damages or losses), even if such Contributor - has been advised of the possibility of such damages. - - 9. Accepting Warranty or Additional Liability. While redistributing - the Work or Derivative Works thereof, You may choose to offer, - and charge a fee for, acceptance of support, warranty, indemnity, - or other liability obligations and/or rights consistent with this - License. However, in accepting such obligations, You may act only - on Your own behalf and on Your sole responsibility, not on behalf - of any other Contributor, and only if You agree to indemnify, - defend, and hold each Contributor harmless for any liability - incurred by, or claims asserted against, such Contributor by reason - of your accepting any such warranty or additional liability. - - END OF TERMS AND CONDITIONS - - APPENDIX: How to apply the Apache License to your work. - - To apply the Apache License to your work, attach the following - boilerplate notice, with the fields enclosed by brackets "[]" - replaced with your own identifying information. (Don't include - the brackets!) The text should be enclosed in the appropriate - comment syntax for the file format. We also recommend that a - file or class name and description of purpose be included on the - same "printed page" as the copyright notice for easier - identification within third-party archives. - - Copyright [yyyy] [name of copyright owner] - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. diff --git a/common/helpers/tf_pcl/package.xml b/common/helpers/tf_pcl/package.xml index dd6c00105..71ef4c6c7 100644 --- a/common/helpers/tf_pcl/package.xml +++ b/common/helpers/tf_pcl/package.xml @@ -1,20 +1,30 @@ - tf_pcl - 0.0.0 - TODO: Package description - jingwen - Apache-2.0 + tf_pcl + 0.0.0 + TODO: Package description + siyao + TODO: License declaration - ament_cmake + rclcpp + std_msgs + sensor_msgs + geometry_msgs + scipy + tf2_ros + numpy + ros2_numpy + ament_virtualenv - std_msgs - ament_lint_auto - ament_lint_common + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest - - ament_cmake - - + + ament_python + requirements.txt + + \ No newline at end of file diff --git a/common/helpers/tf_pcl/requirements.in b/common/helpers/tf_pcl/requirements.in deleted file mode 100644 index 5576e19fe..000000000 --- a/common/helpers/tf_pcl/requirements.in +++ /dev/null @@ -1,2 +0,0 @@ -numpy -scipy \ No newline at end of file diff --git a/common/helpers/tf_pcl/requirements.txt b/common/helpers/tf_pcl/requirements.txt new file mode 100644 index 000000000..40f9facfd --- /dev/null +++ b/common/helpers/tf_pcl/requirements.txt @@ -0,0 +1 @@ +numpy<1.24 \ No newline at end of file diff --git a/common/helpers/tf_pcl/resource/tf_pcl b/common/helpers/tf_pcl/resource/tf_pcl new file mode 100644 index 000000000..e69de29bb diff --git a/common/helpers/tf_pcl/setup.py b/common/helpers/tf_pcl/setup.py index 4040a117b..f1c0cddf7 100755 --- a/common/helpers/tf_pcl/setup.py +++ b/common/helpers/tf_pcl/setup.py @@ -1,23 +1,49 @@ -from setuptools import setup +from setuptools import find_packages, setup -package_name = "tf_pcl" +package_name = 'tf_pcl' +import setuptools.command.install +import ament_virtualenv.install + +class InstallCommand(setuptools.command.install.install): + def run(self): + super().run() + ament_virtualenv.install.install_venv( + install_base=self.install_base, + scripts_base=self.install_scripts, + package_name=package_name, + python_version='3' + ) + # instead of self.install_base we may also use: + # self.config_vars['platbase'] or self.config_vars['base'] + # Exchange the python_version with '3' if your package uses Python3. + return setup( name=package_name, - version="0.0.1", + version='0.0.0', packages=[package_name], package_dir={"": "src"}, + install_requires=['setuptools'], + zip_safe=True, data_files=[ - # Install package.xml in the appropriate location - ("share/ament_index/resource_index/packages", ["resource/" + package_name]), - ("share/" + package_name, ["package.xml"]), + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), ], - install_requires=["setuptools"], # Add any Python dependencies here - zip_safe=True, - maintainer="Your Name", - maintainer_email="your_email@example.com", - description="A ROS 2 package for point cloud transformations", - license="Apache License 2.0", # Update license as per your project - tests_require=["pytest"], # Add any test dependencies here - entry_points={"console_scripts": ["pointcloud_transformer = tf_pcl.__init__:main"]}, -) + maintainer='siyao', + maintainer_email='sveali41@gmail.com', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + extras_require={ + 'test': [ + 'pytest', + ], + }, + cmdclass={'install': InstallCommand + }, + entry_points={ + 'console_scripts': [ + ], + }, +) \ No newline at end of file diff --git a/common/helpers/tf_pcl/src/__init__.py b/common/helpers/tf_pcl/src/__init__.py deleted file mode 100755 index 9c1c9d840..000000000 --- a/common/helpers/tf_pcl/src/__init__.py +++ /dev/null @@ -1,73 +0,0 @@ -#!/usr/bin/env python3 -import rclpy -from rclpy.node import Node -from sensor_msgs.msg import PointCloud2 -from geometry_msgs.msg import TransformStamped -from sensor_msgs_py import point_cloud2 -from copy import deepcopy -import numpy as np -from scipy.spatial.transform import Rotation as R - - -class PointCloudTransformer(Node): - def __init__(self): - super().__init__("pointcloud_transformer") - self.subscription = self.create_subscription( - PointCloud2, "input_pointcloud", self.pcl_callback, 10 - ) - self.publisher = self.create_publisher(PointCloud2, "output_pointcloud", 10) - - def pcl_callback(self, pcl: PointCloud2): - # Example transform (identity transform for demonstration purposes) - transform = TransformStamped() - transform.transform.translation.x = 0.0 - transform.transform.translation.y = 0.0 - transform.transform.translation.z = 0.0 - transform.transform.rotation.x = 0.0 - transform.transform.rotation.y = 0.0 - transform.transform.rotation.z = 0.0 - transform.transform.rotation.w = 1.0 - - transformed_pcl = pcl_transform(pcl, transform) - self.publisher.publish(transformed_pcl) - - -def pcl_transform( - pcl: PointCloud2, transform: TransformStamped, target_frame: str = "map" -) -> PointCloud2: - """Transforms a point cloud using a given transform message.""" - pcl_arr = deepcopy( - list(point_cloud2.read_points(pcl, field_names=("x", "y", "z"), skip_nans=True)) - ) - - translation = transform.transform.translation - rotation_q = transform.transform.rotation - - rotation_matrix = R.from_quat( - [rotation_q.x, rotation_q.y, rotation_q.z, rotation_q.w] - ) - - pcl_x_y_z_arr = np.array(pcl_arr) - - # Apply rotation and translation - transformed_pcl = rotation_matrix.apply(pcl_x_y_z_arr) + np.array( - [translation.x, translation.y, translation.z] - ) - - # Create a new PointCloud2 message - transformed_pcl_msg = point_cloud2.create_cloud( - pcl.header, pcl.fields, transformed_pcl - ) - return transformed_pcl_msg - - -def main(args=None): - rclpy.init(args=args) - node = PointCloudTransformer() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/common/helpers/tf_pcl/src/tf_pcl/__init__.py b/common/helpers/tf_pcl/src/tf_pcl/__init__.py new file mode 100755 index 000000000..312a40942 --- /dev/null +++ b/common/helpers/tf_pcl/src/tf_pcl/__init__.py @@ -0,0 +1,51 @@ +#!/usr/bin/env python3 +from copy import deepcopy +import numpy as np +import ros2_numpy as rnp + +from scipy.spatial.transform import Rotation as R +from sensor_msgs.msg import PointCloud2 +from geometry_msgs.msg import TransformStamped + + +def pcl_transform( + pcl: PointCloud2, transform: TransformStamped, target_frame: str = "map" +) -> PointCloud2: + """Transforms a pointclound using a given transform message. + + Args: + pcl (PointCloud2): source pointcloud to transform. + transform (TransformStamped): transform to apply + target_frame (str): The new frame_id for the output header. + + Returns: + PointCloud2: transformed pointcloud + """ + + pcl_arr = deepcopy(rnp.point_cloud2.pointcloud2_to_array(pcl)) + + original_shape = pcl_arr.shape + + xyz = np.array([pcl_arr["x"], pcl_arr["y"], pcl_arr["z"]]) + + xyz_flat = xyz.reshape(3, -1) + + translation = transform.transform.translation + rotation_q = transform.transform.rotation + rotation_matrix = R.from_quat( + [rotation_q.x, rotation_q.y, rotation_q.z, rotation_q.w] + ) + translation_vec = np.array([translation.x, translation.y, translation.z]) + + transformed_xyz = rotation_matrix.apply(xyz_flat.T) + translation_vec + + transformed_xyz = transformed_xyz.T + + pcl_arr["x"] = transformed_xyz[0].reshape(original_shape) + pcl_arr["y"] = transformed_xyz[1].reshape(original_shape) + pcl_arr["z"] = transformed_xyz[2].reshape(original_shape) + + transformed_pcl = rnp.point_cloud2.array_to_pointcloud2( + pcl_arr, stamp=pcl.header.stamp, frame_id=target_frame + ) + return transformed_pcl \ No newline at end of file diff --git a/common/helpers/tf_pcl/tests/test_pcl_transform.py b/common/helpers/tf_pcl/tests/test_pcl_transform.py new file mode 100644 index 000000000..b64f68d69 --- /dev/null +++ b/common/helpers/tf_pcl/tests/test_pcl_transform.py @@ -0,0 +1,91 @@ +import pytest +import numpy as np +import ros2_numpy as rnp +from geometry_msgs.msg import TransformStamped, Quaternion, Vector3 +from sensor_msgs.msg import PointCloud2 +from std_msgs.msg import Header +from scipy.spatial.transform import Rotation as R + +from tf_pcl import pcl_transform +import warnings +warnings.filterwarnings("ignore", category=DeprecationWarning) + +def create_test_transform(x, y, z, roll, pitch, yaw, frame_id, child_frame_id): + """Helper to create a TransformStamped message.""" + t = TransformStamped() + t.header.stamp.sec = 0 + t.header.stamp.nanosec = 0 + t.header.frame_id = frame_id + t.child_frame_id = child_frame_id + + t.transform.translation = Vector3(x=float(x), y=float(y), z=float(z)) + + q = R.from_euler('xyz', [roll, pitch, yaw], degrees=False).as_quat() + t.transform.rotation = Quaternion(x=q[0], y=q[1], z=q[2], w=q[3]) + + return t + +def test_pcl_transform_logic(): + """ + Tests if a 2x2 ordered pointcloud is correctly transformed. + """ + data = np.zeros((2, 2), dtype=[ + ('x', np.float32), + ('y', np.float32), + ('z', np.float32) + ]) + + data[0, 0] = (0.0, 0.0, 0.0) + data[0, 1] = (1.0, 0.0, 0.0) + data[1, 0] = (0.0, 1.0, 0.0) + data[1, 1] = (0.0, 0.0, 1.0) + + input_pcl = rnp.msgify(PointCloud2, data) + input_pcl.header.frame_id = "camera_link" + + transform_msg = create_test_transform( + x=1.0, y=2.0, z=3.0, + roll=0.0, pitch=0.0, yaw=np.pi/2, + frame_id="map", child_frame_id="camera_link" + ) + + output_pcl = pcl_transform(input_pcl, transform_msg, "map") + + assert output_pcl.header.frame_id == "map" + assert output_pcl.height == 2 + assert output_pcl.width == 2 + + output_arr = rnp.numpify(output_pcl) + + np.testing.assert_allclose( + [output_arr['x'][0,0], output_arr['y'][0,0], output_arr['z'][0,0]], + [1.0, 2.0, 3.0], atol=1e-5 + ) + np.testing.assert_allclose( + [output_arr['x'][0,1], output_arr['y'][0,1], output_arr['z'][0,1]], + [1.0, 3.0, 3.0], atol=1e-5 + ) + +def test_pcl_preserve_extra_fields(): + """ + Ensures extra fields like intensity are preserved. + """ + data = np.zeros((2, 2), dtype=[ + ('x', np.float32), + ('y', np.float32), + ('z', np.float32), + ('intensity', np.float32) + ]) + + data[0,0] = (5.0, 5.0, 5.0, 0.5) + + input_pcl = rnp.msgify(PointCloud2, data) + input_pcl.header.frame_id = "base" + + transform_msg = create_test_transform(0,0,0, 0,0,0, "map", "base") + + output_pcl = pcl_transform(input_pcl, transform_msg, "map") + output_arr = rnp.numpify(output_pcl) + + assert 'intensity' in output_arr.dtype.names + assert output_arr['intensity'][0,0] == 0.5 \ No newline at end of file