diff --git a/common/autoware_universe_utils/NOTICE b/common/autoware_universe_utils/NOTICE new file mode 100644 index 0000000000000..845c72f5edb36 --- /dev/null +++ b/common/autoware_universe_utils/NOTICE @@ -0,0 +1,2 @@ +The function 'opencv_fast_atan2' in /autoware/src/universe/autoware.universe/common/autoware_universe_utils/src/math/trigonometry.cpp +is a modified version of 'fastAtan2' in the OpenCV project. (https://github.com/opencv/opencv/blob/4.x/modules/core/src/mathfuncs_core.simd.hpp) diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/math/trigonometry.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/math/trigonometry.hpp index 19a59523c7f08..184538f158c3c 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/math/trigonometry.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/math/trigonometry.hpp @@ -26,6 +26,8 @@ float cos(float radian); std::pair sin_and_cos(float radian); +float opencv_fast_atan2(float dy, float dx); + } // namespace autoware::universe_utils #endif // AUTOWARE__UNIVERSE_UTILS__MATH__TRIGONOMETRY_HPP_ diff --git a/common/autoware_universe_utils/src/math/trigonometry.cpp b/common/autoware_universe_utils/src/math/trigonometry.cpp index 586b9075ba6d5..2966f35bfe59e 100644 --- a/common/autoware_universe_utils/src/math/trigonometry.cpp +++ b/common/autoware_universe_utils/src/math/trigonometry.cpp @@ -72,4 +72,57 @@ std::pair sin_and_cos(float radian) } } +// This code is modified from a part of the OpenCV project +// (https://github.com/opencv/opencv/blob/4.x/modules/core/src/mathfuncs_core.simd.hpp). It is +// subject to the license terms in the LICENSE file found in the top-level directory of this +// distribution and at http://opencv.org/license.html. +// The license can be found in +// common/autoware_universe_utils/third_party_licenses/opencv-license.md +// and https://github.com/opencv/opencv/blob/master/LICENSE + +// Modification: +// 1. use autoware defined PI +// 2. output of the function is changed from degrees to radians. +namespace detail_fast_atan2 +{ +static const float atan2_p1 = + 0.9997878412794807f * static_cast(180) / autoware::universe_utils::pi; +static const float atan2_p3 = + -0.3258083974640975f * static_cast(180) / autoware::universe_utils::pi; +static const float atan2_p5 = + 0.1555786518463281f * static_cast(180) / autoware::universe_utils::pi; +static const float atan2_p7 = + -0.04432655554792128f * static_cast(180) / autoware::universe_utils::pi; +static const float atan2_DBL_EPSILON = 2.2204460492503131e-016f; +} // namespace detail_fast_atan2 + +float opencv_fast_atan2(float dy, float dx) +{ + float ax = std::abs(dx); + float ay = std::abs(dy); + float a, c, c2; + if (ax >= ay) { + c = ay / (ax + detail_fast_atan2::atan2_DBL_EPSILON); + c2 = c * c; + a = (((detail_fast_atan2::atan2_p7 * c2 + detail_fast_atan2::atan2_p5) * c2 + + detail_fast_atan2::atan2_p3) * + c2 + + detail_fast_atan2::atan2_p1) * + c; + } else { + c = ax / (ay + detail_fast_atan2::atan2_DBL_EPSILON); + c2 = c * c; + a = 90.f - (((detail_fast_atan2::atan2_p7 * c2 + detail_fast_atan2::atan2_p5) * c2 + + detail_fast_atan2::atan2_p3) * + c2 + + detail_fast_atan2::atan2_p1) * + c; + } + if (dx < 0) a = 180.f - a; + if (dy < 0) a = 360.f - a; + + a = a * autoware::universe_utils::pi / 180.f; + return a; +} + } // namespace autoware::universe_utils diff --git a/common/autoware_universe_utils/test/src/math/test_trigonometry.cpp b/common/autoware_universe_utils/test/src/math/test_trigonometry.cpp index b55b27a34a6ac..df05b698693d6 100644 --- a/common/autoware_universe_utils/test/src/math/test_trigonometry.cpp +++ b/common/autoware_universe_utils/test/src/math/test_trigonometry.cpp @@ -50,3 +50,28 @@ TEST(trigonometry, sin_and_cos) EXPECT_TRUE(std::abs(std::cos(x * static_cast(i)) - sin_and_cos.second) < 10e-7); } } + +float normalize_angle(double angle) +{ + const double tau = 2 * autoware::universe_utils::pi; + double factor = std::floor(angle / tau); + return static_cast(angle - (factor * tau)); +} + +TEST(trigonometry, opencv_fast_atan2) +{ + for (int i = 0; i < 100; ++i) { + // Generate random x and y between -10 and 10 + std::srand(0); + float x = static_cast(std::rand()) / RAND_MAX * 20.0 - 10.0; + float y = static_cast(std::rand()) / RAND_MAX * 20.0 - 10.0; + + float fast_atan = autoware::universe_utils::opencv_fast_atan2(y, x); + float std_atan = normalize_angle(std::atan2(y, x)); + + // 0.3 degree accuracy + ASSERT_NEAR(fast_atan, std_atan, 6e-3) + << "Test failed for input (" << y << ", " << x << "): " + << "fast atan2 = " << fast_atan << ", std::atan2 = " << std_atan; + } +} diff --git a/common/autoware_universe_utils/third_party_licenses/opencv-license.md b/common/autoware_universe_utils/third_party_licenses/opencv-license.md new file mode 100644 index 0000000000000..c319da33b7428 --- /dev/null +++ b/common/autoware_universe_utils/third_party_licenses/opencv-license.md @@ -0,0 +1,201 @@ + 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/sensing/autoware_pointcloud_preprocessor/config/distortion_corrector_node.param.yaml b/sensing/autoware_pointcloud_preprocessor/config/distortion_corrector_node.param.yaml index eca08c37b6458..90ded7ffc9c97 100644 --- a/sensing/autoware_pointcloud_preprocessor/config/distortion_corrector_node.param.yaml +++ b/sensing/autoware_pointcloud_preprocessor/config/distortion_corrector_node.param.yaml @@ -3,4 +3,5 @@ base_frame: base_link use_imu: true use_3d_distortion_correction: false + update_azimuth_and_distance: false has_static_tf_only: true diff --git a/sensing/autoware_pointcloud_preprocessor/docs/distortion-corrector.md b/sensing/autoware_pointcloud_preprocessor/docs/distortion-corrector.md index ab5a07b5279bc..75cdccc4453ba 100644 --- a/sensing/autoware_pointcloud_preprocessor/docs/distortion-corrector.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/distortion-corrector.md @@ -48,3 +48,24 @@ ros2 launch autoware_pointcloud_preprocessor distortion_corrector.launch.xml - The node requires time synchronization between the topics from lidars, twist, and IMU. - If you want to use a 3D distortion corrector without IMU, please check that the linear and angular velocity fields of your twist message are not empty. +- The node updates the per-point azimuth and distance values based on the undistorted XYZ coordinates when the input point cloud is in the sensor frame (not in the `base_link`) and the `update_azimuth_and_distance` parameter is set to `true`. The azimuth values are calculated using a modified version of OpenCV's `cv::fastAtan2` function. +- Please note that updating the azimuth and distance fields increases the execution time by approximately 20%. Additionally, due to the `cv::fastAtan2` algorithm's has a maximum error of 0.3 degrees, there is a **possibility of changing the beam order for high azimuth resolution LiDAR**. +- LiDARs from different vendors have different azimuth coordinates, as shown in the images below. Currently, the coordinate systems listed below have been tested, and the node will update the azimuth based on the input coordinate system. + - `velodyne`: (x: 0 degrees, y: 270 degrees) + - `hesai`: (x: 90 degrees, y: 0 degrees) + - `others`: (x: 0 degrees, y: 90 degrees) and (x: 270 degrees, y: 0 degrees) + + + + + + + + + + +
velodyne azimuth coordinatehesai azimuth coordinate

Velodyne azimuth coordinate

Hesai azimuth coordinate

+ +## References/External links + + diff --git a/sensing/autoware_pointcloud_preprocessor/docs/image/hesai.drawio.png b/sensing/autoware_pointcloud_preprocessor/docs/image/hesai.drawio.png new file mode 100644 index 0000000000000..d406eb3896850 Binary files /dev/null and b/sensing/autoware_pointcloud_preprocessor/docs/image/hesai.drawio.png differ diff --git a/sensing/autoware_pointcloud_preprocessor/docs/image/velodyne.drawio.png b/sensing/autoware_pointcloud_preprocessor/docs/image/velodyne.drawio.png new file mode 100644 index 0000000000000..b3b8eda6140a4 Binary files /dev/null and b/sensing/autoware_pointcloud_preprocessor/docs/image/velodyne.drawio.png differ diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp index e786bff04b3cd..d4432c24dd5b0 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp @@ -47,74 +47,95 @@ namespace autoware::pointcloud_preprocessor { -class DistortionCorrectorBase +struct AngleConversion { -public: - virtual bool pointcloud_transform_exists() = 0; - virtual bool pointcloud_transform_needed() = 0; - virtual std::deque get_twist_queue() = 0; - virtual std::deque get_angular_velocity_queue() = 0; - - virtual void processTwistMessage( - const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) = 0; - virtual void processIMUMessage( - const std::string & base_frame, const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) = 0; - virtual void setPointCloudTransform( - const std::string & base_frame, const std::string & lidar_frame) = 0; - virtual void initialize() = 0; - virtual void undistortPointCloud(bool use_imu, sensor_msgs::msg::PointCloud2 & pointcloud) = 0; + // Equation for the conversion between sensor azimuth coordinates and Cartesian coordinates: + // sensor azimuth coordinates = offset_rad + sign * cartesian coordinates; + // offset_rad is restricted to be a multiple of 90, and sign is restricted to be 1 or -1. + float offset_rad{0}; + float sign{1}; + static constexpr float offset_rad_threshold{(5.0f / 180.0f) * M_PI}; + + static constexpr float sign_threshold{0.1f}; }; -template -class DistortionCorrector : public DistortionCorrectorBase +class DistortionCorrectorBase { protected: geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr_; bool pointcloud_transform_needed_{false}; bool pointcloud_transform_exists_{false}; bool imu_transform_exists_{false}; - rclcpp::Node * node_; std::unique_ptr managed_tf_buffer_{nullptr}; std::deque twist_queue_; std::deque angular_velocity_queue_; - void getIMUTransformation(const std::string & base_frame, const std::string & imu_frame); - void enqueueIMU(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg); - void getTwistAndIMUIterator( + rclcpp::Node & node_; + + void get_imu_transformation(const std::string & base_frame, const std::string & imu_frame); + void enqueue_imu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg); + void get_twist_and_imu_iterator( bool use_imu, double first_point_time_stamp_sec, std::deque::iterator & it_twist, std::deque::iterator & it_imu); - void warnIfTimestampIsTooLate(bool is_twist_time_stamp_too_late, bool is_imu_time_stamp_too_late); - void undistortPoint( - sensor_msgs::PointCloud2Iterator & it_x, sensor_msgs::PointCloud2Iterator & it_y, - sensor_msgs::PointCloud2Iterator & it_z, - std::deque::iterator & it_twist, - std::deque::iterator & it_imu, float const & time_offset, - const bool & is_twist_valid, const bool & is_imu_valid) - { - static_cast(this)->undistortPointImplementation( - it_x, it_y, it_z, it_twist, it_imu, time_offset, is_twist_valid, is_imu_valid); - }; - void convertMatrixToTransform(const Eigen::Matrix4f & matrix, tf2::Transform & transform); + void warn_if_timestamp_is_too_late( + bool is_twist_time_stamp_too_late, bool is_imu_time_stamp_too_late); + static tf2::Transform convert_matrix_to_transform(const Eigen::Matrix4f & matrix); public: - explicit DistortionCorrector(rclcpp::Node * node, const bool & has_static_tf_only) : node_(node) + explicit DistortionCorrectorBase(rclcpp::Node & node, const bool & has_static_tf_only) + : node_(node) { managed_tf_buffer_ = - std::make_unique(node, has_static_tf_only); + std::make_unique(&node, has_static_tf_only); } - bool pointcloud_transform_exists(); - bool pointcloud_transform_needed(); + [[nodiscard]] bool pointcloud_transform_exists() const; + [[nodiscard]] bool pointcloud_transform_needed() const; std::deque get_twist_queue(); std::deque get_angular_velocity_queue(); - void processTwistMessage( - const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) override; + void process_twist_message( + const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg); + + void process_imu_message( + const std::string & base_frame, const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg); + + std::optional try_compute_angle_conversion( + sensor_msgs::msg::PointCloud2 & pointcloud); + + bool is_pointcloud_valid(sensor_msgs::msg::PointCloud2 & pointcloud); + + virtual void set_pointcloud_transform( + const std::string & base_frame, const std::string & lidar_frame) = 0; + virtual void initialize() = 0; + virtual void undistort_pointcloud( + bool use_imu, std::optional angle_conversion_opt, + sensor_msgs::msg::PointCloud2 & pointcloud) = 0; +}; - void processIMUMessage( - const std::string & base_frame, const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) override; - void undistortPointCloud(bool use_imu, sensor_msgs::msg::PointCloud2 & pointcloud) override; - bool isInputValid(sensor_msgs::msg::PointCloud2 & pointcloud); +template +class DistortionCorrector : public DistortionCorrectorBase +{ +public: + explicit DistortionCorrector(rclcpp::Node & node, const bool & has_static_tf_only) + : DistortionCorrectorBase(node, has_static_tf_only) + { + } + + void undistort_pointcloud( + bool use_imu, std::optional angle_conversion_opt, + sensor_msgs::msg::PointCloud2 & pointcloud) override; + + void undistort_point( + sensor_msgs::PointCloud2Iterator & it_x, sensor_msgs::PointCloud2Iterator & it_y, + sensor_msgs::PointCloud2Iterator & it_z, + std::deque::iterator & it_twist, + std::deque::iterator & it_imu, float const & time_offset, + const bool & is_twist_valid, const bool & is_imu_valid) + { + static_cast(this)->undistort_point_implementation( + it_x, it_y, it_z, it_twist, it_imu, time_offset, is_twist_valid, is_imu_valid); + }; }; class DistortionCorrector2D : public DistortionCorrector @@ -134,20 +155,19 @@ class DistortionCorrector2D : public DistortionCorrector tf2::Transform tf2_base_link_to_lidar_; public: - explicit DistortionCorrector2D(rclcpp::Node * node, const bool & has_static_tf_only) + explicit DistortionCorrector2D(rclcpp::Node & node, const bool & has_static_tf_only) : DistortionCorrector(node, has_static_tf_only) { } void initialize() override; - void undistortPointImplementation( + void set_pointcloud_transform( + const std::string & base_frame, const std::string & lidar_frame) override; + void undistort_point_implementation( sensor_msgs::PointCloud2Iterator & it_x, sensor_msgs::PointCloud2Iterator & it_y, sensor_msgs::PointCloud2Iterator & it_z, std::deque::iterator & it_twist, std::deque::iterator & it_imu, const float & time_offset, const bool & is_twist_valid, const bool & is_imu_valid); - - void setPointCloudTransform( - const std::string & base_frame, const std::string & lidar_frame) override; }; class DistortionCorrector3D : public DistortionCorrector @@ -164,19 +184,19 @@ class DistortionCorrector3D : public DistortionCorrector Eigen::Matrix4f eigen_base_link_to_lidar_; public: - explicit DistortionCorrector3D(rclcpp::Node * node, const bool & has_static_tf_only) + explicit DistortionCorrector3D(rclcpp::Node & node, const bool & has_static_tf_only) : DistortionCorrector(node, has_static_tf_only) { } void initialize() override; - void undistortPointImplementation( + void set_pointcloud_transform( + const std::string & base_frame, const std::string & lidar_frame) override; + void undistort_point_implementation( sensor_msgs::PointCloud2Iterator & it_x, sensor_msgs::PointCloud2Iterator & it_y, sensor_msgs::PointCloud2Iterator & it_z, std::deque::iterator & it_twist, std::deque::iterator & it_imu, const float & time_offset, const bool & is_twist_valid, const bool & is_imu_valid); - void setPointCloudTransform( - const std::string & base_frame, const std::string & lidar_frame) override; }; } // namespace autoware::pointcloud_preprocessor diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector_node.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector_node.hpp index 0d8c9436bda4b..b96774c37f621 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector_node.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector_node.hpp @@ -52,12 +52,16 @@ class DistortionCorrectorComponent : public rclcpp::Node std::string base_frame_; bool use_imu_; bool use_3d_distortion_correction_; + bool update_azimuth_and_distance_; + + std::optional angle_conversion_opt_; std::unique_ptr distortion_corrector_; - void onPointCloud(PointCloud2::UniquePtr points_msg); - void onTwist(const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg); - void onImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg); + void pointcloud_callback(PointCloud2::UniquePtr pointcloud_msg); + void twist_callback( + const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg); + void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg); }; } // namespace autoware::pointcloud_preprocessor diff --git a/sensing/autoware_pointcloud_preprocessor/schema/distortion_corrector_node.schema.json b/sensing/autoware_pointcloud_preprocessor/schema/distortion_corrector_node.schema.json index 75579227981ac..091695716c610 100644 --- a/sensing/autoware_pointcloud_preprocessor/schema/distortion_corrector_node.schema.json +++ b/sensing/autoware_pointcloud_preprocessor/schema/distortion_corrector_node.schema.json @@ -21,13 +21,24 @@ "description": "Use 3d distortion correction algorithm, otherwise, use 2d distortion correction algorithm.", "default": "false" }, + "update_azimuth_and_distance": { + "type": "boolean", + "description": "Flag to update the azimuth and distance values of each point after undistortion. If set to false, the azimuth and distance values will remain unchanged after undistortion, resulting in a mismatch with the updated x, y, z coordinates.", + "default": "false" + }, "has_static_tf_only": { "type": "boolean", "description": "Flag to indicate if only static TF is used.", "default": false } }, - "required": ["base_frame", "use_imu", "use_3d_distortion_correction", "has_static_tf_only"] + "required": [ + "base_frame", + "use_imu", + "use_3d_distortion_correction", + "update_azimuth_and_distance", + "has_static_tf_only" + ] } }, "properties": { diff --git a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index d0119fbc44f24..43a44f836b61a 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -15,91 +15,86 @@ #include "autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp" #include "autoware/pointcloud_preprocessor/utility/memory.hpp" +#include "autoware/universe_utils/math/constants.hpp" #include #include -#include - namespace autoware::pointcloud_preprocessor { -template -bool DistortionCorrector::pointcloud_transform_exists() +bool DistortionCorrectorBase::pointcloud_transform_exists() const { return pointcloud_transform_exists_; } -template -bool DistortionCorrector::pointcloud_transform_needed() +bool DistortionCorrectorBase::pointcloud_transform_needed() const { return pointcloud_transform_needed_; } -template -std::deque DistortionCorrector::get_twist_queue() +std::deque DistortionCorrectorBase::get_twist_queue() { return twist_queue_; } -template -std::deque DistortionCorrector::get_angular_velocity_queue() +std::deque DistortionCorrectorBase::get_angular_velocity_queue() { return angular_velocity_queue_; } -template -void DistortionCorrector::processTwistMessage( +void DistortionCorrectorBase::process_twist_message( const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) { geometry_msgs::msg::TwistStamped msg; msg.header = twist_msg->header; msg.twist = twist_msg->twist.twist; - twist_queue_.push_back(msg); + + // If time jumps backwards (e.g. when a rosbag restarts), clear buffer + if (!twist_queue_.empty()) { + if (rclcpp::Time(twist_queue_.front().header.stamp) > rclcpp::Time(msg.header.stamp)) { + twist_queue_.clear(); + } + } + + // Twist data in the queue that is older than the current twist by 1 second will be cleared. + auto cutoff_time = rclcpp::Time(msg.header.stamp) - rclcpp::Duration::from_seconds(1.0); while (!twist_queue_.empty()) { - // for replay rosbag - if (rclcpp::Time(twist_queue_.front().header.stamp) > rclcpp::Time(twist_msg->header.stamp)) { - twist_queue_.pop_front(); - } else if ( // NOLINT - rclcpp::Time(twist_queue_.front().header.stamp) < - rclcpp::Time(twist_msg->header.stamp) - rclcpp::Duration::from_seconds(1.0)) { - twist_queue_.pop_front(); - } else { + if (rclcpp::Time(twist_queue_.front().header.stamp) > cutoff_time) { break; } + twist_queue_.pop_front(); } + + twist_queue_.push_back(msg); } -template -void DistortionCorrector::processIMUMessage( +void DistortionCorrectorBase::process_imu_message( const std::string & base_frame, const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) { - getIMUTransformation(base_frame, imu_msg->header.frame_id); - enqueueIMU(imu_msg); + get_imu_transformation(base_frame, imu_msg->header.frame_id); + enqueue_imu(imu_msg); } -template -void DistortionCorrector::getIMUTransformation( +void DistortionCorrectorBase::get_imu_transformation( const std::string & base_frame, const std::string & imu_frame) { if (imu_transform_exists_) { return; } - tf2::Transform tf2_imu_to_base_link; Eigen::Matrix4f eigen_imu_to_base_link; imu_transform_exists_ = managed_tf_buffer_->getTransform(base_frame, imu_frame, eigen_imu_to_base_link); - convertMatrixToTransform(eigen_imu_to_base_link, tf2_imu_to_base_link); + tf2::Transform tf2_imu_to_base_link = convert_matrix_to_transform(eigen_imu_to_base_link); geometry_imu_to_base_link_ptr_ = std::make_shared(); geometry_imu_to_base_link_ptr_->transform.rotation = tf2::toMsg(tf2_imu_to_base_link.getRotation()); } -template -void DistortionCorrector::enqueueIMU(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) +void DistortionCorrectorBase::enqueue_imu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) { geometry_msgs::msg::Vector3Stamped angular_velocity; angular_velocity.vector = imu_msg->angular_velocity; @@ -107,26 +102,30 @@ void DistortionCorrector::enqueueIMU(const sensor_msgs::msg::Imu::ConstShared geometry_msgs::msg::Vector3Stamped transformed_angular_velocity; tf2::doTransform(angular_velocity, transformed_angular_velocity, *geometry_imu_to_base_link_ptr_); transformed_angular_velocity.header = imu_msg->header; - angular_velocity_queue_.push_back(transformed_angular_velocity); - while (!angular_velocity_queue_.empty()) { - // for replay rosbag + // If time jumps backwards (e.g. when a rosbag restarts), clear buffer + if (!angular_velocity_queue_.empty()) { if ( rclcpp::Time(angular_velocity_queue_.front().header.stamp) > rclcpp::Time(imu_msg->header.stamp)) { - angular_velocity_queue_.pop_front(); - } else if ( // NOLINT - rclcpp::Time(angular_velocity_queue_.front().header.stamp) < - rclcpp::Time(imu_msg->header.stamp) - rclcpp::Duration::from_seconds(1.0)) { - angular_velocity_queue_.pop_front(); - } else { + angular_velocity_queue_.clear(); + } + } + + // IMU data in the queue that is older than the current imu msg by 1 second will be cleared. + auto cutoff_time = rclcpp::Time(imu_msg->header.stamp) - rclcpp::Duration::from_seconds(1.0); + + while (!angular_velocity_queue_.empty()) { + if (rclcpp::Time(angular_velocity_queue_.front().header.stamp) > cutoff_time) { break; } + angular_velocity_queue_.pop_front(); } + + angular_velocity_queue_.push_back(transformed_angular_velocity); } -template -void DistortionCorrector::getTwistAndIMUIterator( +void DistortionCorrectorBase::get_twist_and_imu_iterator( bool use_imu, double first_point_time_stamp_sec, std::deque::iterator & it_twist, std::deque::iterator & it_imu) @@ -149,13 +148,11 @@ void DistortionCorrector::getTwistAndIMUIterator( } } -template -bool DistortionCorrector::isInputValid(sensor_msgs::msg::PointCloud2 & pointcloud) +bool DistortionCorrectorBase::is_pointcloud_valid(sensor_msgs::msg::PointCloud2 & pointcloud) { - if (pointcloud.data.empty() || twist_queue_.empty()) { + if (pointcloud.data.empty()) { RCLCPP_WARN_STREAM_THROTTLE( - node_->get_logger(), *node_->get_clock(), 10000 /* ms */, - "input pointcloud or twist_queue_ is empty."); + node_.get_logger(), *node_.get_clock(), 10000 /* ms */, "Input pointcloud is empty."); return false; } @@ -164,19 +161,18 @@ bool DistortionCorrector::isInputValid(sensor_msgs::msg::PointCloud2 & pointc [](const sensor_msgs::msg::PointField & field) { return field.name == "time_stamp"; }); if (time_stamp_field_it == pointcloud.fields.cend()) { RCLCPP_WARN_STREAM_THROTTLE( - node_->get_logger(), *node_->get_clock(), 10000 /* ms */, + node_.get_logger(), *node_.get_clock(), 10000 /* ms */, "Required field time stamp doesn't exist in the point cloud."); return false; } if (!utils::is_data_layout_compatible_with_point_xyzircaedt(pointcloud)) { RCLCPP_ERROR( - node_->get_logger(), - "The pointcloud layout is not compatible with PointXYZIRCAEDT. Aborting"); + node_.get_logger(), "The pointcloud layout is not compatible with PointXYZIRCAEDT. Aborting"); if (utils::is_data_layout_compatible_with_point_xyziradrt(pointcloud)) { RCLCPP_ERROR( - node_->get_logger(), + node_.get_logger(), "The pointcloud layout is compatible with PointXYZIRADRT. You may be using legacy " "code/data"); } @@ -187,15 +183,141 @@ bool DistortionCorrector::isInputValid(sensor_msgs::msg::PointCloud2 & pointc return true; } +std::optional DistortionCorrectorBase::try_compute_angle_conversion( + sensor_msgs::msg::PointCloud2 & pointcloud) +{ + // This function tries to compute the angle conversion from Cartesian coordinates to LiDAR azimuth + // coordinates system + + if (!is_pointcloud_valid(pointcloud)) return std::nullopt; + + AngleConversion angle_conversion; + + sensor_msgs::PointCloud2Iterator it_x(pointcloud, "x"); + sensor_msgs::PointCloud2Iterator it_y(pointcloud, "y"); + sensor_msgs::PointCloud2Iterator it_azimuth(pointcloud, "azimuth"); + + auto next_it_x = it_x; + auto next_it_y = it_y; + auto next_it_azimuth = it_azimuth; + + if (it_x != it_x.end() && it_x + 1 != it_x.end()) { + next_it_x = it_x + 1; + next_it_y = it_y + 1; + next_it_azimuth = it_azimuth + 1; + } else { + RCLCPP_WARN_STREAM_THROTTLE( + node_.get_logger(), *node_.get_clock(), 10000 /* ms */, + "Current point cloud only has a single point. Could not calculate the angle conversion."); + return std::nullopt; + } + + for (; next_it_x != it_x.end(); + ++it_x, ++it_y, ++it_azimuth, ++next_it_x, ++next_it_y, ++next_it_azimuth) { + auto current_cartesian_rad = autoware::universe_utils::opencv_fast_atan2(*it_y, *it_x); + auto next_cartesian_rad = autoware::universe_utils::opencv_fast_atan2(*next_it_y, *next_it_x); + + // If the angle exceeds 180 degrees, it may cross the 0-degree axis, + // which could disrupt the calculation of the formula. + if ( + std::abs(*next_it_azimuth - *it_azimuth) == 0 || + std::abs(next_cartesian_rad - current_cartesian_rad) == 0) { + RCLCPP_DEBUG_STREAM_THROTTLE( + node_.get_logger(), *node_.get_clock(), 10000 /* ms */, + "Angle between two points is 0 degrees. Iterate to next point ..."); + continue; + } + + // restrict the angle difference between [-180, 180] (degrees) + float azimuth_diff = + std::abs(*next_it_azimuth - *it_azimuth) > autoware::universe_utils::pi + ? std::abs(*next_it_azimuth - *it_azimuth) - 2 * autoware::universe_utils::pi + : *next_it_azimuth - *it_azimuth; + float cartesian_rad_diff = + std::abs(next_cartesian_rad - current_cartesian_rad) > autoware::universe_utils::pi + ? std::abs(next_cartesian_rad - current_cartesian_rad) - 2 * autoware::universe_utils::pi + : next_cartesian_rad - current_cartesian_rad; + + float sign = azimuth_diff / cartesian_rad_diff; + + // Check if 'sign' can be adjusted to 1 or -1 + if (std::abs(sign - 1.0f) <= angle_conversion.sign_threshold) { + angle_conversion.sign = 1.0f; + } else if (std::abs(sign + 1.0f) <= angle_conversion.sign_threshold) { + angle_conversion.sign = -1.0f; + } else { + RCLCPP_DEBUG_STREAM_THROTTLE( + node_.get_logger(), *node_.get_clock(), 10000 /* ms */, + "Value of sign is not close to 1 or -1. Iterate to next point ..."); + continue; + } + + float offset_rad = *it_azimuth - sign * current_cartesian_rad; + // Check if 'offset_rad' can be adjusted to offset_rad multiple of π/2 + int multiple_of_90_degrees = std::round(offset_rad / (autoware::universe_utils::pi / 2)); + if ( + std::abs(offset_rad - multiple_of_90_degrees * (autoware::universe_utils::pi / 2)) > + angle_conversion.offset_rad_threshold) { + RCLCPP_DEBUG_STREAM_THROTTLE( + node_.get_logger(), *node_.get_clock(), 10000 /* ms */, + "Value of offset_rad is not close to multiplication of 90 degrees. Iterate to next point " + "..."); + continue; + } + + // Limit the range of offset_rad in [0, 360) + multiple_of_90_degrees = (multiple_of_90_degrees % 4 + 4) % 4; + + angle_conversion.offset_rad = multiple_of_90_degrees * (autoware::universe_utils::pi / 2); + + return angle_conversion; + } + return std::nullopt; +} + +void DistortionCorrectorBase::warn_if_timestamp_is_too_late( + bool is_twist_time_stamp_too_late, bool is_imu_time_stamp_too_late) +{ + if (is_twist_time_stamp_too_late) { + RCLCPP_WARN_STREAM_THROTTLE( + node_.get_logger(), *node_.get_clock(), 10000 /* ms */, + "Twist time_stamp is too late. Could not interpolate."); + } + + if (is_imu_time_stamp_too_late) { + RCLCPP_WARN_STREAM_THROTTLE( + node_.get_logger(), *node_.get_clock(), 10000 /* ms */, + "IMU time_stamp is too late. Could not interpolate."); + } +} + +tf2::Transform DistortionCorrectorBase::convert_matrix_to_transform(const Eigen::Matrix4f & matrix) +{ + tf2::Transform transform; + transform.setOrigin(tf2::Vector3(matrix(0, 3), matrix(1, 3), matrix(2, 3))); + transform.setBasis(tf2::Matrix3x3( + matrix(0, 0), matrix(0, 1), matrix(0, 2), matrix(1, 0), matrix(1, 1), matrix(1, 2), + matrix(2, 0), matrix(2, 1), matrix(2, 2))); + return transform; +} + template -void DistortionCorrector::undistortPointCloud( - bool use_imu, sensor_msgs::msg::PointCloud2 & pointcloud) +void DistortionCorrector::undistort_pointcloud( + bool use_imu, std::optional angle_conversion_opt, + sensor_msgs::msg::PointCloud2 & pointcloud) { - if (!isInputValid(pointcloud)) return; + if (!is_pointcloud_valid(pointcloud)) return; + if (twist_queue_.empty()) { + RCLCPP_WARN_STREAM_THROTTLE( + node_.get_logger(), *node_.get_clock(), 10000 /* ms */, "Twist queue is empty."); + return; + } sensor_msgs::PointCloud2Iterator it_x(pointcloud, "x"); sensor_msgs::PointCloud2Iterator it_y(pointcloud, "y"); sensor_msgs::PointCloud2Iterator it_z(pointcloud, "z"); + sensor_msgs::PointCloud2Iterator it_azimuth(pointcloud, "azimuth"); + sensor_msgs::PointCloud2Iterator it_distance(pointcloud, "distance"); sensor_msgs::PointCloud2ConstIterator it_time_stamp(pointcloud, "time_stamp"); double prev_time_stamp_sec{ @@ -205,7 +327,7 @@ void DistortionCorrector::undistortPointCloud( std::deque::iterator it_twist; std::deque::iterator it_imu; - getTwistAndIMUIterator(use_imu, first_point_time_stamp_sec, it_twist, it_imu); + get_twist_and_imu_iterator(use_imu, first_point_time_stamp_sec, it_twist, it_imu); // For performance, do not instantiate `rclcpp::Time` inside of the for-loop double twist_stamp = rclcpp::Time(it_twist->header.stamp).seconds(); @@ -250,42 +372,39 @@ void DistortionCorrector::undistortPointCloud( is_imu_valid = false; } - float time_offset = static_cast(global_point_stamp - prev_time_stamp_sec); + auto time_offset = static_cast(global_point_stamp - prev_time_stamp_sec); // Undistort a single point based on the strategy - undistortPoint(it_x, it_y, it_z, it_twist, it_imu, time_offset, is_twist_valid, is_imu_valid); - - prev_time_stamp_sec = global_point_stamp; - } + undistort_point(it_x, it_y, it_z, it_twist, it_imu, time_offset, is_twist_valid, is_imu_valid); + + if (angle_conversion_opt.has_value()) { + if (!pointcloud_transform_needed_) { + throw std::runtime_error( + "The pointcloud is not in the sensor's frame and thus azimuth and distance cannot be " + "updated. " + "Please change the input pointcloud or set update_azimuth_and_distance to false."); + } + float cartesian_coordinate_azimuth = + autoware::universe_utils::opencv_fast_atan2(*it_y, *it_x); + float updated_azimuth = angle_conversion_opt->offset_rad + + angle_conversion_opt->sign * cartesian_coordinate_azimuth; + if (updated_azimuth < 0) { + updated_azimuth += autoware::universe_utils::pi * 2; + } else if (updated_azimuth > 2 * autoware::universe_utils::pi) { + updated_azimuth -= autoware::universe_utils::pi * 2; + } - warnIfTimestampIsTooLate(is_twist_time_stamp_too_late, is_imu_time_stamp_too_late); -} + *it_azimuth = updated_azimuth; + *it_distance = sqrt(*it_x * *it_x + *it_y * *it_y + *it_z * *it_z); -template -void DistortionCorrector::warnIfTimestampIsTooLate( - bool is_twist_time_stamp_too_late, bool is_imu_time_stamp_too_late) -{ - if (is_twist_time_stamp_too_late) { - RCLCPP_WARN_STREAM_THROTTLE( - node_->get_logger(), *node_->get_clock(), 10000 /* ms */, - "Twist time_stamp is too late. Could not interpolate."); - } + ++it_azimuth; + ++it_distance; + } - if (is_imu_time_stamp_too_late) { - RCLCPP_WARN_STREAM_THROTTLE( - node_->get_logger(), *node_->get_clock(), 10000 /* ms */, - "IMU time_stamp is too late. Could not interpolate."); + prev_time_stamp_sec = global_point_stamp; } -} -template -void DistortionCorrector::convertMatrixToTransform( - const Eigen::Matrix4f & matrix, tf2::Transform & transform) -{ - transform.setOrigin(tf2::Vector3(matrix(0, 3), matrix(1, 3), matrix(2, 3))); - transform.setBasis(tf2::Matrix3x3( - matrix(0, 0), matrix(0, 1), matrix(0, 2), matrix(1, 0), matrix(1, 1), matrix(1, 2), - matrix(2, 0), matrix(2, 1), matrix(2, 2))); + warn_if_timestamp_is_too_late(is_twist_time_stamp_too_late, is_imu_time_stamp_too_late); } ///////////////////////// Functions for different undistortion strategies ///////////////////////// @@ -302,7 +421,7 @@ void DistortionCorrector3D::initialize() prev_transformation_matrix_ = Eigen::Matrix4f::Identity(); } -void DistortionCorrector2D::setPointCloudTransform( +void DistortionCorrector2D::set_pointcloud_transform( const std::string & base_frame, const std::string & lidar_frame) { if (pointcloud_transform_exists_) { @@ -312,12 +431,12 @@ void DistortionCorrector2D::setPointCloudTransform( Eigen::Matrix4f eigen_lidar_to_base_link; pointcloud_transform_exists_ = managed_tf_buffer_->getTransform(base_frame, lidar_frame, eigen_lidar_to_base_link); - convertMatrixToTransform(eigen_lidar_to_base_link, tf2_lidar_to_base_link_); + tf2_lidar_to_base_link_ = convert_matrix_to_transform(eigen_lidar_to_base_link); tf2_base_link_to_lidar_ = tf2_lidar_to_base_link_.inverse(); pointcloud_transform_needed_ = base_frame != lidar_frame && pointcloud_transform_exists_; } -void DistortionCorrector3D::setPointCloudTransform( +void DistortionCorrector3D::set_pointcloud_transform( const std::string & base_frame, const std::string & lidar_frame) { if (pointcloud_transform_exists_) { @@ -330,7 +449,7 @@ void DistortionCorrector3D::setPointCloudTransform( pointcloud_transform_needed_ = base_frame != lidar_frame && pointcloud_transform_exists_; } -inline void DistortionCorrector2D::undistortPointImplementation( +inline void DistortionCorrector2D::undistort_point_implementation( sensor_msgs::PointCloud2Iterator & it_x, sensor_msgs::PointCloud2Iterator & it_y, sensor_msgs::PointCloud2Iterator & it_z, std::deque::iterator & it_twist, @@ -338,7 +457,8 @@ inline void DistortionCorrector2D::undistortPointImplementation( const bool & is_twist_valid, const bool & is_imu_valid) { // Initialize linear velocity and angular velocity - float v{0.0f}, w{0.0f}; + float v{0.0f}; + float w{0.0f}; if (is_twist_valid) { v = static_cast(it_twist->twist.linear.x); w = static_cast(it_twist->twist.angular.z); @@ -354,14 +474,15 @@ inline void DistortionCorrector2D::undistortPointImplementation( point_tf_ = tf2_lidar_to_base_link_ * point_tf_; } theta_ += w * time_offset; + auto [sin_half_theta, cos_half_theta] = autoware::universe_utils::sin_and_cos(theta_ * 0.5f); + auto [sin_theta, cos_theta] = autoware::universe_utils::sin_and_cos(theta_); + baselink_quat_.setValue( - 0, 0, autoware::universe_utils::sin(theta_ * 0.5f), - autoware::universe_utils::cos( - theta_ * - 0.5f)); // baselink_quat.setRPY(0.0, 0.0, theta); (Note that the value is slightly different) + 0, 0, sin_half_theta, cos_half_theta); // baselink_quat.setRPY(0.0, 0.0, theta); (Note that the + // value is slightly different) const float dis = v * time_offset; - x_ += dis * autoware::universe_utils::cos(theta_); - y_ += dis * autoware::universe_utils::sin(theta_); + x_ += dis * cos_theta; + y_ += dis * sin_theta; baselink_tf_odom_.setOrigin(tf2::Vector3(x_, y_, 0.0)); baselink_tf_odom_.setRotation(baselink_quat_); @@ -377,7 +498,7 @@ inline void DistortionCorrector2D::undistortPointImplementation( *it_z = static_cast(undistorted_point_tf_.getZ()); } -inline void DistortionCorrector3D::undistortPointImplementation( +inline void DistortionCorrector3D::undistort_point_implementation( sensor_msgs::PointCloud2Iterator & it_x, sensor_msgs::PointCloud2Iterator & it_y, sensor_msgs::PointCloud2Iterator & it_z, std::deque::iterator & it_twist, @@ -385,19 +506,24 @@ inline void DistortionCorrector3D::undistortPointImplementation( const bool & is_twist_valid, const bool & is_imu_valid) { // Initialize linear velocity and angular velocity - float v_x_{0.0f}, v_y_{0.0f}, v_z_{0.0f}, w_x_{0.0f}, w_y_{0.0f}, w_z_{0.0f}; + float v_x{0.0f}; + float v_y{0.0f}; + float v_z{0.0f}; + float w_x{0.0f}; + float w_y{0.0f}; + float w_z{0.0f}; if (is_twist_valid) { - v_x_ = static_cast(it_twist->twist.linear.x); - v_y_ = static_cast(it_twist->twist.linear.y); - v_z_ = static_cast(it_twist->twist.linear.z); - w_x_ = static_cast(it_twist->twist.angular.x); - w_y_ = static_cast(it_twist->twist.angular.y); - w_z_ = static_cast(it_twist->twist.angular.z); + v_x = static_cast(it_twist->twist.linear.x); + v_y = static_cast(it_twist->twist.linear.y); + v_z = static_cast(it_twist->twist.linear.z); + w_x = static_cast(it_twist->twist.angular.x); + w_y = static_cast(it_twist->twist.angular.y); + w_z = static_cast(it_twist->twist.angular.z); } if (is_imu_valid) { - w_x_ = static_cast(it_imu->vector.x); - w_y_ = static_cast(it_imu->vector.y); - w_z_ = static_cast(it_imu->vector.z); + w_x = static_cast(it_imu->vector.x); + w_y = static_cast(it_imu->vector.y); + w_z = static_cast(it_imu->vector.z); } // Undistort point @@ -406,7 +532,7 @@ inline void DistortionCorrector3D::undistortPointImplementation( point_eigen_ = eigen_lidar_to_base_link_ * point_eigen_; } - Sophus::SE3f::Tangent twist(v_x_, v_y_, v_z_, w_x_, w_y_, w_z_); + Sophus::SE3f::Tangent twist(v_x, v_y, v_z, w_x, w_y, w_z); twist = twist * time_offset; transformation_matrix_ = Sophus::SE3f::exp(twist).matrix(); transformation_matrix_ = transformation_matrix_ * prev_transformation_matrix_; diff --git a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp index 8e4eaa6ec8f11..896c7fe563e64 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp @@ -35,6 +35,7 @@ DistortionCorrectorComponent::DistortionCorrectorComponent(const rclcpp::NodeOpt base_frame_ = declare_parameter("base_frame"); use_imu_ = declare_parameter("use_imu"); use_3d_distortion_correction_ = declare_parameter("use_3d_distortion_correction"); + update_azimuth_and_distance_ = declare_parameter("update_azimuth_and_distance"); auto has_static_tf_only = declare_parameter("has_static_tf_only", false); // TODO(amadeuszsz): remove default value @@ -50,39 +51,39 @@ DistortionCorrectorComponent::DistortionCorrectorComponent(const rclcpp::NodeOpt // Subscriber twist_sub_ = this->create_subscription( "~/input/twist", 10, - std::bind(&DistortionCorrectorComponent::onTwist, this, std::placeholders::_1)); + std::bind(&DistortionCorrectorComponent::twist_callback, this, std::placeholders::_1)); imu_sub_ = this->create_subscription( "~/input/imu", 10, - std::bind(&DistortionCorrectorComponent::onImu, this, std::placeholders::_1)); + std::bind(&DistortionCorrectorComponent::imu_callback, this, std::placeholders::_1)); pointcloud_sub_ = this->create_subscription( "~/input/pointcloud", rclcpp::SensorDataQoS(), - std::bind(&DistortionCorrectorComponent::onPointCloud, this, std::placeholders::_1)); + std::bind(&DistortionCorrectorComponent::pointcloud_callback, this, std::placeholders::_1)); // Setup the distortion corrector if (use_3d_distortion_correction_) { - distortion_corrector_ = std::make_unique(this, has_static_tf_only); + distortion_corrector_ = std::make_unique(*this, has_static_tf_only); } else { - distortion_corrector_ = std::make_unique(this, has_static_tf_only); + distortion_corrector_ = std::make_unique(*this, has_static_tf_only); } } -void DistortionCorrectorComponent::onTwist( +void DistortionCorrectorComponent::twist_callback( const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) { - distortion_corrector_->processTwistMessage(twist_msg); + distortion_corrector_->process_twist_message(twist_msg); } -void DistortionCorrectorComponent::onImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) +void DistortionCorrectorComponent::imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) { if (!use_imu_) { return; } - distortion_corrector_->processIMUMessage(base_frame_, imu_msg); + distortion_corrector_->process_imu_message(base_frame_, imu_msg); } -void DistortionCorrectorComponent::onPointCloud(PointCloud2::UniquePtr pointcloud_msg) +void DistortionCorrectorComponent::pointcloud_callback(PointCloud2::UniquePtr pointcloud_msg) { stop_watch_ptr_->toc("processing_time", true); const auto points_sub_count = undistorted_pointcloud_pub_->get_subscription_count() + @@ -92,10 +93,25 @@ void DistortionCorrectorComponent::onPointCloud(PointCloud2::UniquePtr pointclou return; } - distortion_corrector_->setPointCloudTransform(base_frame_, pointcloud_msg->header.frame_id); - + distortion_corrector_->set_pointcloud_transform(base_frame_, pointcloud_msg->header.frame_id); distortion_corrector_->initialize(); - distortion_corrector_->undistortPointCloud(use_imu_, *pointcloud_msg); + + if (update_azimuth_and_distance_ && !angle_conversion_opt_.has_value()) { + angle_conversion_opt_ = distortion_corrector_->try_compute_angle_conversion(*pointcloud_msg); + if (angle_conversion_opt_.has_value()) { + RCLCPP_INFO( + this->get_logger(), + "Success to get the conversion formula between Cartesian coordinates and LiDAR azimuth " + "coordinates"); + } else { + RCLCPP_ERROR_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), 10000 /* ms */, + "Failed to get the angle conversion between Cartesian coordinates and LiDAR azimuth " + "coordinates. This pointcloud will not update azimuth and distance"); + } + } + + distortion_corrector_->undistort_pointcloud(use_imu_, angle_conversion_opt_, *pointcloud_msg); if (debug_publisher_) { auto pipeline_latency_ms = diff --git a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp index 047d021a4c6da..895061229a994 100644 --- a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp @@ -26,6 +26,7 @@ // 10.09, 10.117, 10.144, 10.171, 10.198, 10.225 #include "autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp" +#include "autoware/universe_utils/math/constants.hpp" #include "autoware/universe_utils/math/trigonometry.hpp" #include @@ -41,6 +42,7 @@ #include +enum AngleCoordinateSystem { HESAI, VELODYNE, CARTESIAN }; class DistortionCorrectorTest : public ::testing::Test { protected: @@ -48,13 +50,13 @@ class DistortionCorrectorTest : public ::testing::Test { node_ = std::make_shared("test_node"); distortion_corrector_2d_ = - std::make_shared(node_.get(), true); + std::make_shared(*node_, true); distortion_corrector_3d_ = - std::make_shared(node_.get(), true); + std::make_shared(*node_, true); // Setup TF tf_broadcaster_ = std::make_shared(node_); - tf_broadcaster_->sendTransform(generateStaticTransformMsg()); + tf_broadcaster_->sendTransform(generate_static_transform_msgs()); // Spin the node for a while to ensure transforms are published auto start = std::chrono::steady_clock::now(); @@ -67,27 +69,27 @@ class DistortionCorrectorTest : public ::testing::Test void TearDown() override {} - void checkInput(int ms) { ASSERT_LT(ms, 1000) << "ms should be less than a second."; } + static void check_input(int ms) { ASSERT_LT(ms, 1000) << "ms should be less than a second."; } - rclcpp::Time addMilliseconds(rclcpp::Time stamp, int ms) + static rclcpp::Time add_milliseconds(const rclcpp::Time & stamp, int ms) { - checkInput(ms); + check_input(ms); auto ms_in_ns = rclcpp::Duration(0, ms * 1000000); return stamp + ms_in_ns; } - rclcpp::Time subtractMilliseconds(rclcpp::Time stamp, int ms) + static rclcpp::Time subtract_milliseconds(const rclcpp::Time & stamp, int ms) { - checkInput(ms); + check_input(ms); auto ms_in_ns = rclcpp::Duration(0, ms * 1000000); return stamp - ms_in_ns; } - geometry_msgs::msg::TransformStamped generateTransformMsg( + static geometry_msgs::msg::TransformStamped generate_transform_msg( const std::string & parent_frame, const std::string & child_frame, double x, double y, double z, double qx, double qy, double qz, double qw) { - rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); + rclcpp::Time timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME); geometry_msgs::msg::TransformStamped tf_msg; tf_msg.header.stamp = timestamp; tf_msg.header.frame_id = parent_frame; @@ -102,16 +104,16 @@ class DistortionCorrectorTest : public ::testing::Test return tf_msg; } - std::vector generateStaticTransformMsg() + static std::vector generate_static_transform_msgs() { // generate defined transformations return { - generateTransformMsg("base_link", "lidar_top", 5.0, 5.0, 5.0, 0.683, 0.5, 0.183, 0.499), - generateTransformMsg("base_link", "imu_link", 1.0, 1.0, 3.0, 0.278, 0.717, 0.441, 0.453)}; + generate_transform_msg("base_link", "lidar_top", 5.0, 5.0, 5.0, 0.683, 0.5, 0.183, 0.499), + generate_transform_msg("base_link", "imu_link", 1.0, 1.0, 3.0, 0.278, 0.717, 0.441, 0.453)}; } - std::shared_ptr generateTwistMsg( - double linear_x, double angular_z, rclcpp::Time stamp) + static std::shared_ptr generate_twist_msg( + double linear_x, double angular_z, const rclcpp::Time & stamp) { auto twist_msg = std::make_shared(); twist_msg->header.stamp = stamp; @@ -121,8 +123,8 @@ class DistortionCorrectorTest : public ::testing::Test return twist_msg; } - std::shared_ptr generateImuMsg( - double angular_vel_x, double angular_vel_y, double angular_vel_z, rclcpp::Time stamp) + static std::shared_ptr generate_imu_msg( + double angular_vel_x, double angular_vel_y, double angular_vel_z, const rclcpp::Time & stamp) { auto imu_msg = std::make_shared(); imu_msg->header.stamp = stamp; @@ -133,44 +135,98 @@ class DistortionCorrectorTest : public ::testing::Test return imu_msg; } - std::vector> generateTwistMsgs( - rclcpp::Time pointcloud_timestamp) + static std::vector> + generate_twist_msgs(const rclcpp::Time & pointcloud_timestamp) { std::vector> twist_msgs; - rclcpp::Time twist_stamp = subtractMilliseconds(pointcloud_timestamp, 5); + rclcpp::Time twist_stamp = subtract_milliseconds(pointcloud_timestamp, 5); - for (int i = 0; i < number_of_twist_msgs_; ++i) { - auto twist_msg = generateTwistMsg( - twist_linear_x_ + i * twist_linear_x_increment_, - twist_angular_z_ + i * twist_angular_z_increment_, twist_stamp); + for (int i = 0; i < number_of_twist_msgs; ++i) { + auto twist_msg = generate_twist_msg( + twist_linear_x + i * twist_linear_x_increment, + twist_angular_z + i * twist_angular_z_increment, twist_stamp); twist_msgs.push_back(twist_msg); - twist_stamp = addMilliseconds(twist_stamp, twist_msgs_interval_ms_); + twist_stamp = add_milliseconds(twist_stamp, twist_msgs_interval_ms); } return twist_msgs; } - std::vector> generateImuMsgs( - rclcpp::Time pointcloud_timestamp) + static std::vector> generate_imu_msgs( + const rclcpp::Time & pointcloud_timestamp) { std::vector> imu_msgs; - rclcpp::Time imu_stamp = subtractMilliseconds(pointcloud_timestamp, 10); + rclcpp::Time imu_stamp = subtract_milliseconds(pointcloud_timestamp, 10); - for (int i = 0; i < number_of_imu_msgs_; ++i) { - auto imu_msg = generateImuMsg( - imu_angular_x_ + i * imu_angular_x_increment_, - imu_angular_y_ + i * imu_angular_y_increment_, - imu_angular_z_ + i * imu_angular_z_increment_, imu_stamp); + for (int i = 0; i < number_of_imu_msgs; ++i) { + auto imu_msg = generate_imu_msg( + imu_angular_x + i * imu_angular_x_increment, imu_angular_y + i * imu_angular_y_increment, + imu_angular_z + i * imu_angular_z_increment, imu_stamp); imu_msgs.push_back(imu_msg); - imu_stamp = addMilliseconds(imu_stamp, imu_msgs_interval_ms_); + imu_stamp = add_milliseconds(imu_stamp, imu_msgs_interval_ms); } return imu_msgs; } - sensor_msgs::msg::PointCloud2 generatePointCloudMsg( - bool generate_points, bool is_lidar_frame, rclcpp::Time stamp) + static std::tuple, std::vector> generate_default_pointcloud( + AngleCoordinateSystem coordinate_system) + { + // Generate all combinations of signs { -, 0, + } x { -, 0, + } for x and y. + // Also include the case of (0, 0 ,0) + std::vector default_points = {{ + Eigen::Vector3f(0.0f, 0.0f, 0.0f), // point 1 + Eigen::Vector3f(0.0f, 0.0f, 0.0f), // point 2 + Eigen::Vector3f(10.0f, 0.0f, 1.0f), // point 3 + Eigen::Vector3f(5.0f, -5.0f, 2.0f), // point 4 + Eigen::Vector3f(0.0f, -10.0f, 3.0f), // point 5 + Eigen::Vector3f(-5.0f, -5.0f, 4.0f), // point 6 + Eigen::Vector3f(-10.0f, 0.0f, 5.0f), // point 7 + Eigen::Vector3f(-5.0f, 5.0f, -5.0f), // point 8 + Eigen::Vector3f(0.0f, 10.0f, -4.0f), // point 9 + Eigen::Vector3f(5.0f, 5.0f, -3.0f), // point 10 + }}; + + std::vector default_azimuths; + for (const auto & point : default_points) { + if (coordinate_system == AngleCoordinateSystem::VELODYNE) { + // velodyne coordinates: x-axis is 0 degrees, y-axis is 270 degrees, angle increase in + // clockwise direction + float cartesian_deg = std::atan2(point.y(), point.x()) * 180 / autoware::universe_utils::pi; + if (cartesian_deg < 0) cartesian_deg += 360; + float velodyne_deg = 360 - cartesian_deg; + if (velodyne_deg == 360) velodyne_deg = 0; + default_azimuths.push_back(velodyne_deg * autoware::universe_utils::pi / 180); + } else if (coordinate_system == AngleCoordinateSystem::HESAI) { + // hesai coordinates: y-axis is 0 degrees, x-axis is 90 degrees, angle increase in clockwise + // direction + float cartesian_deg = std::atan2(point.y(), point.x()) * 180 / autoware::universe_utils::pi; + if (cartesian_deg < 0) cartesian_deg += 360; + float hesai_deg = 90 - cartesian_deg < 0 ? 90 - cartesian_deg + 360 : 90 - cartesian_deg; + if (hesai_deg == 360) hesai_deg = 0; + default_azimuths.push_back(hesai_deg * autoware::universe_utils::pi / 180); + } else if (coordinate_system == AngleCoordinateSystem::CARTESIAN) { + // Cartesian coordinates: x-axis is 0 degrees, y-axis is 90 degrees, angle increase in + // counterclockwise direction + default_azimuths.push_back(std::atan2(point.y(), point.x())); + } else { + throw std::runtime_error("Invalid angle coordinate system"); + } + } + + return std::make_tuple(default_points, default_azimuths); + } + + sensor_msgs::msg::PointCloud2 generate_empty_pointcloud_msg(const rclcpp::Time & stamp) + { + auto empty_pointcloud_msg = generate_pointcloud_msg(true, stamp, {}, {}); + return empty_pointcloud_msg; + } + + sensor_msgs::msg::PointCloud2 generate_pointcloud_msg( + bool is_lidar_frame, const rclcpp::Time & stamp, std::vector points, + std::vector azimuths) { sensor_msgs::msg::PointCloud2 pointcloud_msg; pointcloud_msg.header.stamp = stamp; @@ -179,72 +235,81 @@ class DistortionCorrectorTest : public ::testing::Test pointcloud_msg.is_dense = true; pointcloud_msg.is_bigendian = false; - if (generate_points) { - std::array points = {{ - Eigen::Vector3f(10.0f, 0.0f, 0.0f), // point 1 - Eigen::Vector3f(0.0f, 10.0f, 0.0f), // point 2 - Eigen::Vector3f(0.0f, 0.0f, 10.0f), // point 3 - Eigen::Vector3f(20.0f, 0.0f, 0.0f), // point 4 - Eigen::Vector3f(0.0f, 20.0f, 0.0f), // point 5 - Eigen::Vector3f(0.0f, 0.0f, 20.0f), // point 6 - Eigen::Vector3f(30.0f, 0.0f, 0.0f), // point 7 - Eigen::Vector3f(0.0f, 30.0f, 0.0f), // point 8 - Eigen::Vector3f(0.0f, 0.0f, 30.0f), // point 9 - Eigen::Vector3f(10.0f, 10.0f, 10.0f) // point 10 - }}; - - // Generate timestamps for the points - std::vector timestamps = generatePointTimestamps(stamp, number_of_points_); - - sensor_msgs::PointCloud2Modifier modifier(pointcloud_msg); - modifier.setPointCloud2Fields( - 10, "x", 1, sensor_msgs::msg::PointField::FLOAT32, "y", 1, - sensor_msgs::msg::PointField::FLOAT32, "z", 1, sensor_msgs::msg::PointField::FLOAT32, - "intensity", 1, sensor_msgs::msg::PointField::UINT8, "return_type", 1, - sensor_msgs::msg::PointField::UINT8, "channel", 1, sensor_msgs::msg::PointField::UINT16, - "azimuth", 1, sensor_msgs::msg::PointField::FLOAT32, "elevation", 1, - sensor_msgs::msg::PointField::FLOAT32, "distance", 1, sensor_msgs::msg::PointField::FLOAT32, - "time_stamp", 1, sensor_msgs::msg::PointField::UINT32); - - modifier.resize(number_of_points_); - - sensor_msgs::PointCloud2Iterator iter_x(pointcloud_msg, "x"); - sensor_msgs::PointCloud2Iterator iter_y(pointcloud_msg, "y"); - sensor_msgs::PointCloud2Iterator iter_z(pointcloud_msg, "z"); - sensor_msgs::PointCloud2Iterator iter_t(pointcloud_msg, "time_stamp"); - - for (size_t i = 0; i < number_of_points_; ++i) { - *iter_x = points[i].x(); - *iter_y = points[i].y(); - *iter_z = points[i].z(); - *iter_t = timestamps[i]; - ++iter_x; - ++iter_y; - ++iter_z; - ++iter_t; - } - } else { - pointcloud_msg.width = 0; - pointcloud_msg.row_step = 0; + // Generate timestamps for the points + std::vector timestamps = generate_point_timestamps(stamp, points.size()); + + sensor_msgs::PointCloud2Modifier modifier(pointcloud_msg); + modifier.setPointCloud2Fields( + 10, "x", 1, sensor_msgs::msg::PointField::FLOAT32, "y", 1, + sensor_msgs::msg::PointField::FLOAT32, "z", 1, sensor_msgs::msg::PointField::FLOAT32, + "intensity", 1, sensor_msgs::msg::PointField::UINT8, "return_type", 1, + sensor_msgs::msg::PointField::UINT8, "channel", 1, sensor_msgs::msg::PointField::UINT16, + "azimuth", 1, sensor_msgs::msg::PointField::FLOAT32, "elevation", 1, + sensor_msgs::msg::PointField::FLOAT32, "distance", 1, sensor_msgs::msg::PointField::FLOAT32, + "time_stamp", 1, sensor_msgs::msg::PointField::UINT32); + + modifier.resize(points.size()); + + sensor_msgs::PointCloud2Iterator iter_x(pointcloud_msg, "x"); + sensor_msgs::PointCloud2Iterator iter_y(pointcloud_msg, "y"); + sensor_msgs::PointCloud2Iterator iter_z(pointcloud_msg, "z"); + sensor_msgs::PointCloud2Iterator iter_azimuth(pointcloud_msg, "azimuth"); + sensor_msgs::PointCloud2Iterator iter_distance(pointcloud_msg, "distance"); + sensor_msgs::PointCloud2Iterator iter_t(pointcloud_msg, "time_stamp"); + + for (size_t i = 0; i < points.size(); ++i) { + *iter_x = points[i].x(); + *iter_y = points[i].y(); + *iter_z = points[i].z(); + + *iter_azimuth = azimuths[i]; + *iter_distance = points[i].norm(); + *iter_t = timestamps[i]; + ++iter_x; + ++iter_y; + ++iter_z; + ++iter_azimuth; + ++iter_distance; + ++iter_t; } return pointcloud_msg; } - std::vector generatePointTimestamps( - rclcpp::Time pointcloud_timestamp, size_t number_of_points) + std::vector generate_point_timestamps( + const rclcpp::Time & pointcloud_timestamp, size_t number_of_points) { std::vector timestamps; rclcpp::Time global_point_stamp = pointcloud_timestamp; for (size_t i = 0; i < number_of_points; ++i) { std::uint32_t relative_timestamp = (global_point_stamp - pointcloud_timestamp).nanoseconds(); timestamps.push_back(relative_timestamp); - global_point_stamp = addMilliseconds(global_point_stamp, points_interval_ms_); + global_point_stamp = add_milliseconds(global_point_stamp, points_interval_ms); } return timestamps; } + template + void generate_and_process_twist_msgs( + const std::shared_ptr & distortion_corrector, const rclcpp::Time & timestamp) + { + auto twist_msgs = generate_twist_msgs(timestamp); + for (const auto & twist_msg : twist_msgs) { + distortion_corrector->process_twist_message(twist_msg); + } + } + + template + void generate_and_process_imu_msgs( + const std::shared_ptr & distortion_corrector, const rclcpp::Time & timestamp) + { + auto imu_msgs = generate_imu_msgs(timestamp); + for (const auto & imu_msg : imu_msgs) { + distortion_corrector->process_imu_message("base_link", imu_msg); + } + } + std::shared_ptr node_; std::shared_ptr distortion_corrector_2d_; @@ -252,29 +317,29 @@ class DistortionCorrectorTest : public ::testing::Test distortion_corrector_3d_; std::shared_ptr tf_broadcaster_; - static constexpr float standard_tolerance_{1e-4}; - static constexpr float coarse_tolerance_{5e-3}; - static constexpr int number_of_twist_msgs_{6}; - static constexpr int number_of_imu_msgs_{6}; - static constexpr size_t number_of_points_{10}; - static constexpr int32_t timestamp_seconds_{10}; - static constexpr uint32_t timestamp_nanoseconds_{100000000}; - - static constexpr double twist_linear_x_{10.0}; - static constexpr double twist_angular_z_{0.02}; - static constexpr double twist_linear_x_increment_{2.0}; - static constexpr double twist_angular_z_increment_{0.01}; - - static constexpr double imu_angular_x_{0.01}; - static constexpr double imu_angular_y_{-0.02}; - static constexpr double imu_angular_z_{0.05}; - static constexpr double imu_angular_x_increment_{0.005}; - static constexpr double imu_angular_y_increment_{0.005}; - static constexpr double imu_angular_z_increment_{0.005}; - - static constexpr int points_interval_ms_{10}; - static constexpr int twist_msgs_interval_ms_{24}; - static constexpr int imu_msgs_interval_ms_{27}; + static constexpr float standard_tolerance{1e-4}; + static constexpr float coarse_tolerance{5e-3}; + static constexpr int number_of_twist_msgs{6}; + static constexpr int number_of_imu_msgs{6}; + static constexpr size_t number_of_points{10}; + static constexpr int32_t timestamp_seconds{10}; + static constexpr uint32_t timestamp_nanoseconds{100000000}; + + static constexpr double twist_linear_x{10.0}; + static constexpr double twist_angular_z{0.02}; + static constexpr double twist_linear_x_increment{2.0}; + static constexpr double twist_angular_z_increment{0.01}; + + static constexpr double imu_angular_x{0.01}; + static constexpr double imu_angular_y{-0.02}; + static constexpr double imu_angular_z{0.05}; + static constexpr double imu_angular_x_increment{0.005}; + static constexpr double imu_angular_y_increment{0.005}; + static constexpr double imu_angular_z_increment{0.005}; + + static constexpr int points_interval_ms{10}; + static constexpr int twist_msgs_interval_ms{24}; + static constexpr int imu_msgs_interval_ms{27}; // for debugging or regenerating the ground truth point cloud bool debug_{false}; @@ -282,92 +347,87 @@ class DistortionCorrectorTest : public ::testing::Test TEST_F(DistortionCorrectorTest, TestProcessTwistMessage) { - rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); - auto twist_msg = generateTwistMsg(twist_linear_x_, twist_angular_z_, timestamp); - distortion_corrector_2d_->processTwistMessage(twist_msg); + rclcpp::Time timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME); + auto twist_msg = generate_twist_msg(twist_linear_x, twist_angular_z, timestamp); + distortion_corrector_2d_->process_twist_message(twist_msg); ASSERT_FALSE(distortion_corrector_2d_->get_twist_queue().empty()); - EXPECT_EQ(distortion_corrector_2d_->get_twist_queue().front().twist.linear.x, twist_linear_x_); - EXPECT_EQ(distortion_corrector_2d_->get_twist_queue().front().twist.angular.z, twist_angular_z_); + EXPECT_EQ(distortion_corrector_2d_->get_twist_queue().front().twist.linear.x, twist_linear_x); + EXPECT_EQ(distortion_corrector_2d_->get_twist_queue().front().twist.angular.z, twist_angular_z); } -TEST_F(DistortionCorrectorTest, TestProcessIMUMessage) +TEST_F(DistortionCorrectorTest, TestProcessImuMessage) { - rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); - auto imu_msg = generateImuMsg(imu_angular_x_, imu_angular_y_, imu_angular_z_, timestamp); - distortion_corrector_2d_->processIMUMessage("base_link", imu_msg); + rclcpp::Time timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME); + auto imu_msg = generate_imu_msg(imu_angular_x, imu_angular_y, imu_angular_z, timestamp); + distortion_corrector_2d_->process_imu_message("base_link", imu_msg); ASSERT_FALSE(distortion_corrector_2d_->get_angular_velocity_queue().empty()); EXPECT_NEAR( distortion_corrector_2d_->get_angular_velocity_queue().front().vector.z, -0.03159, - standard_tolerance_); + standard_tolerance); } -TEST_F(DistortionCorrectorTest, TestIsInputValid) +TEST_F(DistortionCorrectorTest, TestIsPointcloudValid) { - rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); + rclcpp::Time timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME); - // input normal pointcloud without twist - sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, timestamp); - bool result = distortion_corrector_2d_->isInputValid(pointcloud); - EXPECT_FALSE(result); - - // input normal pointcloud with valid twist - auto twist_msg = generateTwistMsg(twist_linear_x_, twist_angular_z_, timestamp); - distortion_corrector_2d_->processTwistMessage(twist_msg); - - pointcloud = generatePointCloudMsg(true, false, timestamp); - result = distortion_corrector_2d_->isInputValid(pointcloud); + auto [default_points, default_azimuths] = + generate_default_pointcloud(AngleCoordinateSystem::CARTESIAN); + auto pointcloud = generate_pointcloud_msg(false, timestamp, default_points, default_azimuths); + auto result = distortion_corrector_2d_->is_pointcloud_valid(pointcloud); EXPECT_TRUE(result); // input empty pointcloud - pointcloud = generatePointCloudMsg(false, false, timestamp); - result = distortion_corrector_2d_->isInputValid(pointcloud); + auto empty_pointcloud = generate_empty_pointcloud_msg(timestamp); + result = distortion_corrector_2d_->is_pointcloud_valid(empty_pointcloud); EXPECT_FALSE(result); } -TEST_F(DistortionCorrectorTest, TestSetPointCloudTransformWithBaseLink) +TEST_F(DistortionCorrectorTest, TestSetPointcloudTransformWithBaseLink) { - distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link"); + distortion_corrector_2d_->set_pointcloud_transform("base_link", "base_link"); EXPECT_TRUE(distortion_corrector_2d_->pointcloud_transform_exists()); EXPECT_FALSE(distortion_corrector_2d_->pointcloud_transform_needed()); } -TEST_F(DistortionCorrectorTest, TestSetPointCloudTransformWithLidarFrame) +TEST_F(DistortionCorrectorTest, TestSetPointcloudTransformWithLidarFrame) { - distortion_corrector_2d_->setPointCloudTransform("base_link", "lidar_top"); + distortion_corrector_2d_->set_pointcloud_transform("base_link", "lidar_top"); EXPECT_TRUE(distortion_corrector_2d_->pointcloud_transform_exists()); EXPECT_TRUE(distortion_corrector_2d_->pointcloud_transform_needed()); } -TEST_F(DistortionCorrectorTest, TestSetPointCloudTransformWithMissingFrame) +TEST_F(DistortionCorrectorTest, TestSetPointcloudTransformWithMissingFrame) { - distortion_corrector_2d_->setPointCloudTransform("base_link", "missing_lidar_frame"); + distortion_corrector_2d_->set_pointcloud_transform("base_link", "missing_lidar_frame"); EXPECT_FALSE(distortion_corrector_2d_->pointcloud_transform_exists()); EXPECT_FALSE(distortion_corrector_2d_->pointcloud_transform_needed()); } -TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithEmptyTwist) +TEST_F(DistortionCorrectorTest, TestUndistortPointcloudWithEmptyTwist) { - rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); + rclcpp::Time timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME); // Generate the point cloud message - sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, timestamp); + auto [default_points, default_azimuths] = + generate_default_pointcloud(AngleCoordinateSystem::CARTESIAN); + auto pointcloud = generate_pointcloud_msg(false, timestamp, default_points, default_azimuths); // Process empty twist queue distortion_corrector_2d_->initialize(); - distortion_corrector_2d_->undistortPointCloud(false, pointcloud); + distortion_corrector_2d_->undistort_pointcloud(false, std::nullopt, pointcloud); // Verify the point cloud is not changed sensor_msgs::PointCloud2ConstIterator iter_x(pointcloud, "x"); sensor_msgs::PointCloud2ConstIterator iter_y(pointcloud, "y"); sensor_msgs::PointCloud2ConstIterator iter_z(pointcloud, "z"); - std::array expected_pointcloud = { - {Eigen::Vector3f(10.0f, 0.0f, 0.0f), Eigen::Vector3f(0.0f, 10.0f, 0.0f), - Eigen::Vector3f(0.0f, 0.0f, 10.0f), Eigen::Vector3f(20.0f, 0.0f, 0.0f), - Eigen::Vector3f(0.0f, 20.0f, 0.0f), Eigen::Vector3f(0.0f, 0.0f, 20.0f), - Eigen::Vector3f(30.0f, 0.0f, 0.0f), Eigen::Vector3f(0.0f, 30.0f, 0.0f), - Eigen::Vector3f(0.0f, 0.0f, 30.0f), Eigen::Vector3f(10.0f, 10.0f, 10.0f)}}; + std::array expected_pointcloud = { + {Eigen::Vector3f(0.0f, 0.0f, 0.0f), Eigen::Vector3f(0.0f, 0.0f, 0.0f), + Eigen::Vector3f(10.0f, 0.0f, 1.0f), Eigen::Vector3f(5.0f, -5.0f, 2.0f), + Eigen::Vector3f(0.0f, -10.0f, 3.0f), Eigen::Vector3f(-5.0f, -5.0f, 4.0f), + Eigen::Vector3f(-10.0f, 0.0f, 5.0f), Eigen::Vector3f(-5.0f, 5.0f, -5.0f), + Eigen::Vector3f(0.0f, 10.0f, -4.0f), Eigen::Vector3f(5.0f, 5.0f, -3.0f)}}; size_t i = 0; std::ostringstream oss; @@ -375,9 +435,9 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithEmptyTwist) for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++i) { oss << "Point " << i << ": (" << *iter_x << ", " << *iter_y << ", " << *iter_z << ")\n"; - EXPECT_NEAR(*iter_x, expected_pointcloud[i].x(), standard_tolerance_); - EXPECT_NEAR(*iter_y, expected_pointcloud[i].y(), standard_tolerance_); - EXPECT_NEAR(*iter_z, expected_pointcloud[i].z(), standard_tolerance_); + EXPECT_NEAR(*iter_x, expected_pointcloud[i].x(), standard_tolerance); + EXPECT_NEAR(*iter_y, expected_pointcloud[i].y(), standard_tolerance); + EXPECT_NEAR(*iter_z, expected_pointcloud[i].z(), standard_tolerance); } if (debug_) { @@ -385,42 +445,38 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithEmptyTwist) } } -TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithEmptyPointCloud) +TEST_F(DistortionCorrectorTest, TestUndistortPointcloudWithEmptyPointCloud) { - rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); + rclcpp::Time timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME); // Generate and process multiple twist messages - auto twist_msgs = generateTwistMsgs(timestamp); - for (const auto & twist_msg : twist_msgs) { - distortion_corrector_2d_->processTwistMessage(twist_msg); - } + generate_and_process_twist_msgs(distortion_corrector_2d_, timestamp); // Generate an empty point cloud message - sensor_msgs::msg::PointCloud2 empty_pointcloud = generatePointCloudMsg(false, false, timestamp); + auto empty_pointcloud = generate_empty_pointcloud_msg(timestamp); // Process empty point cloud distortion_corrector_2d_->initialize(); - distortion_corrector_2d_->undistortPointCloud(true, empty_pointcloud); + distortion_corrector_2d_->undistort_pointcloud(true, std::nullopt, empty_pointcloud); // Verify the point cloud is still empty EXPECT_EQ(empty_pointcloud.width, 0); EXPECT_EQ(empty_pointcloud.row_step, 0); } -TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithoutImuInBaseLink) +TEST_F(DistortionCorrectorTest, TestUndistortPointcloud2dWithoutImuInBaseLink) { // Generate the point cloud message - rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); - sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, timestamp); + rclcpp::Time timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME); + auto [default_points, default_azimuths] = + generate_default_pointcloud(AngleCoordinateSystem::CARTESIAN); + auto pointcloud = generate_pointcloud_msg(false, timestamp, default_points, default_azimuths); // Generate and process multiple twist messages - auto twist_msgs = generateTwistMsgs(timestamp); - for (const auto & twist_msg : twist_msgs) { - distortion_corrector_2d_->processTwistMessage(twist_msg); - } + generate_and_process_twist_msgs(distortion_corrector_2d_, timestamp); // Test using only twist distortion_corrector_2d_->initialize(); - distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link"); - distortion_corrector_2d_->undistortPointCloud(false, pointcloud); + distortion_corrector_2d_->set_pointcloud_transform("base_link", "base_link"); + distortion_corrector_2d_->undistort_pointcloud(false, std::nullopt, pointcloud); sensor_msgs::PointCloud2ConstIterator iter_x(pointcloud, "x"); sensor_msgs::PointCloud2ConstIterator iter_y(pointcloud, "y"); @@ -428,11 +484,11 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithoutImuInBaseLink) // Expected undistorted point cloud values std::array expected_pointcloud = { - {Eigen::Vector3f(10.0f, 0.0f, 0.0f), Eigen::Vector3f(0.117124f, 10.0f, 0.0f), - Eigen::Vector3f(0.26f, 0.000135182f, 10.0f), Eigen::Vector3f(20.4f, 0.0213818f, 0.0f), - Eigen::Vector3f(0.50932f, 20.0005f, 0.0f), Eigen::Vector3f(0.699999f, 0.000819721f, 20.0f), - Eigen::Vector3f(30.8599f, 0.076f, 0.0f), Eigen::Vector3f(0.947959f, 30.0016f, 0.0f), - Eigen::Vector3f(1.22f, 0.00244382f, 30.0f), Eigen::Vector3f(11.3568f, 10.0463f, 10.0f)}}; + {Eigen::Vector3f(0.0f, 0.0f, 0.0f), Eigen::Vector3f(0.12f, 3.45146e-05f, 0.0f), + Eigen::Vector3f(10.26f, 0.00684635f, 1.0f), Eigen::Vector3f(5.40527f, -4.99443f, 2.0f), + Eigen::Vector3f(0.55534f, -9.99949f, 3.0f), Eigen::Vector3f(-4.28992f, -5.00924f, 4.0f), + Eigen::Vector3f(-9.13997f, -0.0237086f, 5.0f), Eigen::Vector3f(-3.97532f, 4.98642f, -5.0f), + Eigen::Vector3f(1.18261f, 10.0024f, -4.0f), Eigen::Vector3f(6.37838f, 5.02475f, -3.0f)}}; // Verify each point in the undistorted point cloud size_t i = 0; @@ -441,9 +497,9 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithoutImuInBaseLink) for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++i) { oss << "Point " << i << ": (" << *iter_x << ", " << *iter_y << ", " << *iter_z << ")\n"; - EXPECT_NEAR(*iter_x, expected_pointcloud[i].x(), standard_tolerance_); - EXPECT_NEAR(*iter_y, expected_pointcloud[i].y(), standard_tolerance_); - EXPECT_NEAR(*iter_z, expected_pointcloud[i].z(), standard_tolerance_); + EXPECT_NEAR(*iter_x, expected_pointcloud[i].x(), standard_tolerance); + EXPECT_NEAR(*iter_y, expected_pointcloud[i].y(), standard_tolerance); + EXPECT_NEAR(*iter_z, expected_pointcloud[i].z(), standard_tolerance); } if (debug_) { @@ -451,27 +507,23 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithoutImuInBaseLink) } } -TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithImuInBaseLink) +TEST_F(DistortionCorrectorTest, TestUndistortPointcloud2dWithImuInBaseLink) { // Generate the point cloud message - rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); - sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, timestamp); + rclcpp::Time timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME); + auto [default_points, default_azimuths] = + generate_default_pointcloud(AngleCoordinateSystem::CARTESIAN); + auto pointcloud = generate_pointcloud_msg(false, timestamp, default_points, default_azimuths); // Generate and process multiple twist messages - auto twist_msgs = generateTwistMsgs(timestamp); - for (const auto & twist_msg : twist_msgs) { - distortion_corrector_2d_->processTwistMessage(twist_msg); - } + generate_and_process_twist_msgs(distortion_corrector_2d_, timestamp); // Generate and process multiple IMU messages - auto imu_msgs = generateImuMsgs(timestamp); - for (const auto & imu_msg : imu_msgs) { - distortion_corrector_2d_->processIMUMessage("base_link", imu_msg); - } + generate_and_process_imu_msgs(distortion_corrector_2d_, timestamp); distortion_corrector_2d_->initialize(); - distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link"); - distortion_corrector_2d_->undistortPointCloud(true, pointcloud); + distortion_corrector_2d_->set_pointcloud_transform("base_link", "base_link"); + distortion_corrector_2d_->undistort_pointcloud(true, std::nullopt, pointcloud); sensor_msgs::PointCloud2ConstIterator iter_x(pointcloud, "x"); sensor_msgs::PointCloud2ConstIterator iter_y(pointcloud, "y"); @@ -479,11 +531,11 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithImuInBaseLink) // Expected undistorted point cloud values std::array expected_pointcloud = { - {Eigen::Vector3f(10.0f, 0.0f, 0.0f), Eigen::Vector3f(0.122876f, 9.99996f, 0.0f), - Eigen::Vector3f(0.26f, -0.000115049f, 10.0f), Eigen::Vector3f(20.4f, -0.0174931f, 0.0f), - Eigen::Vector3f(0.56301f, 19.9996f, 0.0f), Eigen::Vector3f(0.7f, -0.000627014f, 20.0f), - Eigen::Vector3f(30.86f, -0.052675f, 0.0f), Eigen::Vector3f(1.1004f, 29.9987f, 0.0f), - Eigen::Vector3f(1.22f, -0.00166245f, 30.0f), Eigen::Vector3f(11.4249f, 9.97293f, 10.0f)}}; + {Eigen::Vector3f(0.0f, 0.0f, 0.0f), Eigen::Vector3f(0.12f, -3.45146e-05f, 0.0f), + Eigen::Vector3f(10.26f, -0.00586748f, 1.0f), Eigen::Vector3f(5.39568f, -5.00455f, 2.0f), + Eigen::Vector3f(0.528495f, -10.0004f, 3.0f), Eigen::Vector3f(-4.30719f, -4.99343f, 4.0f), + Eigen::Vector3f(-9.13999f, 0.0163541f, 5.0f), Eigen::Vector3f(-3.94992f, 5.0088f, -5.0f), + Eigen::Vector3f(1.24205f, 9.99831f, -4.0f), Eigen::Vector3f(6.41245f, 4.98541f, -3.0f)}}; // Verify each point in the undistorted point cloud size_t i = 0; @@ -492,9 +544,9 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithImuInBaseLink) for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++i) { oss << "Point " << i << ": (" << *iter_x << ", " << *iter_y << ", " << *iter_z << ")\n"; - EXPECT_NEAR(*iter_x, expected_pointcloud[i].x(), standard_tolerance_); - EXPECT_NEAR(*iter_y, expected_pointcloud[i].y(), standard_tolerance_); - EXPECT_NEAR(*iter_z, expected_pointcloud[i].z(), standard_tolerance_); + EXPECT_NEAR(*iter_x, expected_pointcloud[i].x(), standard_tolerance); + EXPECT_NEAR(*iter_y, expected_pointcloud[i].y(), standard_tolerance); + EXPECT_NEAR(*iter_z, expected_pointcloud[i].z(), standard_tolerance); } if (debug_) { @@ -502,27 +554,23 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithImuInBaseLink) } } -TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithImuInLidarFrame) +TEST_F(DistortionCorrectorTest, TestUndistortPointcloud2dWithImuInLidarFrame) { // Generate the point cloud message - rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); - sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, true, timestamp); + rclcpp::Time timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME); + auto [default_points, default_azimuths] = + generate_default_pointcloud(AngleCoordinateSystem::CARTESIAN); + auto pointcloud = generate_pointcloud_msg(true, timestamp, default_points, default_azimuths); // Generate and process multiple twist messages - auto twist_msgs = generateTwistMsgs(timestamp); - for (const auto & twist_msg : twist_msgs) { - distortion_corrector_2d_->processTwistMessage(twist_msg); - } + generate_and_process_twist_msgs(distortion_corrector_2d_, timestamp); // Generate and process multiple IMU messages - auto imu_msgs = generateImuMsgs(timestamp); - for (const auto & imu_msg : imu_msgs) { - distortion_corrector_2d_->processIMUMessage("base_link", imu_msg); - } + generate_and_process_imu_msgs(distortion_corrector_2d_, timestamp); distortion_corrector_2d_->initialize(); - distortion_corrector_2d_->setPointCloudTransform("base_link", "lidar_top"); - distortion_corrector_2d_->undistortPointCloud(true, pointcloud); + distortion_corrector_2d_->set_pointcloud_transform("base_link", "lidar_top"); + distortion_corrector_2d_->undistort_pointcloud(true, std::nullopt, pointcloud); sensor_msgs::PointCloud2ConstIterator iter_x(pointcloud, "x"); sensor_msgs::PointCloud2ConstIterator iter_y(pointcloud, "y"); @@ -530,16 +578,14 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithImuInLidarFrame) // Expected undistorted point cloud values std::array expected_pointcloud = { - {Eigen::Vector3f(10.0f, -1.77636e-15f, -4.44089e-16f), - Eigen::Vector3f(0.049989f, 10.0608f, 0.0924992f), - Eigen::Vector3f(0.106107f, 0.130237f, 10.1986f), - Eigen::Vector3f(20.1709f, 0.210011f, 0.32034f), - Eigen::Vector3f(0.220674f, 20.2734f, 0.417974f), - Eigen::Vector3f(0.274146f, 0.347043f, 20.5341f), - Eigen::Vector3f(30.3673f, 0.457564f, 0.700818f), - Eigen::Vector3f(0.418014f, 30.5259f, 0.807963f), - Eigen::Vector3f(0.464088f, 0.600081f, 30.9292f), - Eigen::Vector3f(10.5657f, 10.7121f, 11.094f)}}; + {Eigen::Vector3f(0.0f, 0.0f, 0.0f), Eigen::Vector3f(0.0512387f, 0.0608269f, 0.0917824f), + Eigen::Vector3f(10.1106f, 0.134026f, 1.20356f), Eigen::Vector3f(5.17128f, -4.79604f, 2.30806f), + Eigen::Vector3f(0.232686f, -9.7275f, 3.40938f), + Eigen::Vector3f(-4.70281f, -4.65034f, 4.52609f), + Eigen::Vector3f(-9.64009f, 0.425434f, 5.64106f), + Eigen::Vector3f(-4.55139f, 5.5241f, -4.21327f), + Eigen::Vector3f(0.519385f, 10.6188f, -3.06522f), + Eigen::Vector3f(5.5992f, 5.71475f, -1.91985f)}}; // Verify each point in the undistorted point cloud size_t i = 0; @@ -548,9 +594,9 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithImuInLidarFrame) for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++i) { oss << "Point " << i << ": (" << *iter_x << ", " << *iter_y << ", " << *iter_z << ")\n"; - EXPECT_NEAR(*iter_x, expected_pointcloud[i].x(), standard_tolerance_); - EXPECT_NEAR(*iter_y, expected_pointcloud[i].y(), standard_tolerance_); - EXPECT_NEAR(*iter_z, expected_pointcloud[i].z(), standard_tolerance_); + EXPECT_NEAR(*iter_x, expected_pointcloud[i].x(), standard_tolerance); + EXPECT_NEAR(*iter_y, expected_pointcloud[i].y(), standard_tolerance); + EXPECT_NEAR(*iter_z, expected_pointcloud[i].z(), standard_tolerance); } if (debug_) { @@ -558,22 +604,21 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithImuInLidarFrame) } } -TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithoutImuInBaseLink) +TEST_F(DistortionCorrectorTest, TestUndistortPointcloud3dWithoutImuInBaseLink) { // Generate the point cloud message - rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); - sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, timestamp); + rclcpp::Time timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME); + auto [default_points, default_azimuths] = + generate_default_pointcloud(AngleCoordinateSystem::CARTESIAN); + auto pointcloud = generate_pointcloud_msg(false, timestamp, default_points, default_azimuths); // Generate and process multiple twist messages - auto twist_msgs = generateTwistMsgs(timestamp); - for (const auto & twist_msg : twist_msgs) { - distortion_corrector_3d_->processTwistMessage(twist_msg); - } + generate_and_process_twist_msgs(distortion_corrector_3d_, timestamp); // Test using only twist distortion_corrector_3d_->initialize(); - distortion_corrector_3d_->setPointCloudTransform("base_link", "base_link"); - distortion_corrector_3d_->undistortPointCloud(false, pointcloud); + distortion_corrector_3d_->set_pointcloud_transform("base_link", "base_link"); + distortion_corrector_3d_->undistort_pointcloud(false, std::nullopt, pointcloud); sensor_msgs::PointCloud2ConstIterator iter_x(pointcloud, "x"); sensor_msgs::PointCloud2ConstIterator iter_y(pointcloud, "y"); @@ -581,11 +626,11 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithoutImuInBaseLink) // Expected undistorted point cloud values std::array expected_pointcloud = { - {Eigen::Vector3f(10.0f, 0.0f, 0.0f), Eigen::Vector3f(0.117f, 10.0f, 0.0f), - Eigen::Vector3f(0.26f, 9.27035e-05f, 10.0f), Eigen::Vector3f(20.4f, 0.0222176f, 0.0f), - Eigen::Vector3f(0.51f, 20.0004f, 0.0f), Eigen::Vector3f(0.7f, 0.000706573f, 20.0f), - Eigen::Vector3f(30.8599f, 0.0760946f, 0.0f), Eigen::Vector3f(0.946998f, 30.0015f, 0.0f), - Eigen::Vector3f(1.22f, 0.00234201f, 30.0f), Eigen::Vector3f(11.3569f, 10.046f, 10.0f)}}; + {Eigen::Vector3f(0.0f, 0.0f, 0.0f), Eigen::Vector3f(0.12f, 2.38419e-05f, 0.0f), + Eigen::Vector3f(10.26f, 0.0070927f, 1.0f), Eigen::Vector3f(5.4055f, -4.99428f, 2.0f), + Eigen::Vector3f(0.555f, -9.99959f, 3.0f), Eigen::Vector3f(-4.28999f, -5.00928f, 4.0f), + Eigen::Vector3f(-9.13997f, -0.0239053f, 5.0f), Eigen::Vector3f(-3.97548f, 4.98614f, -5.0f), + Eigen::Vector3f(1.183f, 10.0023f, -4.0f), Eigen::Vector3f(6.37845f, 5.02458f, -3.0f)}}; // Verify each point in the undistorted point cloud size_t i = 0; @@ -594,9 +639,9 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithoutImuInBaseLink) for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++i) { oss << "Point " << i << ": (" << *iter_x << ", " << *iter_y << ", " << *iter_z << ")\n"; - EXPECT_NEAR(*iter_x, expected_pointcloud[i].x(), standard_tolerance_); - EXPECT_NEAR(*iter_y, expected_pointcloud[i].y(), standard_tolerance_); - EXPECT_NEAR(*iter_z, expected_pointcloud[i].z(), standard_tolerance_); + EXPECT_NEAR(*iter_x, expected_pointcloud[i].x(), standard_tolerance); + EXPECT_NEAR(*iter_y, expected_pointcloud[i].y(), standard_tolerance); + EXPECT_NEAR(*iter_z, expected_pointcloud[i].z(), standard_tolerance); } if (debug_) { @@ -604,27 +649,23 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithoutImuInBaseLink) } } -TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithImuInBaseLink) +TEST_F(DistortionCorrectorTest, TestUndistortPointcloud3dWithImuInBaseLink) { // Generate the point cloud message - rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); - sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, timestamp); + rclcpp::Time timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME); + auto [default_points, default_azimuths] = + generate_default_pointcloud(AngleCoordinateSystem::CARTESIAN); + auto pointcloud = generate_pointcloud_msg(false, timestamp, default_points, default_azimuths); // Generate and process multiple twist messages - auto twist_msgs = generateTwistMsgs(timestamp); - for (const auto & twist_msg : twist_msgs) { - distortion_corrector_3d_->processTwistMessage(twist_msg); - } + generate_and_process_twist_msgs(distortion_corrector_3d_, timestamp); // Generate and process multiple IMU messages - auto imu_msgs = generateImuMsgs(timestamp); - for (const auto & imu_msg : imu_msgs) { - distortion_corrector_3d_->processIMUMessage("base_link", imu_msg); - } + generate_and_process_imu_msgs(distortion_corrector_3d_, timestamp); distortion_corrector_3d_->initialize(); - distortion_corrector_3d_->setPointCloudTransform("base_link", "base_link"); - distortion_corrector_3d_->undistortPointCloud(true, pointcloud); + distortion_corrector_3d_->set_pointcloud_transform("base_link", "base_link"); + distortion_corrector_3d_->undistort_pointcloud(true, std::nullopt, pointcloud); sensor_msgs::PointCloud2ConstIterator iter_x(pointcloud, "x"); sensor_msgs::PointCloud2ConstIterator iter_y(pointcloud, "y"); @@ -632,15 +673,14 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithImuInBaseLink) // Expected undistorted point cloud values std::array expected_pointcloud = { - {Eigen::Vector3f(10.0f, 0.0f, 0.0f), Eigen::Vector3f(0.123015f, 9.99998f, 0.00430552f), - Eigen::Vector3f(0.266103f, -0.00895269f, 9.99992f), - Eigen::Vector3f(20.4f, -0.0176539f, -0.0193392f), - Eigen::Vector3f(0.563265f, 19.9997f, 0.035628f), - Eigen::Vector3f(0.734597f, -0.046068f, 19.9993f), - Eigen::Vector3f(30.8599f, -0.0517931f, -0.0658165f), - Eigen::Vector3f(1.0995f, 29.9989f, 0.0956997f), - Eigen::Vector3f(1.31283f, -0.113544f, 29.9977f), - Eigen::Vector3f(11.461f, 9.93096f, 10.0035f)}}; + {Eigen::Vector3f(0.0f, 0.0f, 0.0f), Eigen::Vector3f(0.12f, -1.86084e-05f, -1.63216e-05f), + Eigen::Vector3f(10.2606f, -0.00683919f, 0.993812f), + Eigen::Vector3f(5.39753f, -5.00722f, 1.9883f), Eigen::Vector3f(0.532273f, -10.0057f, 2.98165f), + Eigen::Vector3f(-4.30025f, -5.0024f, 3.99665f), + Eigen::Vector3f(-9.12918f, 0.00256404f, 5.02064f), + Eigen::Vector3f(-3.96298f, 5.02511f, -4.97218f), + Eigen::Vector3f(1.23005f, 10.0137f, -3.96452f), + Eigen::Vector3f(6.40165f, 4.99868f, -2.99944f)}}; // Verify each point in the undistorted point cloud size_t i = 0; @@ -649,9 +689,9 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithImuInBaseLink) for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++i) { oss << "Point " << i << ": (" << *iter_x << ", " << *iter_y << ", " << *iter_z << ")\n"; - EXPECT_NEAR(*iter_x, expected_pointcloud[i].x(), standard_tolerance_); - EXPECT_NEAR(*iter_y, expected_pointcloud[i].y(), standard_tolerance_); - EXPECT_NEAR(*iter_z, expected_pointcloud[i].z(), standard_tolerance_); + EXPECT_NEAR(*iter_x, expected_pointcloud[i].x(), standard_tolerance); + EXPECT_NEAR(*iter_y, expected_pointcloud[i].y(), standard_tolerance); + EXPECT_NEAR(*iter_z, expected_pointcloud[i].z(), standard_tolerance); } if (debug_) { @@ -659,27 +699,23 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithImuInBaseLink) } } -TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithImuInLidarFrame) +TEST_F(DistortionCorrectorTest, TestUndistortPointcloud3dWithImuInLidarFrame) { // Generate the point cloud message - rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); - sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, true, timestamp); + rclcpp::Time timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME); + auto [default_points, default_azimuths] = + generate_default_pointcloud(AngleCoordinateSystem::CARTESIAN); + auto pointcloud = generate_pointcloud_msg(true, timestamp, default_points, default_azimuths); // Generate and process multiple twist messages - auto twist_msgs = generateTwistMsgs(timestamp); - for (const auto & twist_msg : twist_msgs) { - distortion_corrector_3d_->processTwistMessage(twist_msg); - } + generate_and_process_twist_msgs(distortion_corrector_3d_, timestamp); // Generate and process multiple IMU messages - auto imu_msgs = generateImuMsgs(timestamp); - for (const auto & imu_msg : imu_msgs) { - distortion_corrector_3d_->processIMUMessage("base_link", imu_msg); - } + generate_and_process_imu_msgs(distortion_corrector_3d_, timestamp); distortion_corrector_3d_->initialize(); - distortion_corrector_3d_->setPointCloudTransform("base_link", "lidar_top"); - distortion_corrector_3d_->undistortPointCloud(true, pointcloud); + distortion_corrector_3d_->set_pointcloud_transform("base_link", "lidar_top"); + distortion_corrector_3d_->undistort_pointcloud(true, std::nullopt, pointcloud); sensor_msgs::PointCloud2ConstIterator iter_x(pointcloud, "x"); sensor_msgs::PointCloud2ConstIterator iter_y(pointcloud, "y"); @@ -687,15 +723,13 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithImuInLidarFrame) // Expected undistorted point cloud values std::array expected_pointcloud = { - {Eigen::Vector3f(10.0f, 0.0f, 0.0f), Eigen::Vector3f(0.046484f, 10.0622f, 0.098484f), - Eigen::Vector3f(0.107595f, 0.123767f, 10.2026f), - Eigen::Vector3f(20.1667f, 0.22465f, 0.313351f), - Eigen::Vector3f(0.201149f, 20.2784f, 0.464665f), - Eigen::Vector3f(0.290531f, 0.303489f, 20.5452f), - Eigen::Vector3f(30.3598f, 0.494116f, 0.672914f), - Eigen::Vector3f(0.375848f, 30.5336f, 0.933633f), - Eigen::Vector3f(0.510001f, 0.479651f, 30.9493f), - Eigen::Vector3f(10.5629f, 10.6855f, 11.1461f)}}; + {Eigen::Vector3f(0.0f, 4.76837e-07f, 0.0f), Eigen::Vector3f(0.049716f, 0.0622373f, 0.0935726f), + Eigen::Vector3f(10.1082f, 0.139472f, 1.20323f), Eigen::Vector3f(5.17113f, -4.79225f, 2.30392f), + Eigen::Vector3f(0.23695f, -9.72807f, 3.39875f), + Eigen::Vector3f(-4.70053f, -4.65832f, 4.53053f), + Eigen::Vector3f(-9.64065f, 0.407413f, 5.66885f), Eigen::Vector3f(-4.5738f, 5.5446f, -4.17022f), + Eigen::Vector3f(0.489763f, 10.6448f, -3.00165f), + Eigen::Vector3f(5.57566f, 5.74589f, -1.88189f)}}; // Verify each point in the undistorted point cloud size_t i = 0; @@ -704,9 +738,9 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithImuInLidarFrame) for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++i) { oss << "Point " << i << ": (" << *iter_x << ", " << *iter_y << ", " << *iter_z << ")\n"; - EXPECT_NEAR(*iter_x, expected_pointcloud[i].x(), standard_tolerance_); - EXPECT_NEAR(*iter_y, expected_pointcloud[i].y(), standard_tolerance_); - EXPECT_NEAR(*iter_z, expected_pointcloud[i].z(), standard_tolerance_); + EXPECT_NEAR(*iter_x, expected_pointcloud[i].x(), standard_tolerance); + EXPECT_NEAR(*iter_y, expected_pointcloud[i].y(), standard_tolerance); + EXPECT_NEAR(*iter_z, expected_pointcloud[i].z(), standard_tolerance); } if (debug_) { @@ -714,35 +748,39 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithImuInLidarFrame) } } -TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureLinearMotion) +TEST_F(DistortionCorrectorTest, TestUndistortPointcloudWithPureLinearMotion) { - rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); - sensor_msgs::msg::PointCloud2 test2d_pointcloud = generatePointCloudMsg(true, false, timestamp); - sensor_msgs::msg::PointCloud2 test3d_pointcloud = generatePointCloudMsg(true, false, timestamp); + rclcpp::Time timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME); + auto [default_points, default_azimuths] = + generate_default_pointcloud(AngleCoordinateSystem::CARTESIAN); + auto test2d_pointcloud = + generate_pointcloud_msg(false, timestamp, default_points, default_azimuths); + auto test3d_pointcloud = + generate_pointcloud_msg(false, timestamp, default_points, default_azimuths); // Generate and process a single twist message with constant linear velocity - auto twist_msg = generateTwistMsg(1.0, 0.0, timestamp); + auto twist_msg = generate_twist_msg(1.0, 0.0, timestamp); - distortion_corrector_2d_->processTwistMessage(twist_msg); + distortion_corrector_2d_->process_twist_message(twist_msg); distortion_corrector_2d_->initialize(); - distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link"); - distortion_corrector_2d_->undistortPointCloud(false, test2d_pointcloud); + distortion_corrector_2d_->set_pointcloud_transform("base_link", "base_link"); + distortion_corrector_2d_->undistort_pointcloud(false, std::nullopt, test2d_pointcloud); - distortion_corrector_3d_->processTwistMessage(twist_msg); + distortion_corrector_3d_->process_twist_message(twist_msg); distortion_corrector_3d_->initialize(); - distortion_corrector_3d_->setPointCloudTransform("base_link", "base_link"); - distortion_corrector_3d_->undistortPointCloud(false, test3d_pointcloud); + distortion_corrector_3d_->set_pointcloud_transform("base_link", "base_link"); + distortion_corrector_3d_->undistort_pointcloud(false, std::nullopt, test3d_pointcloud); // Generate expected point cloud for testing - sensor_msgs::msg::PointCloud2 expected_pointcloud_msg = - generatePointCloudMsg(true, false, timestamp); + auto expected_pointcloud = + generate_pointcloud_msg(false, timestamp, default_points, default_azimuths); // Calculate expected point cloud values based on constant linear motion double velocity = 1.0; // 1 m/s linear velocity - sensor_msgs::PointCloud2Iterator iter_x(expected_pointcloud_msg, "x"); - sensor_msgs::PointCloud2Iterator iter_y(expected_pointcloud_msg, "y"); - sensor_msgs::PointCloud2Iterator iter_z(expected_pointcloud_msg, "z"); - sensor_msgs::PointCloud2Iterator iter_t(expected_pointcloud_msg, "time_stamp"); + sensor_msgs::PointCloud2Iterator iter_x(expected_pointcloud, "x"); + sensor_msgs::PointCloud2Iterator iter_y(expected_pointcloud, "y"); + sensor_msgs::PointCloud2Iterator iter_z(expected_pointcloud, "z"); + sensor_msgs::PointCloud2Iterator iter_t(expected_pointcloud, "time_stamp"); std::vector expected_points; for (; iter_t != iter_t.end(); ++iter_t, ++iter_x, ++iter_y, ++iter_z) { @@ -801,37 +839,41 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureLinearMotion) } } -TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureRotationalMotion) +TEST_F(DistortionCorrectorTest, TestUndistortPointcloudWithPureRotationalMotion) { - rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); - sensor_msgs::msg::PointCloud2 test2d_pointcloud = generatePointCloudMsg(true, false, timestamp); - sensor_msgs::msg::PointCloud2 test3d_pointcloud = generatePointCloudMsg(true, false, timestamp); + rclcpp::Time timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME); + auto [default_points, default_azimuths] = + generate_default_pointcloud(AngleCoordinateSystem::CARTESIAN); + auto test2d_pointcloud = + generate_pointcloud_msg(false, timestamp, default_points, default_azimuths); + auto test3d_pointcloud = + generate_pointcloud_msg(false, timestamp, default_points, default_azimuths); // Generate and process a single twist message with constant angular velocity - auto twist_msg = generateTwistMsg(0.0, 0.1, timestamp); + auto twist_msg = generate_twist_msg(0.0, 0.1, timestamp); - distortion_corrector_2d_->processTwistMessage(twist_msg); + distortion_corrector_2d_->process_twist_message(twist_msg); distortion_corrector_2d_->initialize(); - distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link"); - distortion_corrector_2d_->undistortPointCloud(false, test2d_pointcloud); + distortion_corrector_2d_->set_pointcloud_transform("base_link", "base_link"); + distortion_corrector_2d_->undistort_pointcloud(false, std::nullopt, test2d_pointcloud); - distortion_corrector_3d_->processTwistMessage(twist_msg); + distortion_corrector_3d_->process_twist_message(twist_msg); distortion_corrector_3d_->initialize(); - distortion_corrector_3d_->setPointCloudTransform("base_link", "base_link"); - distortion_corrector_3d_->undistortPointCloud(false, test3d_pointcloud); + distortion_corrector_3d_->set_pointcloud_transform("base_link", "base_link"); + distortion_corrector_3d_->undistort_pointcloud(false, std::nullopt, test3d_pointcloud); // Generate expected point cloud for testing - sensor_msgs::msg::PointCloud2 expected_pointcloud_msg = - generatePointCloudMsg(true, false, timestamp); + auto expected_pointcloud = + generate_pointcloud_msg(false, timestamp, default_points, default_azimuths); // Calculate expected point cloud values based on constant rotational motion double angular_velocity = 0.1; // 0.1 rad/s rotational velocity - sensor_msgs::PointCloud2Iterator iter_x(expected_pointcloud_msg, "x"); - sensor_msgs::PointCloud2Iterator iter_y(expected_pointcloud_msg, "y"); - sensor_msgs::PointCloud2Iterator iter_z(expected_pointcloud_msg, "z"); - sensor_msgs::PointCloud2Iterator iter_t(expected_pointcloud_msg, "time_stamp"); + sensor_msgs::PointCloud2Iterator iter_x(expected_pointcloud, "x"); + sensor_msgs::PointCloud2Iterator iter_y(expected_pointcloud, "y"); + sensor_msgs::PointCloud2Iterator iter_z(expected_pointcloud, "z"); + sensor_msgs::PointCloud2Iterator iter_t(expected_pointcloud, "time_stamp"); - std::vector expected_pointcloud; + std::vector expected_points; for (; iter_t != iter_t.end(); ++iter_t, ++iter_x, ++iter_y, ++iter_z) { double time_offset = static_cast(*iter_t) / 1e9; float angle = angular_velocity * time_offset; @@ -844,7 +886,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureRotationalMotion) tf2::Vector3 point(*iter_x, *iter_y, *iter_z); tf2::Vector3 rotated_point = tf2::quatRotate(quaternion, point); - expected_pointcloud.emplace_back( + expected_points.emplace_back( static_cast(rotated_point.x()), static_cast(rotated_point.y()), static_cast(rotated_point.z())); } @@ -862,9 +904,9 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureRotationalMotion) ++test2d_iter_x, ++test2d_iter_y, ++test2d_iter_z, ++i) { oss << "Point " << i << ": (" << *test2d_iter_x << ", " << *test2d_iter_y << ", " << *test2d_iter_z << ")\n"; - EXPECT_FLOAT_EQ(*test2d_iter_x, expected_pointcloud[i].x()); - EXPECT_FLOAT_EQ(*test2d_iter_y, expected_pointcloud[i].y()); - EXPECT_FLOAT_EQ(*test2d_iter_z, expected_pointcloud[i].z()); + EXPECT_FLOAT_EQ(*test2d_iter_x, expected_points[i].x()); + EXPECT_FLOAT_EQ(*test2d_iter_y, expected_points[i].y()); + EXPECT_FLOAT_EQ(*test2d_iter_z, expected_points[i].z()); } if (debug_) { @@ -891,9 +933,9 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureRotationalMotion) << *test2d_iter_z << ")" << " vs 3D: (" << *test3d_iter_x << ", " << *test3d_iter_y << ", " << *test3d_iter_z << ")\n"; - EXPECT_NEAR(*test2d_iter_x, *test3d_iter_x, coarse_tolerance_); - EXPECT_NEAR(*test2d_iter_y, *test3d_iter_y, coarse_tolerance_); - EXPECT_NEAR(*test2d_iter_z, *test3d_iter_z, coarse_tolerance_); + EXPECT_NEAR(*test2d_iter_x, *test3d_iter_x, coarse_tolerance); + EXPECT_NEAR(*test2d_iter_y, *test3d_iter_y, coarse_tolerance); + EXPECT_NEAR(*test2d_iter_z, *test3d_iter_z, coarse_tolerance); } if (debug_) { @@ -901,6 +943,341 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureRotationalMotion) } } +TEST_F(DistortionCorrectorTest, TestUndistortPointcloudNotUpdatingAzimuthAndDistance) +{ + // Test the case when the cloud will not update the azimuth and distance values + // 1. when pointcloud is in base_link (pointcloud_transform_needed() is false) + + // Generate the point cloud message in base_link + rclcpp::Time timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME); + auto [default_points, default_azimuths] = + generate_default_pointcloud(AngleCoordinateSystem::CARTESIAN); + auto pointcloud_base_link = + generate_pointcloud_msg(false, timestamp, default_points, default_azimuths); + + // Generate and process multiple twist messages + generate_and_process_twist_msgs(distortion_corrector_2d_, timestamp); + + // Generate and process multiple IMU messages + generate_and_process_imu_msgs(distortion_corrector_2d_, timestamp); + + distortion_corrector_2d_->initialize(); + distortion_corrector_2d_->set_pointcloud_transform("base_link", "base_link"); + auto angle_conversion_opt = + distortion_corrector_2d_->try_compute_angle_conversion(pointcloud_base_link); + + // Test for expected runtime error + EXPECT_THROW( + { + distortion_corrector_2d_->undistort_pointcloud( + true, angle_conversion_opt, pointcloud_base_link); + }, + std::runtime_error); + + // Test the case when the cloud will not update the azimuth and distance values + // 2. when the return value of try_compute_angle_conversion is std::nullopt (couldn't find the + // angle conversion) + + // Generate the point cloud message in sensor frame + auto pointcloud_lidar_top = + generate_pointcloud_msg(true, timestamp, default_points, default_azimuths); + + // Generate and process multiple twist messages + generate_and_process_twist_msgs(distortion_corrector_2d_, timestamp); + + // Generate and process multiple IMU messages + generate_and_process_imu_msgs(distortion_corrector_2d_, timestamp); + + distortion_corrector_2d_->initialize(); + distortion_corrector_2d_->set_pointcloud_transform("base_link", "lidar_top"); + + angle_conversion_opt = std::nullopt; + distortion_corrector_2d_->undistort_pointcloud(true, angle_conversion_opt, pointcloud_lidar_top); + + auto original_pointcloud_lidar_top = + generate_pointcloud_msg(true, timestamp, default_points, default_azimuths); + + sensor_msgs::PointCloud2ConstIterator test_iter_azimuth_lidar_top( + pointcloud_lidar_top, "azimuth"); + sensor_msgs::PointCloud2ConstIterator test_iter_distance_lidar_top( + pointcloud_lidar_top, "distance"); + + sensor_msgs::PointCloud2ConstIterator original_iter_azimuth_lidar_top( + original_pointcloud_lidar_top, "azimuth"); + sensor_msgs::PointCloud2ConstIterator original_iter_distance_lidar_top( + original_pointcloud_lidar_top, "distance"); + + size_t i = 0; + std::ostringstream oss; + + oss << "Expected pointcloud:\n"; + for (; test_iter_azimuth_lidar_top != test_iter_azimuth_lidar_top.end(); + ++test_iter_azimuth_lidar_top, ++test_iter_distance_lidar_top, + ++original_iter_azimuth_lidar_top, ++original_iter_distance_lidar_top, ++i) { + oss << "Point " << i << " - Output azimuth and distance: (" << *test_iter_azimuth_lidar_top + << ", " << *test_iter_distance_lidar_top << ")" + << " vs Original azimuth and distance: (" << *original_iter_azimuth_lidar_top << ", " + << *original_iter_distance_lidar_top << ")\n"; + + EXPECT_FLOAT_EQ(*test_iter_azimuth_lidar_top, *original_iter_azimuth_lidar_top); + EXPECT_FLOAT_EQ(*test_iter_distance_lidar_top, *original_iter_distance_lidar_top); + } + + if (debug_) { + RCLCPP_INFO(node_->get_logger(), "%s", oss.str().c_str()); + } +} + +TEST_F(DistortionCorrectorTest, TestUndistortPointcloudUpdateAzimuthAndDistanceInVelodyne) +{ + // Generate the point cloud message in sensor frame + rclcpp::Time timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME); + auto [default_points, default_azimuths] = + generate_default_pointcloud(AngleCoordinateSystem::VELODYNE); + auto pointcloud = generate_pointcloud_msg(true, timestamp, default_points, default_azimuths); + + // Generate and process multiple twist messages + generate_and_process_twist_msgs(distortion_corrector_2d_, timestamp); + + // Generate and process multiple IMU messages + generate_and_process_imu_msgs(distortion_corrector_2d_, timestamp); + + distortion_corrector_2d_->initialize(); + distortion_corrector_2d_->set_pointcloud_transform("base_link", "lidar_top"); + + auto angle_conversion_opt = distortion_corrector_2d_->try_compute_angle_conversion(pointcloud); + distortion_corrector_2d_->undistort_pointcloud(true, angle_conversion_opt, pointcloud); + + auto original_pointcloud = + generate_pointcloud_msg(true, timestamp, default_points, default_azimuths); + + sensor_msgs::PointCloud2ConstIterator iter_azimuth(pointcloud, "azimuth"); + sensor_msgs::PointCloud2ConstIterator iter_distance(pointcloud, "distance"); + + // Expected undistorted azimuth and distance values + std::array, 10> expected_azimuth_distance = {{ + {0.0f, 0.0f}, // points: (0.0f, 0.0f, 0.0f) + {5.41248f, 0.121447f}, // points: (0.0512387f, 0.0608269f, 0.0917824f) + {6.26993f, 10.1829f}, // points: (10.1106f, 0.134026f, 1.20356f) + {0.747926f, 7.421f}, // points: (5.17128f, -4.79604f, 2.30806f) + {1.54689f, 10.3103f}, // points: (0.232686f, -9.7275f, 3.40938f) + {2.36187f, 8.01421f}, // points: (-4.70281f, -4.65034f, 4.52609f) + {3.18569f, 11.1774f}, // points: (-9.64009f, 0.425434f, 5.64106f) + {4.02323f, 8.30557f}, // points: (-4.55139f, 5.5241f, -4.21327f) + {4.76125f, 11.0645f}, // points: (0.519385f, 10.6188f, -3.06522f) + {5.48757f, 8.22771f} // points: (5.5992f, 5.71475f, -1.91985f) + }}; + + size_t i = 0; + std::ostringstream oss; + + oss << "Expected pointcloud:\n"; + for (; iter_azimuth != iter_azimuth.end(); ++iter_azimuth, ++iter_distance, ++i) { + oss << "Point " << i << " - Output azimuth and distance: (" << *iter_azimuth << ", " + << *iter_distance << ")" + << " vs Expected azimuth and distance: (" << expected_azimuth_distance[i][0] << ", " + << expected_azimuth_distance[i][1] << ")\n"; + + EXPECT_NEAR(*iter_azimuth, expected_azimuth_distance[i][0], standard_tolerance); + EXPECT_NEAR(*iter_distance, expected_azimuth_distance[i][1], standard_tolerance); + } + + if (debug_) { + RCLCPP_INFO(node_->get_logger(), "%s", oss.str().c_str()); + } +} + +TEST_F(DistortionCorrectorTest, TestUndistortPointcloudUpdateAzimuthAndDistanceInHesai) +{ + // Generate the point cloud message in sensor frame + rclcpp::Time timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME); + + auto [default_points, default_azimuths] = + generate_default_pointcloud(AngleCoordinateSystem::HESAI); + auto pointcloud = generate_pointcloud_msg(true, timestamp, default_points, default_azimuths); + + // Generate and process multiple twist messages + generate_and_process_twist_msgs(distortion_corrector_2d_, timestamp); + + // Generate and process multiple IMU messages + generate_and_process_imu_msgs(distortion_corrector_2d_, timestamp); + + distortion_corrector_2d_->initialize(); + distortion_corrector_2d_->set_pointcloud_transform("base_link", "lidar_top"); + + auto angle_conversion_opt = distortion_corrector_2d_->try_compute_angle_conversion(pointcloud); + distortion_corrector_2d_->undistort_pointcloud(true, angle_conversion_opt, pointcloud); + + auto original_pointcloud = + generate_pointcloud_msg(true, timestamp, default_points, default_azimuths); + + sensor_msgs::PointCloud2ConstIterator iter_azimuth(pointcloud, "azimuth"); + sensor_msgs::PointCloud2ConstIterator iter_distance(pointcloud, "distance"); + + // Expected undistorted azimuth and distance values + std::array, 10> expected_azimuth_distance = {{ + {1.5708f, 0.0f}, // points: (0.0f, 0.0f, 0.0f) + {0.70009f, 0.121447f}, // points: (0.0512387f, 0.0608269f, 0.0917824f) + {1.55754f, 10.1829f}, // points: (10.1106f, 0.134026f, 1.20356f) + {2.31872f, 7.421f}, // points: (5.17128f, -4.79604f, 2.30806f) + {3.11768f, 10.3103f}, // points: (0.232686f, -9.7275f, 3.40938f) + {3.93267f, 8.01421f}, // points: (-4.70281f, -4.65034f, 4.52609f) + {4.75648f, 11.1774f}, // points: (-9.64009f, 0.425434f, 5.64106f) + {5.59403f, 8.30557f}, // points: (-4.55139f, 5.5241f, -4.21327f) + {0.0488634f, 11.0645f}, // points: (0.519385f, 10.6188f, -3.06522f) + {0.775183f, 8.22771f} // points: (5.5992f, 5.71475f, -1.91985f) + }}; + + size_t i = 0; + std::ostringstream oss; + + oss << "Expected pointcloud:\n"; + for (; iter_azimuth != iter_azimuth.end(); ++iter_azimuth, ++iter_distance, ++i) { + oss << "Point " << i << " - Output azimuth and distance: (" << *iter_azimuth << ", " + << *iter_distance << ")" + << " vs Expected azimuth and distance: (" << expected_azimuth_distance[i][0] << ", " + << expected_azimuth_distance[i][1] << ")\n"; + + EXPECT_NEAR(*iter_azimuth, expected_azimuth_distance[i][0], standard_tolerance); + EXPECT_NEAR(*iter_distance, expected_azimuth_distance[i][1], standard_tolerance); + } + + if (debug_) { + RCLCPP_INFO(node_->get_logger(), "%s", oss.str().c_str()); + } +} + +TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionOnEmptyPointcloud) +{ + // test empty pointcloud + rclcpp::Time timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME); + + auto empty_pointcloud = generate_empty_pointcloud_msg(timestamp); + auto angle_conversion_opt = + distortion_corrector_2d_->try_compute_angle_conversion(empty_pointcloud); + + EXPECT_FALSE(angle_conversion_opt.has_value()); +} + +TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionOnVelodynePointcloud) +{ + // test velodyne pointcloud (x-axis: 0 degree, y-axis: 270 degree) + rclcpp::Time timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME); + + std::vector velodyne_points = { + Eigen::Vector3f(0.0f, 0.0f, 0.0f), + Eigen::Vector3f(1.0f, -1.0f, 1.0f), + Eigen::Vector3f(0.0f, -2.0f, 1.0f), + }; + std::vector velodyne_azimuths = { + 0.0f, autoware::universe_utils::pi / 4, autoware::universe_utils::pi / 2}; + + auto velodyne_pointcloud = + generate_pointcloud_msg(true, timestamp, velodyne_points, velodyne_azimuths); + auto angle_conversion_opt = + distortion_corrector_2d_->try_compute_angle_conversion(velodyne_pointcloud); + EXPECT_TRUE(angle_conversion_opt.has_value()); + + EXPECT_EQ(angle_conversion_opt->sign, -1); + EXPECT_NEAR(angle_conversion_opt->offset_rad, 0, standard_tolerance); +} + +TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionOnHesaiPointcloud) +{ + // test hesai pointcloud (x-axis: 90 degree, y-axis: 0 degree) + rclcpp::Time timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME); + std::vector hesai_points = { + Eigen::Vector3f(0.0f, 0.0f, 0.0f), + Eigen::Vector3f(1.0f, -1.0f, 1.0f), + Eigen::Vector3f(0.0f, -2.0f, 1.0f), + }; + std::vector hesai_azimuths = { + autoware::universe_utils::pi / 2, autoware::universe_utils::pi * 3 / 4, + autoware::universe_utils::pi}; + + auto hesai_pointcloud = generate_pointcloud_msg(true, timestamp, hesai_points, hesai_azimuths); + auto angle_conversion_opt = + distortion_corrector_2d_->try_compute_angle_conversion(hesai_pointcloud); + + EXPECT_TRUE(angle_conversion_opt.has_value()); + EXPECT_EQ(angle_conversion_opt->sign, -1); + EXPECT_NEAR( + angle_conversion_opt->offset_rad, autoware::universe_utils::pi / 2, standard_tolerance); +} + +TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionCartesianPointcloud) +{ + // test pointcloud that use cartesian coordinate for azimuth (x-axis: 0 degree, y-axis: 90 degree) + rclcpp::Time timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME); + // Generate and process multiple twist messages + generate_and_process_twist_msgs(distortion_corrector_2d_, timestamp); + + std::vector cartesian_points = { + Eigen::Vector3f(0.0f, 0.0f, 0.0f), + Eigen::Vector3f(1.0f, 1.0f, 1.0f), + Eigen::Vector3f(0.0f, 2.0f, 1.0f), + }; + std::vector cartesian_azimuths = { + 0, autoware::universe_utils::pi / 4, autoware::universe_utils::pi / 2}; + + auto cartesian_pointcloud = + generate_pointcloud_msg(true, timestamp, cartesian_points, cartesian_azimuths); + auto angle_conversion_opt = + distortion_corrector_2d_->try_compute_angle_conversion(cartesian_pointcloud); + + EXPECT_TRUE(angle_conversion_opt.has_value()); + EXPECT_EQ(angle_conversion_opt->sign, 1); + EXPECT_NEAR(angle_conversion_opt->offset_rad, 0, standard_tolerance); +} + +TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionOnRandomPointcloud) +{ + // test pointcloud that use coordinate (x-axis: 270 degree, y-axis: 0 degree) + rclcpp::Time timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME); + // Generate and process multiple twist messages + generate_and_process_twist_msgs(distortion_corrector_2d_, timestamp); + + std::vector points = { + Eigen::Vector3f(0.0f, 1.0f, 0.0f), + Eigen::Vector3f(2.0f, 0.0f, 1.0f), + Eigen::Vector3f(1.0f, 1.0f, 1.0f), + }; + std::vector azimuths = { + 0, autoware::universe_utils::pi * 3 / 2, autoware::universe_utils::pi * 7 / 4}; + + auto pointcloud = generate_pointcloud_msg(true, timestamp, points, azimuths); + auto angle_conversion_opt = distortion_corrector_2d_->try_compute_angle_conversion(pointcloud); + + EXPECT_TRUE(angle_conversion_opt.has_value()); + EXPECT_EQ(angle_conversion_opt->sign, 1); + EXPECT_NEAR( + angle_conversion_opt->offset_rad, autoware::universe_utils::pi * 3 / 2, standard_tolerance); +} + +TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionOnBadAzimuthPointcloud) +{ + // test pointcloud that can cause the angle conversion to fail. + // 1. angle difference is 0 + // 2. azimuth value is wrong + rclcpp::Time timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME); + // Generate and process multiple twist messages + generate_and_process_twist_msgs(distortion_corrector_2d_, timestamp); + + std::vector points = { + Eigen::Vector3f(0.0f, 1.0f, 0.0f), + Eigen::Vector3f(2.0f, 0.0f, 1.0f), + Eigen::Vector3f(1.0f, 1.0f, 1.0f), + }; + + // generate random bad azimuths + std::vector azimuths = {0, 0, autoware::universe_utils::pi}; + + auto pointcloud = generate_pointcloud_msg(true, timestamp, points, azimuths); + auto angle_conversion_opt = distortion_corrector_2d_->try_compute_angle_conversion(pointcloud); + + EXPECT_FALSE(angle_conversion_opt.has_value()); +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv);