From 5f85981407db223d752c261f177b42dc18ffdb50 Mon Sep 17 00:00:00 2001 From: vividf Date: Thu, 16 May 2024 12:21:06 +0900 Subject: [PATCH 01/47] add support for 3d distortion correction Signed-off-by: vividf --- .../pointcloud_preprocessor/CMakeLists.txt | 3 + .../docs/distortion-corrector.md | 40 ++--- .../distortion_corrector.hpp | 11 +- .../distortion_corrector.cpp | 158 ++++++++++++++---- 4 files changed, 160 insertions(+), 52 deletions(-) diff --git a/sensing/pointcloud_preprocessor/CMakeLists.txt b/sensing/pointcloud_preprocessor/CMakeLists.txt index 0cbb04345477e..0f44629ab211e 100644 --- a/sensing/pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/pointcloud_preprocessor/CMakeLists.txt @@ -9,6 +9,7 @@ set(CGAL_DATA_DIR ".") find_package(OpenCV REQUIRED) find_package(Eigen3 REQUIRED) +find_package(Sophus REQUIRED) find_package(Boost REQUIRED) find_package(PCL REQUIRED) find_package(CGAL REQUIRED COMPONENTS Core) @@ -19,6 +20,7 @@ include_directories( ${Boost_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} + ${Sophus_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} ) @@ -86,6 +88,7 @@ target_link_libraries(pointcloud_preprocessor_filter faster_voxel_grid_downsample_filter ${Boost_LIBRARIES} ${OpenCV_LIBRARIES} + ${Sophus_LIBRARIES} ${PCL_LIBRARIES} ) diff --git a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md index dcb03cc792cae..805bd29f87b73 100644 --- a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md +++ b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md @@ -2,20 +2,17 @@ ## Purpose -The `distortion_corrector` is a node that compensates pointcloud distortion caused by ego vehicle's movement during 1 scan. +The `distortion_corrector` is a node that compensates for pointcloud distortion caused by the ego-vehicle's movement during 1 scan. -Since the LiDAR sensor scans by rotating an internal laser, the resulting point cloud will be distorted if the ego-vehicle moves during a single scan (as shown by the figure below). The node corrects this by interpolating sensor data using odometry of ego-vehicle. +Since the LiDAR sensor scans by rotating an internal laser, the resulting point cloud will be distorted if the ego-vehicle moves during a single scan (as shown by the figure below). The node corrects this by interpolating sensor data using the odometry of the ego-vehicle. ## Inner-workings / Algorithms -- Use the equations below (specific to the Velodyne 32C sensor) to obtain an accurate timestamp for each scan data point. -- Use twist information to determine the distance the ego-vehicle has traveled between the time that the scan started and the corrected timestamp of each point, and then correct the position of the point. +The node utilizes twist information (linear velocity and angular velocity) from the `~/input/twist` topic. If the user sets `use_imu` to true, the node will replace the twist's angular velocity with the angular velocity from IMU. Afterward, the node will undistort all of the points one by one based on the velocity information. -The offset equation is given by -$ TimeOffset = (55.296 \mu s _SequenceIndex) + (2.304 \mu s_ DataPointIndex) $ +The node also supports two different kinds of distortion methods: 2D distortion correction and 3D distortion correction. The main difference is that the 2D distortion corrector only utilizes the x-axis of linear velocity and the z-axis of angular velocity to correct the points. On the other hand, the 3D distortion corrector utilizes all linear and angular velocity components to correct the points. -To calculate the exact point time, add the TimeOffset to the timestamp. -$ ExactPointTime = TimeStamp + TimeOffset $ +Please note that the processing time difference between the two distortion methods is significant; the 3D corrector takes 50% more time than the 2D corrector. Therefore, it is recommended that in general cases, users should set `use_3d_distortion_correction` to `false`. However, in scenarios such as a vehicle going over speed bumps, using the 3D corrector can be beneficial. ![distortion corrector figure](./image/distortion_corrector.jpg) @@ -23,25 +20,28 @@ $ ExactPointTime = TimeStamp + TimeOffset $ ### Input -| Name | Type | Description | -| ---------------- | ------------------------------------------------ | ---------------- | -| `~/input/points` | `sensor_msgs::msg::PointCloud2` | reference points | -| `~/input/twist` | `geometry_msgs::msg::TwistWithCovarianceStamped` | twist | -| `~/input/imu` | `sensor_msgs::msg::Imu` | imu data | +| Name | Type | Description | +| ---------------- | ------------------------------------------------ | ---------------------------------- | +| `~/input/points` | `sensor_msgs::msg::PointCloud2` | Topic of the distorted pointcloud. | +| `~/input/twist` | `geometry_msgs::msg::TwistWithCovarianceStamped` | Topic of the twist information. | +| `~/input/imu` | `sensor_msgs::msg::Imu` | Topic of the IMU data. | ### Output -| Name | Type | Description | -| ----------------- | ------------------------------- | --------------- | -| `~/output/points` | `sensor_msgs::msg::PointCloud2` | filtered points | +| Name | Type | Description | +| ----------------- | ------------------------------- | ----------------------------------- | +| `~/output/points` | `sensor_msgs::msg::PointCloud2` | Topic of the undistorted pointcloud | ## Parameters ### Core Parameters -| Name | Type | Default Value | Description | -| ---------------------- | ------ | ------------- | ----------------------------------------------------------- | -| `timestamp_field_name` | string | "time_stamp" | time stamp field name | -| `use_imu` | bool | true | use gyroscope for yaw rate if true, else use vehicle status | +| Name | Type | Default Value | Description | +| ------------------------------ | -------- | ------------- | ----------------------------------------------------------- | +| `timestamp_field_name` | `string` | `time_stamp` | Name of time stamp field. | +| `use_imu` | `bool` | `true` | Use gyroscope for yaw rate if true, else use vehicle status | +| `use_3d_distortion_correction` | `bool` | | Use 3d correction if true, otherwise use 2d correction | ## Assumptions / Known limits + +- The node requires that time synchronization works well between the pointcloud, twist, and IMU. diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp index 618957a2ac783..bc81c854adf93 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp @@ -15,7 +15,9 @@ #ifndef POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_HPP_ #define POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_HPP_ +#include #include +#include #include #include @@ -58,11 +60,17 @@ class DistortionCorrectorComponent : public rclcpp::Node void onTwistWithCovarianceStamped( const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg); void onImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg); + bool getTransform( const std::string & target_frame, const std::string & source_frame, tf2::Transform * tf2_transform_ptr); + bool getTransform( + const std::string & target_frame, const std::string & source_frame, + tf2::Transform * tf2_transform_ptr, Eigen::Matrix4f & eigen_transform); - bool undistortPointCloud(const tf2::Transform & tf2_base_link_to_sensor, PointCloud2 & points); + bool undistortPointCloud( + const tf2::Transform & tf2_base_link_to_sensor, Eigen::Matrix4f & eigen_base_link_to_sensor, + PointCloud2 & points); rclcpp::Subscription::SharedPtr input_points_sub_; rclcpp::Subscription::SharedPtr imu_sub_; @@ -81,6 +89,7 @@ class DistortionCorrectorComponent : public rclcpp::Node std::string base_link_frame_ = "base_link"; std::string time_stamp_field_name_; bool use_imu_; + bool use_3d_distortion_correction_; }; } // namespace pointcloud_preprocessor diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index b90a64dc47c91..feeb7c76dac1a 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -16,6 +16,8 @@ #include "tier4_autoware_utils/math/trigonometry.hpp" +#include + #include #include #include @@ -39,6 +41,7 @@ DistortionCorrectorComponent::DistortionCorrectorComponent(const rclcpp::NodeOpt // Parameter time_stamp_field_name_ = declare_parameter("time_stamp_field_name", "time_stamp"); use_imu_ = declare_parameter("use_imu", true); + use_3d_distortion_correction_ = declare_parameter("use_3d_distortion_correction"); // Publisher undistorted_points_pub_ = @@ -123,10 +126,13 @@ void DistortionCorrectorComponent::onPointCloud(PointCloud2::UniquePtr points_ms return; } + Eigen::Matrix4f eigen_base_link_to_sensor; tf2::Transform tf2_base_link_to_sensor{}; - getTransform(points_msg->header.frame_id, base_link_frame_, &tf2_base_link_to_sensor); + getTransform( + points_msg->header.frame_id, base_link_frame_, &tf2_base_link_to_sensor, + eigen_base_link_to_sensor); - undistortPointCloud(tf2_base_link_to_sensor, *points_msg); + undistortPointCloud(tf2_base_link_to_sensor, eigen_base_link_to_sensor, *points_msg); if (debug_publisher_) { auto pipeline_latency_ms = @@ -151,6 +157,35 @@ void DistortionCorrectorComponent::onPointCloud(PointCloud2::UniquePtr points_ms } } +bool DistortionCorrectorComponent::getTransform( + const std::string & target_frame, const std::string & source_frame, + tf2::Transform * tf2_transform_ptr, Eigen::Matrix4f & eigen_transform) +{ + if (target_frame == source_frame) { + tf2_transform_ptr->setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); + tf2_transform_ptr->setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0)); + eigen_transform = Eigen::Matrix4f::Identity(); + return true; + } + + try { + const auto transform_msg = + tf2_buffer_.lookupTransform(target_frame, source_frame, tf2::TimePointZero); + tf2::convert(transform_msg.transform, *tf2_transform_ptr); + eigen_transform = tf2::transformToEigen(transform_msg.transform).matrix().cast(); + } catch (const tf2::TransformException & ex) { + RCLCPP_WARN(get_logger(), "%s", ex.what()); + RCLCPP_ERROR( + get_logger(), "Please publish TF %s to %s", target_frame.c_str(), source_frame.c_str()); + + tf2_transform_ptr->setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); + tf2_transform_ptr->setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0)); + eigen_transform = Eigen::Matrix4f::Identity(); + return false; + } + return true; +} + bool DistortionCorrectorComponent::getTransform( const std::string & target_frame, const std::string & source_frame, tf2::Transform * tf2_transform_ptr) @@ -178,7 +213,8 @@ bool DistortionCorrectorComponent::getTransform( } bool DistortionCorrectorComponent::undistortPointCloud( - const tf2::Transform & tf2_base_link_to_sensor, PointCloud2 & points) + const tf2::Transform & tf2_base_link_to_sensor, Eigen::Matrix4f & eigen_base_link_to_sensor, + PointCloud2 & points) { if (points.data.empty() || twist_queue_.empty()) { RCLCPP_WARN_STREAM_THROTTLE( @@ -204,9 +240,6 @@ bool DistortionCorrectorComponent::undistortPointCloud( sensor_msgs::PointCloud2Iterator it_z(points, "z"); sensor_msgs::PointCloud2ConstIterator it_time_stamp(points, time_stamp_field_name_); - float theta{0.0f}; - float x{0.0f}; - float y{0.0f}; double prev_time_stamp_sec{*it_time_stamp}; const double first_point_time_stamp_sec{*it_time_stamp}; @@ -229,6 +262,7 @@ bool DistortionCorrectorComponent::undistortPointCloud( } const tf2::Transform tf2_base_link_to_sensor_inv{tf2_base_link_to_sensor.inverse()}; + const Eigen::Matrix4f eigen_base_link_to_sensor_inv = eigen_base_link_to_sensor.inverse(); // For performance, do not instantiate `rclcpp::Time` inside of the for-loop double twist_stamp = rclcpp::Time(twist_it->header.stamp).seconds(); @@ -252,19 +286,52 @@ bool DistortionCorrectorComponent::undistortPointCloud( bool twist_time_stamp_is_too_late = false; bool imu_time_stamp_is_too_late = false; + // for 2d motion + float theta{0.0f}; + float x{0.0f}; + float y{0.0f}; + + // for 3d motion + Eigen::Vector4f point_eigen; + Eigen::Vector4f undistorted_point_eigen; + Eigen::Matrix4f transformation_matrix; + Eigen::Matrix4f prev_transformation_matrix = Eigen::Matrix4f::Identity(); + + 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; // 3d motion + float v = 0.0f, w = 0.0f; // 2d motion + for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_time_stamp) { while (twist_it != std::end(twist_queue_) - 1 && *it_time_stamp > twist_stamp) { ++twist_it; twist_stamp = rclcpp::Time(twist_it->header.stamp).seconds(); } - float v{static_cast(twist_it->twist.linear.x)}; - float w{static_cast(twist_it->twist.angular.z)}; + if (use_3d_distortion_correction_) { + v_x = static_cast(twist_it->twist.linear.x); + v_y = static_cast(twist_it->twist.linear.y); + v_z = static_cast(twist_it->twist.linear.z); + w_x = static_cast(twist_it->twist.angular.x); + w_y = static_cast(twist_it->twist.angular.y); + w_z = static_cast(twist_it->twist.angular.z); + } else { + v = static_cast(twist_it->twist.linear.x); + w = static_cast(twist_it->twist.angular.z); + } if (std::abs(*it_time_stamp - twist_stamp) > 0.1) { twist_time_stamp_is_too_late = true; - v = 0.0f; - w = 0.0f; + + if (use_3d_distortion_correction_) { + v_x = 0.0f; + v_y = 0.0f; + v_z = 0.0f; + w_x = 0.0f; + w_y = 0.0f; + w_z = 0.0f; + } else { + v = 0.0f; + w = 0.0f; + } } if (use_imu_ && !angular_velocity_queue_.empty()) { @@ -276,38 +343,67 @@ bool DistortionCorrectorComponent::undistortPointCloud( if (std::abs(*it_time_stamp - imu_stamp) > 0.1) { imu_time_stamp_is_too_late = true; } else { - w = static_cast(imu_it->vector.z); + if (use_3d_distortion_correction_) { + w_x = static_cast(imu_it->vector.x); + w_y = static_cast(imu_it->vector.y); + w_z = static_cast(imu_it->vector.z); + } else { + w = static_cast(imu_it->vector.z); + } } } const auto time_offset = static_cast(*it_time_stamp - prev_time_stamp_sec); - point.setValue(*it_x, *it_y, *it_z); + if (use_3d_distortion_correction_) { + point_eigen << *it_x, *it_y, *it_z, 1.0; - if (need_transform) { - point = tf2_base_link_to_sensor_inv * point; - } + if (need_transform) { + point_eigen = eigen_base_link_to_sensor_inv * point_eigen; + } - theta += w * time_offset; - baselink_quat.setValue( - 0, 0, tier4_autoware_utils::sin(theta * 0.5f), - tier4_autoware_utils::cos(theta * 0.5f)); // baselink_quat.setRPY(0.0, 0.0, theta); - const float dis = v * time_offset; - x += dis * tier4_autoware_utils::cos(theta); - y += dis * tier4_autoware_utils::sin(theta); + 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; + undistorted_point_eigen = transformation_matrix * point_eigen; - baselink_tf_odom.setOrigin(tf2::Vector3(x, y, 0.0)); - baselink_tf_odom.setRotation(baselink_quat); + if (need_transform) { + undistorted_point_eigen = eigen_base_link_to_sensor * undistorted_point_eigen; + } + *it_x = undistorted_point_eigen[0]; + *it_y = undistorted_point_eigen[1]; + *it_z = undistorted_point_eigen[2]; - undistorted_point = baselink_tf_odom * point; + prev_transformation_matrix = transformation_matrix; + } else { + point.setValue(*it_x, *it_y, *it_z); - if (need_transform) { - undistorted_point = tf2_base_link_to_sensor * undistorted_point; - } + if (need_transform) { + point = tf2_base_link_to_sensor_inv * point; + } - *it_x = static_cast(undistorted_point.getX()); - *it_y = static_cast(undistorted_point.getY()); - *it_z = static_cast(undistorted_point.getZ()); + theta += w * time_offset; + baselink_quat.setValue( + 0, 0, tier4_autoware_utils::sin(theta * 0.5f), + tier4_autoware_utils::cos(theta * 0.5f)); // baselink_quat.setRPY(0.0, 0.0, theta); + const float dis = v * time_offset; + x += dis * tier4_autoware_utils::cos(theta); + y += dis * tier4_autoware_utils::sin(theta); + + baselink_tf_odom.setOrigin(tf2::Vector3(x, y, 0.0)); + baselink_tf_odom.setRotation(baselink_quat); + + undistorted_point = baselink_tf_odom * point; + + if (need_transform) { + undistorted_point = tf2_base_link_to_sensor * undistorted_point; + } + + *it_x = static_cast(undistorted_point.getX()); + *it_y = static_cast(undistorted_point.getY()); + *it_z = static_cast(undistorted_point.getZ()); + } prev_time_stamp_sec = *it_time_stamp; } From 026edf316d4f030c4f572b0747581a1f46fb631c Mon Sep 17 00:00:00 2001 From: vividf Date: Tue, 21 May 2024 00:53:19 +0900 Subject: [PATCH 02/47] change parameter back to default and do small refactor Signed-off-by: vividf --- .../docs/distortion-corrector.md | 1 + .../distortion_corrector.cpp | 21 +++++++++---------- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md index 805bd29f87b73..ed44976cd47e4 100644 --- a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md +++ b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md @@ -45,3 +45,4 @@ Please note that the processing time difference between the two distortion metho ## Assumptions / Known limits - The node requires that time synchronization works well between the pointcloud, twist, and IMU. +- If you want to use 3D distortion corrector without IMU, please check that the linear and angular velocity fields of your twist message are not empty. diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index feeb7c76dac1a..537fbb80a86c1 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -41,7 +41,7 @@ DistortionCorrectorComponent::DistortionCorrectorComponent(const rclcpp::NodeOpt // Parameter time_stamp_field_name_ = declare_parameter("time_stamp_field_name", "time_stamp"); use_imu_ = declare_parameter("use_imu", true); - use_3d_distortion_correction_ = declare_parameter("use_3d_distortion_correction"); + use_3d_distortion_correction_ = declare_parameter("use_3d_distortion_correction", false); // Publisher undistorted_points_pub_ = @@ -267,12 +267,6 @@ bool DistortionCorrectorComponent::undistortPointCloud( // For performance, do not instantiate `rclcpp::Time` inside of the for-loop double twist_stamp = rclcpp::Time(twist_it->header.stamp).seconds(); - // For performance, instantiate outside of the for-loop - tf2::Quaternion baselink_quat{}; - tf2::Transform baselink_tf_odom{}; - tf2::Vector3 point{}; - tf2::Vector3 undistorted_point{}; - // For performance, avoid transform computation if unnecessary bool need_transform = points.header.frame_id != base_link_frame_; @@ -286,10 +280,16 @@ bool DistortionCorrectorComponent::undistortPointCloud( bool twist_time_stamp_is_too_late = false; bool imu_time_stamp_is_too_late = false; + // For performance, instantiate outside of the for-loop + tf2::Quaternion baselink_quat{}; + tf2::Transform baselink_tf_odom{}; + // for 2d motion + tf2::Vector3 point{}; + tf2::Vector3 undistorted_point{}; float theta{0.0f}; - float x{0.0f}; - float y{0.0f}; + float x{0.0f}, y{0.0f}; + float v{0.0f}, w{0.0f}; // for 3d motion Eigen::Vector4f point_eigen; @@ -297,8 +297,7 @@ bool DistortionCorrectorComponent::undistortPointCloud( Eigen::Matrix4f transformation_matrix; Eigen::Matrix4f prev_transformation_matrix = Eigen::Matrix4f::Identity(); - 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; // 3d motion - float v = 0.0f, w = 0.0f; // 2d motion + 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}; for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_time_stamp) { while (twist_it != std::end(twist_queue_) - 1 && *it_time_stamp > twist_stamp) { From 89216a11b5baa947b8db21ba417d90a1519ac5a4 Mon Sep 17 00:00:00 2001 From: vividf Date: Thu, 23 May 2024 11:29:31 +0900 Subject: [PATCH 03/47] init version, need to double check Signed-off-by: vividf --- .../pointcloud_preprocessor/CMakeLists.txt | 1 + .../distortion_corrector.hpp | 61 ++-- .../distortion_corrector/undistorter.hpp | 129 +++++++ .../distortion_corrector.cpp | 314 ++++++------------ .../src/distortion_corrector/undistorter.cpp | 212 ++++++++++++ 5 files changed, 467 insertions(+), 250 deletions(-) create mode 100644 sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/undistorter.hpp create mode 100644 sensing/pointcloud_preprocessor/src/distortion_corrector/undistorter.cpp diff --git a/sensing/pointcloud_preprocessor/CMakeLists.txt b/sensing/pointcloud_preprocessor/CMakeLists.txt index 0f44629ab211e..ca20b985fc7a0 100644 --- a/sensing/pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/pointcloud_preprocessor/CMakeLists.txt @@ -78,6 +78,7 @@ ament_auto_add_library(pointcloud_preprocessor_filter SHARED src/pointcloud_accumulator/pointcloud_accumulator_nodelet.cpp src/vector_map_filter/lanelet2_map_filter_nodelet.cpp src/distortion_corrector/distortion_corrector.cpp + src/distortion_corrector/undistorter.cpp src/blockage_diag/blockage_diag_nodelet.cpp src/polygon_remover/polygon_remover.cpp src/vector_map_filter/vector_map_inside_area_filter.cpp diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp index bc81c854adf93..18096631ad3c8 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp @@ -25,25 +25,18 @@ #include #include -#include -#include - -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - -#include -#include - // Include tier4 autoware utils +#include "pointcloud_preprocessor/distortion_corrector/undistorter.hpp" + #include #include #include +#include #include +#include #include +#include namespace pointcloud_preprocessor { @@ -56,22 +49,6 @@ class DistortionCorrectorComponent : public rclcpp::Node explicit DistortionCorrectorComponent(const rclcpp::NodeOptions & options); private: - void onPointCloud(PointCloud2::UniquePtr points_msg); - void onTwistWithCovarianceStamped( - const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg); - void onImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg); - - bool getTransform( - const std::string & target_frame, const std::string & source_frame, - tf2::Transform * tf2_transform_ptr); - bool getTransform( - const std::string & target_frame, const std::string & source_frame, - tf2::Transform * tf2_transform_ptr, Eigen::Matrix4f & eigen_transform); - - bool undistortPointCloud( - const tf2::Transform & tf2_base_link_to_sensor, Eigen::Matrix4f & eigen_base_link_to_sensor, - PointCloud2 & points); - rclcpp::Subscription::SharedPtr input_points_sub_; rclcpp::Subscription::SharedPtr imu_sub_; rclcpp::Subscription::SharedPtr twist_sub_; @@ -80,16 +57,38 @@ class DistortionCorrectorComponent : public rclcpp::Node std::unique_ptr> stop_watch_ptr_; std::unique_ptr debug_publisher_; - tf2_ros::Buffer tf2_buffer_{get_clock()}; - tf2_ros::TransformListener tf2_listener_{tf2_buffer_}; - std::deque twist_queue_; std::deque angular_velocity_queue_; + tf2_ros::Buffer tf2_buffer{get_clock()}; + std::string base_link_frame_ = "base_link"; std::string time_stamp_field_name_; bool use_imu_; bool use_3d_distortion_correction_; + + std::unique_ptr undistorter_; + + void onPointCloud(PointCloud2::UniquePtr points_msg); + void onTwistWithCovarianceStamped( + const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg); + void onImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg); + + void getTransformTF( + const std::string & target_frame, const std::string & source_frame, + tf2::Transform * tf2_transform_ptr); + + void undistortPointCloud(PointCloud2 & points); + + bool isInputValid(PointCloud2 & points); + + void getIteratorOfTwistAndIMU( + double first_point_time_stamp_sec, + std::deque::iterator & it_twist, + std::deque::iterator & it_imu); + + void warnIfTimestampsTooLate( + bool is_twist_time_stamp_too_late, bool is_imu_time_stamp_is_too_late); }; } // namespace pointcloud_preprocessor diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/undistorter.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/undistorter.hpp new file mode 100644 index 0000000000000..28b76b450068e --- /dev/null +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/undistorter.hpp @@ -0,0 +1,129 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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. + +#ifndef POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__UNDISTORTER_HPP_ +#define POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__UNDISTORTER_HPP_ + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include +#include + +// Include tier4 autoware utils +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace pointcloud_preprocessor +{ +class Undistorter +{ +public: + bool is_pointcloud_transform_needed = false; + bool is_pointcloud_transfrom_exist = false; + bool is_imu_transfrom_exist = false; + tf2_ros::Buffer & tf2_buffer; + geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr; + + explicit Undistorter(tf2_ros::Buffer & buffer) : tf2_buffer(buffer) {} + void setIMUTransform(const std::string & target_frame, const std::string & source_frame); + + virtual void initialize() = 0; + virtual 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 time_offset, + bool is_twist_valid, bool is_imu_valid) = 0; + + virtual void setPointCloudTransform( + const std::string & base_link_frame, const std::string & lidar_frame) = 0; +}; + +class Undistorter2D : public Undistorter +{ +public: + // defined outside of for loop for performance reasons. + tf2::Quaternion baselink_quat; + tf2::Transform baselink_tf_odom; + tf2::Vector3 point_tf; + tf2::Vector3 undistorted_point_tf; + float theta; + + // TF + tf2::Transform tf2_lidar_to_base_link; + tf2::Transform tf2_base_link_to_lidar; + + explicit Undistorter2D(tf2_ros::Buffer & buffer) : Undistorter(buffer) {} + void initialize() override; + 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 time_offset, + bool is_twist_valid, bool is_imu_valid) override; + + void setPointCloudTransform(const std::string & base_link_frame, const std::string & lidar_frame); +}; + +class Undistorter3D : public Undistorter +{ +public: + // defined outside of for loop for performance reasons. + Eigen::Vector4f point_eigen; + Eigen::Vector4f undistorted_point_eigen; + Eigen::Matrix4f transformation_matrix; + Eigen::Matrix4f prev_transformation_matrix; + + // TF + Eigen::Matrix4f eigen_lidar_to_base_link; + Eigen::Matrix4f eigen_base_link_to_lidar; + + explicit Undistorter3D(tf2_ros::Buffer & buffer) : Undistorter(buffer) {} + void initialize() override; + 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 time_offset, + bool is_twist_valid, bool is_imu_valid) override; + void setPointCloudTransform(const std::string & base_link_frame, const std::string & lidar_frame); +}; + +} // namespace pointcloud_preprocessor + +#endif // POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__UNDISTORTER_HPP_ diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index 537fbb80a86c1..956c9622b72e2 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -14,11 +14,13 @@ #include "pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp" +#include "pointcloud_preprocessor/distortion_corrector/undistorter.hpp" #include "tier4_autoware_utils/math/trigonometry.hpp" #include #include +#include #include #include @@ -58,6 +60,14 @@ DistortionCorrectorComponent::DistortionCorrectorComponent(const rclcpp::NodeOpt input_points_sub_ = this->create_subscription( "~/input/pointcloud", rclcpp::SensorDataQoS(), std::bind(&DistortionCorrectorComponent::onPointCloud, this, std::placeholders::_1)); + + // Setup the undistortor + + if (use_3d_distortion_correction_) { + undistorter_ = std::make_unique(tf2_buffer); + } else { + undistorter_ = std::make_unique(tf2_buffer); + } } void DistortionCorrectorComponent::onTwistWithCovarianceStamped( @@ -87,17 +97,15 @@ void DistortionCorrectorComponent::onImu(const sensor_msgs::msg::Imu::ConstShare return; } - tf2::Transform tf2_imu_link_to_base_link{}; - getTransform(base_link_frame_, imu_msg->header.frame_id, &tf2_imu_link_to_base_link); - geometry_msgs::msg::TransformStamped::SharedPtr tf_base2imu_ptr = - std::make_shared(); - tf_base2imu_ptr->transform.rotation = tf2::toMsg(tf2_imu_link_to_base_link.getRotation()); + if (!undistorter_->is_imu_transfrom_exist) + undistorter_->setIMUTransform(base_link_frame_, imu_msg->header.frame_id); geometry_msgs::msg::Vector3Stamped angular_velocity; angular_velocity.vector = imu_msg->angular_velocity; geometry_msgs::msg::Vector3Stamped transformed_angular_velocity; - tf2::doTransform(angular_velocity, transformed_angular_velocity, *tf_base2imu_ptr); + tf2::doTransform( + angular_velocity, transformed_angular_velocity, *undistorter_->geometry_imu_to_base_link_ptr); transformed_angular_velocity.header = imu_msg->header; angular_velocity_queue_.push_back(transformed_angular_velocity); @@ -116,7 +124,7 @@ void DistortionCorrectorComponent::onImu(const sensor_msgs::msg::Imu::ConstShare } } -void DistortionCorrectorComponent::onPointCloud(PointCloud2::UniquePtr points_msg) +void DistortionCorrectorComponent::onPointCloud(PointCloud2::UniquePtr pointcloud_msg) { stop_watch_ptr_->toc("processing_time", true); const auto points_sub_count = undistorted_points_pub_->get_subscription_count() + @@ -126,25 +134,23 @@ void DistortionCorrectorComponent::onPointCloud(PointCloud2::UniquePtr points_ms return; } - Eigen::Matrix4f eigen_base_link_to_sensor; - tf2::Transform tf2_base_link_to_sensor{}; - getTransform( - points_msg->header.frame_id, base_link_frame_, &tf2_base_link_to_sensor, - eigen_base_link_to_sensor); + if (!undistorter_->is_pointcloud_transfrom_exist) + undistorter_->setPointCloudTransform(base_link_frame_, pointcloud_msg->header.frame_id); - undistortPointCloud(tf2_base_link_to_sensor, eigen_base_link_to_sensor, *points_msg); + undistorter_->initialize(); + undistortPointCloud(*pointcloud_msg); if (debug_publisher_) { auto pipeline_latency_ms = std::chrono::duration( std::chrono::nanoseconds( - (this->get_clock()->now() - points_msg->header.stamp).nanoseconds())) + (this->get_clock()->now() - pointcloud_msg->header.stamp).nanoseconds())) .count(); debug_publisher_->publish( "debug/pipeline_latency_ms", pipeline_latency_ms); } - undistorted_points_pub_->publish(std::move(points_msg)); + undistorted_points_pub_->publish(std::move(pointcloud_msg)); // add processing time for debug if (debug_publisher_) { @@ -157,269 +163,139 @@ void DistortionCorrectorComponent::onPointCloud(PointCloud2::UniquePtr points_ms } } -bool DistortionCorrectorComponent::getTransform( - const std::string & target_frame, const std::string & source_frame, - tf2::Transform * tf2_transform_ptr, Eigen::Matrix4f & eigen_transform) +bool DistortionCorrectorComponent::isInputValid(PointCloud2 & pointcloud) { - if (target_frame == source_frame) { - tf2_transform_ptr->setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); - tf2_transform_ptr->setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0)); - eigen_transform = Eigen::Matrix4f::Identity(); - return true; - } - - try { - const auto transform_msg = - tf2_buffer_.lookupTransform(target_frame, source_frame, tf2::TimePointZero); - tf2::convert(transform_msg.transform, *tf2_transform_ptr); - eigen_transform = tf2::transformToEigen(transform_msg.transform).matrix().cast(); - } catch (const tf2::TransformException & ex) { - RCLCPP_WARN(get_logger(), "%s", ex.what()); - RCLCPP_ERROR( - get_logger(), "Please publish TF %s to %s", target_frame.c_str(), source_frame.c_str()); - - tf2_transform_ptr->setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); - tf2_transform_ptr->setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0)); - eigen_transform = Eigen::Matrix4f::Identity(); + if (pointcloud.data.empty() || twist_queue_.empty()) { + RCLCPP_WARN_STREAM_THROTTLE( + get_logger(), *get_clock(), 10000 /* ms */, "input pointcloud or twist_queue_ is empty."); return false; } - return true; -} -bool DistortionCorrectorComponent::getTransform( - const std::string & target_frame, const std::string & source_frame, - tf2::Transform * tf2_transform_ptr) -{ - if (target_frame == source_frame) { - tf2_transform_ptr->setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); - tf2_transform_ptr->setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0)); - return true; - } - - try { - const auto transform_msg = - tf2_buffer_.lookupTransform(target_frame, source_frame, tf2::TimePointZero); - tf2::convert(transform_msg.transform, *tf2_transform_ptr); - } catch (const tf2::TransformException & ex) { - RCLCPP_WARN(get_logger(), "%s", ex.what()); - RCLCPP_ERROR( - get_logger(), "Please publish TF %s to %s", target_frame.c_str(), source_frame.c_str()); - - tf2_transform_ptr->setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); - tf2_transform_ptr->setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0)); + auto time_stamp_field_it = std::find_if( + std::cbegin(pointcloud.fields), std::cend(pointcloud.fields), + [this](const sensor_msgs::msg::PointField & field) { + return field.name == time_stamp_field_name_; + }); + if (time_stamp_field_it == pointcloud.fields.cend()) { + RCLCPP_WARN_STREAM_THROTTLE( + get_logger(), *get_clock(), 10000 /* ms */, + "Required field time stamp doesn't exist in the point cloud."); return false; } return true; } -bool DistortionCorrectorComponent::undistortPointCloud( - const tf2::Transform & tf2_base_link_to_sensor, Eigen::Matrix4f & eigen_base_link_to_sensor, - PointCloud2 & points) +void DistortionCorrectorComponent::warnIfTimestampsTooLate( + bool is_twist_time_stamp_too_late, bool is_imu_time_stamp_is_too_late) { - if (points.data.empty() || twist_queue_.empty()) { + if (is_twist_time_stamp_too_late) { RCLCPP_WARN_STREAM_THROTTLE( get_logger(), *get_clock(), 10000 /* ms */, - "input_pointcloud->points or twist_queue_ is empty."); - return false; + "twist time_stamp is too late. Could not interpolate."); } - auto time_stamp_field_it = std::find_if( - std::cbegin(points.fields), std::cend(points.fields), - [this](const sensor_msgs::msg::PointField & field) { - return field.name == time_stamp_field_name_; - }); - if (time_stamp_field_it == points.fields.cend()) { + if (is_imu_time_stamp_is_too_late) { RCLCPP_WARN_STREAM_THROTTLE( get_logger(), *get_clock(), 10000 /* ms */, - "Required field time stamp doesn't exist in the point cloud."); - return false; + "imu time_stamp is too late. Could not interpolate."); } +} - sensor_msgs::PointCloud2Iterator it_x(points, "x"); - sensor_msgs::PointCloud2Iterator it_y(points, "y"); - sensor_msgs::PointCloud2Iterator it_z(points, "z"); - sensor_msgs::PointCloud2ConstIterator it_time_stamp(points, time_stamp_field_name_); - - double prev_time_stamp_sec{*it_time_stamp}; - const double first_point_time_stamp_sec{*it_time_stamp}; - - auto twist_it = std::lower_bound( +void DistortionCorrectorComponent::getIteratorOfTwistAndIMU( + double first_point_time_stamp_sec, + std::deque::iterator & it_twist, + std::deque::iterator & it_imu) +{ + it_twist = std::lower_bound( std::begin(twist_queue_), std::end(twist_queue_), first_point_time_stamp_sec, [](const geometry_msgs::msg::TwistStamped & x, const double t) { return rclcpp::Time(x.header.stamp).seconds() < t; }); - twist_it = twist_it == std::end(twist_queue_) ? std::end(twist_queue_) - 1 : twist_it; + it_twist = it_twist == std::end(twist_queue_) ? std::end(twist_queue_) - 1 : it_twist; - decltype(angular_velocity_queue_)::iterator imu_it; if (use_imu_ && !angular_velocity_queue_.empty()) { - imu_it = std::lower_bound( + it_imu = std::lower_bound( std::begin(angular_velocity_queue_), std::end(angular_velocity_queue_), first_point_time_stamp_sec, [](const geometry_msgs::msg::Vector3Stamped & x, const double t) { return rclcpp::Time(x.header.stamp).seconds() < t; }); - imu_it = - imu_it == std::end(angular_velocity_queue_) ? std::end(angular_velocity_queue_) - 1 : imu_it; + it_imu = + it_imu == std::end(angular_velocity_queue_) ? std::end(angular_velocity_queue_) - 1 : it_imu; } +} + +void DistortionCorrectorComponent::undistortPointCloud(PointCloud2 & pointcloud) +{ + if (!isInputValid(pointcloud)) return; - const tf2::Transform tf2_base_link_to_sensor_inv{tf2_base_link_to_sensor.inverse()}; - const Eigen::Matrix4f eigen_base_link_to_sensor_inv = eigen_base_link_to_sensor.inverse(); + sensor_msgs::PointCloud2Iterator it_x(pointcloud, "x"); + sensor_msgs::PointCloud2Iterator it_y(pointcloud, "y"); + sensor_msgs::PointCloud2Iterator it_z(pointcloud, "z"); + sensor_msgs::PointCloud2ConstIterator it_time_stamp(pointcloud, time_stamp_field_name_); - // For performance, do not instantiate `rclcpp::Time` inside of the for-loop - double twist_stamp = rclcpp::Time(twist_it->header.stamp).seconds(); + double prev_time_stamp_sec{*it_time_stamp}; + const double first_point_time_stamp_sec{*it_time_stamp}; - // For performance, avoid transform computation if unnecessary - bool need_transform = points.header.frame_id != base_link_frame_; + std::deque::iterator it_twist; + std::deque::iterator it_imu; + getIteratorOfTwistAndIMU(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(); double imu_stamp{0.0}; if (use_imu_ && !angular_velocity_queue_.empty()) { - imu_stamp = rclcpp::Time(imu_it->header.stamp).seconds(); + imu_stamp = rclcpp::Time(it_imu->header.stamp).seconds(); } - // If there is a point that cannot be associated, record it to issue a warning - bool twist_time_stamp_is_too_late = false; - bool imu_time_stamp_is_too_late = false; - - // For performance, instantiate outside of the for-loop - tf2::Quaternion baselink_quat{}; - tf2::Transform baselink_tf_odom{}; - - // for 2d motion - tf2::Vector3 point{}; - tf2::Vector3 undistorted_point{}; - float theta{0.0f}; - float x{0.0f}, y{0.0f}; - float v{0.0f}, w{0.0f}; - - // for 3d motion - Eigen::Vector4f point_eigen; - Eigen::Vector4f undistorted_point_eigen; - Eigen::Matrix4f transformation_matrix; - Eigen::Matrix4f prev_transformation_matrix = Eigen::Matrix4f::Identity(); - - 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}; + // If there is a point in a pointlcoud that cannot be associated, record it to issue a warning + bool is_twist_time_stamp_too_late = false; + bool is_imu_time_stamp_is_too_late = false; for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_time_stamp) { - while (twist_it != std::end(twist_queue_) - 1 && *it_time_stamp > twist_stamp) { - ++twist_it; - twist_stamp = rclcpp::Time(twist_it->header.stamp).seconds(); - } + bool is_twist_valid = true; + bool is_imu_valid = true; - if (use_3d_distortion_correction_) { - v_x = static_cast(twist_it->twist.linear.x); - v_y = static_cast(twist_it->twist.linear.y); - v_z = static_cast(twist_it->twist.linear.z); - w_x = static_cast(twist_it->twist.angular.x); - w_y = static_cast(twist_it->twist.angular.y); - w_z = static_cast(twist_it->twist.angular.z); - } else { - v = static_cast(twist_it->twist.linear.x); - w = static_cast(twist_it->twist.angular.z); + // Get closest twist information + while (it_twist != std::end(twist_queue_) - 1 && *it_time_stamp > twist_stamp) { + ++it_twist; + twist_stamp = rclcpp::Time(it_twist->header.stamp).seconds(); } - if (std::abs(*it_time_stamp - twist_stamp) > 0.1) { - twist_time_stamp_is_too_late = true; - - if (use_3d_distortion_correction_) { - v_x = 0.0f; - v_y = 0.0f; - v_z = 0.0f; - w_x = 0.0f; - w_y = 0.0f; - w_z = 0.0f; - } else { - v = 0.0f; - w = 0.0f; - } + is_twist_time_stamp_too_late = true; + is_twist_valid = false; } + // Get closest IMU information if (use_imu_ && !angular_velocity_queue_.empty()) { - while (imu_it != std::end(angular_velocity_queue_) - 1 && *it_time_stamp > imu_stamp) { - ++imu_it; - imu_stamp = rclcpp::Time(imu_it->header.stamp).seconds(); + while (it_imu != std::end(angular_velocity_queue_) - 1 && *it_time_stamp > imu_stamp) { + ++it_imu; + imu_stamp = rclcpp::Time(it_imu->header.stamp).seconds(); } if (std::abs(*it_time_stamp - imu_stamp) > 0.1) { - imu_time_stamp_is_too_late = true; - } else { - if (use_3d_distortion_correction_) { - w_x = static_cast(imu_it->vector.x); - w_y = static_cast(imu_it->vector.y); - w_z = static_cast(imu_it->vector.z); - } else { - w = static_cast(imu_it->vector.z); - } + is_imu_time_stamp_is_too_late = true; + is_imu_valid = false; } + } else { + is_imu_valid = false; } const auto time_offset = static_cast(*it_time_stamp - prev_time_stamp_sec); - if (use_3d_distortion_correction_) { - point_eigen << *it_x, *it_y, *it_z, 1.0; - - if (need_transform) { - point_eigen = eigen_base_link_to_sensor_inv * point_eigen; - } + // std::cout << "before undistortPoint" << std::endl; + // std::cout << "it_x: " << *it_x << " it_y: " << *it_y << " it_z: " << *it_z << std::endl; - 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; - undistorted_point_eigen = transformation_matrix * point_eigen; - - if (need_transform) { - undistorted_point_eigen = eigen_base_link_to_sensor * undistorted_point_eigen; - } - *it_x = undistorted_point_eigen[0]; - *it_y = undistorted_point_eigen[1]; - *it_z = undistorted_point_eigen[2]; - - prev_transformation_matrix = transformation_matrix; - } else { - point.setValue(*it_x, *it_y, *it_z); - - if (need_transform) { - point = tf2_base_link_to_sensor_inv * point; - } - - theta += w * time_offset; - baselink_quat.setValue( - 0, 0, tier4_autoware_utils::sin(theta * 0.5f), - tier4_autoware_utils::cos(theta * 0.5f)); // baselink_quat.setRPY(0.0, 0.0, theta); - const float dis = v * time_offset; - x += dis * tier4_autoware_utils::cos(theta); - y += dis * tier4_autoware_utils::sin(theta); - - baselink_tf_odom.setOrigin(tf2::Vector3(x, y, 0.0)); - baselink_tf_odom.setRotation(baselink_quat); - - undistorted_point = baselink_tf_odom * point; - - if (need_transform) { - undistorted_point = tf2_base_link_to_sensor * undistorted_point; - } - - *it_x = static_cast(undistorted_point.getX()); - *it_y = static_cast(undistorted_point.getY()); - *it_z = static_cast(undistorted_point.getZ()); - } + // Undistorted a single point based on the strategy + undistorter_->undistortPoint( + it_x, it_y, it_z, it_twist, it_imu, time_offset, is_twist_valid, is_imu_valid); + // std::cout << "after undistortPoint" << std::endl; + // std::cout << "it_x: " << *it_x << " it_y: " << *it_y << " it_z: " << *it_z << std::endl; + // std::cout << "//////////////////\n" << std::endl; prev_time_stamp_sec = *it_time_stamp; } - if (twist_time_stamp_is_too_late) { - RCLCPP_WARN_STREAM_THROTTLE( - get_logger(), *get_clock(), 10000 /* ms */, - "twist time_stamp is too late. Could not interpolate."); - } - - if (imu_time_stamp_is_too_late) { - RCLCPP_WARN_STREAM_THROTTLE( - get_logger(), *get_clock(), 10000 /* ms */, - "imu time_stamp is too late. Could not interpolate."); - } - - return true; + warnIfTimestampsTooLate(is_twist_time_stamp_too_late, is_imu_time_stamp_is_too_late); } } // namespace pointcloud_preprocessor diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/undistorter.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/undistorter.cpp new file mode 100644 index 0000000000000..0ec67ba1dc7e9 --- /dev/null +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/undistorter.cpp @@ -0,0 +1,212 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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. + +#include "pointcloud_preprocessor/distortion_corrector/undistorter.hpp" + +#include "tier4_autoware_utils/math/trigonometry.hpp" + +#include + +#include +#include +#include +#include + +namespace pointcloud_preprocessor +{ + +void Undistorter::setIMUTransform( + const std::string & base_link_frame, const std::string & imu_frame) +{ + tf2::Transform tf2_imu_to_base_link; + if (base_link_frame == imu_frame) { + tf2_imu_to_base_link.setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); + tf2_imu_to_base_link.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0)); + is_imu_transfrom_exist = true; + } + + try { + const auto transform_msg = + tf2_buffer.lookupTransform(base_link_frame, imu_frame, tf2::TimePointZero); + tf2::convert(transform_msg.transform, tf2_imu_to_base_link); + is_imu_transfrom_exist = true; + } catch (const tf2::TransformException & ex) { + // RCLCPP_WARN(get_logger(), "%s", ex.what()); + // RCLCPP_ERROR( + // get_logger(), "Please publish TF %s to %s", target_frame.c_str(), source_frame.c_str()); + + tf2_imu_to_base_link.setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); + tf2_imu_to_base_link.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0)); + } + + 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()); +} + +void Undistorter2D::initialize() +{ + theta = 0.0f; +} + +void Undistorter3D::initialize() +{ + prev_transformation_matrix = Eigen::Matrix4f::Identity(); +} + +void Undistorter2D::setPointCloudTransform( + const std::string & base_link_frame, const std::string & lidar_frame) +{ + if (base_link_frame == lidar_frame) { + tf2_lidar_to_base_link.setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); + tf2_lidar_to_base_link.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0)); + tf2_base_link_to_lidar = tf2_lidar_to_base_link; + is_pointcloud_transfrom_exist = true; + } + + try { + const auto transform_msg = + tf2_buffer.lookupTransform(base_link_frame, lidar_frame, tf2::TimePointZero); + tf2::convert(transform_msg.transform, tf2_lidar_to_base_link); + tf2_base_link_to_lidar = tf2_lidar_to_base_link.inverse(); + is_pointcloud_transfrom_exist = true; + is_pointcloud_transform_needed = true; + } catch (const tf2::TransformException & ex) { + // RCLCPP_WARN(get_logger(), "%s", ex.what()); + // RCLCPP_ERROR( + // get_logger(), "Please publish TF %s to %s", target_frame.c_str(), source_frame.c_str()); + + tf2_lidar_to_base_link.setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); + tf2_lidar_to_base_link.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0)); + tf2_base_link_to_lidar = tf2_lidar_to_base_link; + } +} + +void Undistorter3D::setPointCloudTransform( + const std::string & base_link_frame, const std::string & lidar_frame) +{ + if (base_link_frame == lidar_frame) { + eigen_lidar_to_base_link = Eigen::Matrix4f::Identity(); + eigen_base_link_to_lidar = Eigen::Matrix4f::Identity(); + is_pointcloud_transfrom_exist = true; + } + + try { + const auto transform_msg = + tf2_buffer.lookupTransform(base_link_frame, lidar_frame, tf2::TimePointZero); + eigen_lidar_to_base_link = + tf2::transformToEigen(transform_msg.transform).matrix().cast(); + eigen_base_link_to_lidar = eigen_lidar_to_base_link.inverse(); + is_pointcloud_transfrom_exist = true; + is_pointcloud_transform_needed = true; + } catch (const tf2::TransformException & ex) { + // RCLCPP_WARN(get_logger(), "%s", ex.what()); + // RCLCPP_ERROR( + // get_logger(), "Please publish TF %s to %s", target_frame.c_str(), source_frame.c_str()); + eigen_lidar_to_base_link = Eigen::Matrix4f::Identity(); + eigen_base_link_to_lidar = Eigen::Matrix4f::Identity(); + } +} + +void Undistorter2D::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 time_offset, + bool is_twist_valid, bool is_imu_valid) +{ + // Initialize linear velocity and angular velocity + float x{0.0f}, y{0.0f}, v{0.0f}, w{0.0f}; + if (is_twist_valid) { + v = static_cast(it_twist->twist.linear.x); + w = static_cast(it_twist->twist.angular.z); + } + if (is_imu_valid) { + w = static_cast(it_imu->vector.z); + } + + // Undistort point + point_tf.setValue(*it_x, *it_y, *it_z); + + if (is_pointcloud_transform_needed) { + point_tf = tf2_lidar_to_base_link * point_tf; + } + theta += w * time_offset; + baselink_quat.setValue( + 0, 0, tier4_autoware_utils::sin(theta * 0.5f), + tier4_autoware_utils::cos(theta * 0.5f)); // baselink_quat.setRPY(0.0, 0.0, theta); + const float dis = v * time_offset; + x += dis * tier4_autoware_utils::cos(theta); + y += dis * tier4_autoware_utils::sin(theta); + + baselink_tf_odom.setOrigin(tf2::Vector3(x, y, 0.0)); + baselink_tf_odom.setRotation(baselink_quat); + + undistorted_point_tf = baselink_tf_odom * point_tf; + + if (is_pointcloud_transform_needed) { + undistorted_point_tf = tf2_base_link_to_lidar * undistorted_point_tf; + } + + *it_x = static_cast(undistorted_point_tf.getX()); + *it_y = static_cast(undistorted_point_tf.getY()); + *it_z = static_cast(undistorted_point_tf.getZ()); +} + +void Undistorter3D::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 time_offset, + bool is_twist_valid, 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}; + 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); + } + 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); + } + + // Undistort point + point_eigen << *it_x, *it_y, *it_z, 1.0; + if (is_pointcloud_transform_needed) { + 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_); + twist = twist * time_offset; + transformation_matrix = Sophus::SE3f::exp(twist).matrix(); + transformation_matrix = transformation_matrix * prev_transformation_matrix; + undistorted_point_eigen = transformation_matrix * point_eigen; + + if (is_pointcloud_transform_needed) { + undistorted_point_eigen = eigen_base_link_to_lidar * undistorted_point_eigen; + } + *it_x = undistorted_point_eigen[0]; + *it_y = undistorted_point_eigen[1]; + *it_z = undistorted_point_eigen[2]; + + prev_transformation_matrix = transformation_matrix; +} + +} // namespace pointcloud_preprocessor From da8f829fa08768b7fff47ba49e2835436921b38f Mon Sep 17 00:00:00 2001 From: vividf Date: Thu, 23 May 2024 12:39:22 +0900 Subject: [PATCH 04/47] fix the logic error Signed-off-by: vividf --- .../distortion_corrector/undistorter.hpp | 2 + .../src/distortion_corrector/undistorter.cpp | 66 ++++++++++--------- 2 files changed, 36 insertions(+), 32 deletions(-) diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/undistorter.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/undistorter.hpp index 28b76b450068e..b616417270c2d 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/undistorter.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/undistorter.hpp @@ -83,6 +83,8 @@ class Undistorter2D : public Undistorter tf2::Vector3 point_tf; tf2::Vector3 undistorted_point_tf; float theta; + float x; + float y; // TF tf2::Transform tf2_lidar_to_base_link; diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/undistorter.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/undistorter.cpp index 0ec67ba1dc7e9..87d5b16592b0c 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/undistorter.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/undistorter.cpp @@ -34,20 +34,20 @@ void Undistorter::setIMUTransform( tf2_imu_to_base_link.setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); tf2_imu_to_base_link.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0)); is_imu_transfrom_exist = true; - } - - try { - const auto transform_msg = - tf2_buffer.lookupTransform(base_link_frame, imu_frame, tf2::TimePointZero); - tf2::convert(transform_msg.transform, tf2_imu_to_base_link); - is_imu_transfrom_exist = true; - } catch (const tf2::TransformException & ex) { - // RCLCPP_WARN(get_logger(), "%s", ex.what()); - // RCLCPP_ERROR( - // get_logger(), "Please publish TF %s to %s", target_frame.c_str(), source_frame.c_str()); - - tf2_imu_to_base_link.setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); - tf2_imu_to_base_link.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0)); + } else { + try { + const auto transform_msg = + tf2_buffer.lookupTransform(base_link_frame, imu_frame, tf2::TimePointZero); + tf2::convert(transform_msg.transform, tf2_imu_to_base_link); + is_imu_transfrom_exist = true; + } catch (const tf2::TransformException & ex) { + // RCLCPP_WARN(get_logger(), "%s", ex.what()); + // RCLCPP_ERROR( + // get_logger(), "Please publish TF %s to %s", target_frame.c_str(), source_frame.c_str()); + + tf2_imu_to_base_link.setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); + tf2_imu_to_base_link.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0)); + } } geometry_imu_to_base_link_ptr = std::make_shared(); @@ -57,6 +57,8 @@ void Undistorter::setIMUTransform( void Undistorter2D::initialize() { + x = 0.0f; + y = 0.0f; theta = 0.0f; } @@ -73,23 +75,23 @@ void Undistorter2D::setPointCloudTransform( tf2_lidar_to_base_link.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0)); tf2_base_link_to_lidar = tf2_lidar_to_base_link; is_pointcloud_transfrom_exist = true; - } - - try { - const auto transform_msg = - tf2_buffer.lookupTransform(base_link_frame, lidar_frame, tf2::TimePointZero); - tf2::convert(transform_msg.transform, tf2_lidar_to_base_link); - tf2_base_link_to_lidar = tf2_lidar_to_base_link.inverse(); - is_pointcloud_transfrom_exist = true; - is_pointcloud_transform_needed = true; - } catch (const tf2::TransformException & ex) { - // RCLCPP_WARN(get_logger(), "%s", ex.what()); - // RCLCPP_ERROR( - // get_logger(), "Please publish TF %s to %s", target_frame.c_str(), source_frame.c_str()); - - tf2_lidar_to_base_link.setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); - tf2_lidar_to_base_link.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0)); - tf2_base_link_to_lidar = tf2_lidar_to_base_link; + } else { + try { + const auto transform_msg = + tf2_buffer.lookupTransform(base_link_frame, lidar_frame, tf2::TimePointZero); + tf2::convert(transform_msg.transform, tf2_lidar_to_base_link); + tf2_base_link_to_lidar = tf2_lidar_to_base_link.inverse(); + is_pointcloud_transfrom_exist = true; + is_pointcloud_transform_needed = true; + } catch (const tf2::TransformException & ex) { + // RCLCPP_WARN(get_logger(), "%s", ex.what()); + // RCLCPP_ERROR( + // get_logger(), "Please publish TF %s to %s", target_frame.c_str(), source_frame.c_str()); + + tf2_lidar_to_base_link.setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); + tf2_lidar_to_base_link.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0)); + tf2_base_link_to_lidar = tf2_lidar_to_base_link; + } } } @@ -127,7 +129,7 @@ void Undistorter2D::undistortPoint( bool is_twist_valid, bool is_imu_valid) { // Initialize linear velocity and angular velocity - float x{0.0f}, y{0.0f}, v{0.0f}, w{0.0f}; + float v{0.0f}, w{0.0f}; if (is_twist_valid) { v = static_cast(it_twist->twist.linear.x); w = static_cast(it_twist->twist.angular.z); From 80d88e108adf50e9b1ebfdbb40f981ea7abba6af Mon Sep 17 00:00:00 2001 From: vividf Date: Thu, 23 May 2024 13:00:30 +0900 Subject: [PATCH 05/47] temporary save, need to delete some code --- .../distortion_corrector/undistorter.hpp | 13 +++++++------ .../distortion_corrector/distortion_corrector.cpp | 15 ++++++++++++--- .../src/distortion_corrector/undistorter.cpp | 8 ++++---- 3 files changed, 23 insertions(+), 13 deletions(-) diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/undistorter.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/undistorter.hpp index b616417270c2d..83547bb051abc 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/undistorter.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/undistorter.hpp @@ -38,6 +38,7 @@ #include // Include tier4 autoware utils +// delete unused #include #include @@ -67,8 +68,8 @@ class Undistorter 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 time_offset, - bool is_twist_valid, bool is_imu_valid) = 0; + std::deque::iterator & it_imu, float & time_offset, + bool & is_twist_valid, bool & is_imu_valid) = 0; virtual void setPointCloudTransform( const std::string & base_link_frame, const std::string & lidar_frame) = 0; @@ -96,8 +97,8 @@ class Undistorter2D : public Undistorter 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 time_offset, - bool is_twist_valid, bool is_imu_valid) override; + std::deque::iterator & it_imu, float & time_offset, + bool & is_twist_valid, bool & is_imu_valid) override; void setPointCloudTransform(const std::string & base_link_frame, const std::string & lidar_frame); }; @@ -121,8 +122,8 @@ class Undistorter3D : public Undistorter 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 time_offset, - bool is_twist_valid, bool is_imu_valid) override; + std::deque::iterator & it_imu, float & time_offset, + bool & is_twist_valid, bool & is_imu_valid) override; void setPointCloudTransform(const std::string & base_link_frame, const std::string & lidar_frame); }; diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index 956c9622b72e2..2a92dc6d5acc3 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -19,6 +19,8 @@ #include +// delete this +#include #include #include #include @@ -250,10 +252,13 @@ void DistortionCorrectorComponent::undistortPointCloud(PointCloud2 & pointcloud) // If there is a point in a pointlcoud that cannot be associated, record it to issue a warning bool is_twist_time_stamp_too_late = false; bool is_imu_time_stamp_is_too_late = false; + bool is_twist_valid = true; + bool is_imu_valid = true; for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_time_stamp) { - bool is_twist_valid = true; - bool is_imu_valid = true; + auto start = std::chrono::high_resolution_clock::now(); + is_twist_valid = true; + is_imu_valid = true; // Get closest twist information while (it_twist != std::end(twist_queue_) - 1 && *it_time_stamp > twist_stamp) { @@ -280,7 +285,7 @@ void DistortionCorrectorComponent::undistortPointCloud(PointCloud2 & pointcloud) is_imu_valid = false; } - const auto time_offset = static_cast(*it_time_stamp - prev_time_stamp_sec); + float time_offset = static_cast(*it_time_stamp - prev_time_stamp_sec); // std::cout << "before undistortPoint" << std::endl; // std::cout << "it_x: " << *it_x << " it_y: " << *it_y << " it_z: " << *it_z << std::endl; @@ -293,6 +298,10 @@ void DistortionCorrectorComponent::undistortPointCloud(PointCloud2 & pointcloud) // std::cout << "it_x: " << *it_x << " it_y: " << *it_y << " it_z: " << *it_z << std::endl; // std::cout << "//////////////////\n" << std::endl; prev_time_stamp_sec = *it_time_stamp; + + auto end = std::chrono::high_resolution_clock::now(); + std::chrono::duration duration = end - start; + std::cout << "Function execution time: " << duration.count() << " seconds" << std::endl; } warnIfTimestampsTooLate(is_twist_time_stamp_too_late, is_imu_time_stamp_is_too_late); diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/undistorter.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/undistorter.cpp index 87d5b16592b0c..03d68ffb7cedf 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/undistorter.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/undistorter.cpp @@ -125,8 +125,8 @@ void Undistorter2D::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 time_offset, - bool is_twist_valid, bool is_imu_valid) + std::deque::iterator & it_imu, float & time_offset, + bool & is_twist_valid, bool & is_imu_valid) { // Initialize linear velocity and angular velocity float v{0.0f}, w{0.0f}; @@ -170,8 +170,8 @@ void Undistorter3D::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 time_offset, - bool is_twist_valid, bool is_imu_valid) + std::deque::iterator & it_imu, float & time_offset, + bool & is_twist_valid, 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}; From 4d96e944ae28320b665b0951aa5307f4db61eef9 Mon Sep 17 00:00:00 2001 From: vividf Date: Fri, 24 May 2024 18:31:29 +0900 Subject: [PATCH 06/47] init done, need to check for 3d as time is high Signed-off-by: vividf --- .../distortion_corrector.hpp | 21 +- .../distortion_corrector/undistorter.hpp | 69 +++++- .../distortion_corrector.cpp | 192 +-------------- .../src/distortion_corrector/undistorter.cpp | 228 +++++++++++++++++- 4 files changed, 285 insertions(+), 225 deletions(-) diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp index 18096631ad3c8..90bde06aa841d 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp @@ -57,9 +57,6 @@ class DistortionCorrectorComponent : public rclcpp::Node std::unique_ptr> stop_watch_ptr_; std::unique_ptr debug_publisher_; - std::deque twist_queue_; - std::deque angular_velocity_queue_; - tf2_ros::Buffer tf2_buffer{get_clock()}; std::string base_link_frame_ = "base_link"; @@ -67,28 +64,12 @@ class DistortionCorrectorComponent : public rclcpp::Node bool use_imu_; bool use_3d_distortion_correction_; - std::unique_ptr undistorter_; + std::unique_ptr undistorter_; void onPointCloud(PointCloud2::UniquePtr points_msg); void onTwistWithCovarianceStamped( const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg); void onImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg); - - void getTransformTF( - const std::string & target_frame, const std::string & source_frame, - tf2::Transform * tf2_transform_ptr); - - void undistortPointCloud(PointCloud2 & points); - - bool isInputValid(PointCloud2 & points); - - void getIteratorOfTwistAndIMU( - double first_point_time_stamp_sec, - std::deque::iterator & it_twist, - std::deque::iterator & it_imu); - - void warnIfTimestampsTooLate( - bool is_twist_time_stamp_too_late, bool is_imu_time_stamp_is_too_late); }; } // namespace pointcloud_preprocessor diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/undistorter.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/undistorter.hpp index 83547bb051abc..9863791bd315d 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/undistorter.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/undistorter.hpp @@ -51,31 +51,72 @@ namespace pointcloud_preprocessor { -class Undistorter + +class UndistorterBase +{ +public: + virtual void processTwistMessage( + const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) = 0; + virtual void processIMUMessage( + const std::string & base_link_frame, const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) = 0; + virtual void setPointCloudTransform( + const std::string & base_link_frame, const std::string & lidar_frame) = 0; + virtual void initialize() = 0; + virtual void undistortPointCloud(bool use_imu, sensor_msgs::msg::PointCloud2 & pointcloud) = 0; +}; + +template +class Undistorter : public UndistorterBase { public: bool is_pointcloud_transform_needed = false; bool is_pointcloud_transfrom_exist = false; bool is_imu_transfrom_exist = false; tf2_ros::Buffer & tf2_buffer; - geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr; + + std::deque twist_queue; + std::deque angular_velocity_queue; explicit Undistorter(tf2_ros::Buffer & buffer) : tf2_buffer(buffer) {} - void setIMUTransform(const std::string & target_frame, const std::string & source_frame); + void processTwistMessage( + const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) override; + + void processIMUMessage( + const std::string & base_link_frame, + const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) override; + void getIMUTransformation( + const std::string & base_link_frame, const std::string & imu_frame, + geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr); + void storeIMUToQueue( + const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg, + geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr); + + bool isInputValid(sensor_msgs::msg::PointCloud2 & pointcloud); + void getIteratorOfTwistAndIMU( + bool use_imu, double first_point_time_stamp_sec, + std::deque::iterator & it_twist, + std::deque::iterator & it_imu); + void undistortPointCloud(bool use_imu, sensor_msgs::msg::PointCloud2 & pointcloud) override; + void warnIfTimestampsTooLate( + bool is_twist_time_stamp_too_late, bool is_imu_time_stamp_is_too_late); virtual void initialize() = 0; - virtual void undistortPoint( + 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 & time_offset, - bool & is_twist_valid, bool & is_imu_valid) = 0; + bool & is_twist_valid, bool & is_imu_valid) + { + static_cast(this)->implementation( + it_x, it_y, it_z, it_twist, it_imu, time_offset, is_twist_valid, is_imu_valid); + }; virtual void setPointCloudTransform( const std::string & base_link_frame, const std::string & lidar_frame) = 0; }; -class Undistorter2D : public Undistorter +class Undistorter2D : public Undistorter { public: // defined outside of for loop for performance reasons. @@ -93,17 +134,18 @@ class Undistorter2D : public Undistorter explicit Undistorter2D(tf2_ros::Buffer & buffer) : Undistorter(buffer) {} void initialize() override; - void undistortPoint( + void 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, float & time_offset, - bool & is_twist_valid, bool & is_imu_valid) override; + bool & is_twist_valid, bool & is_imu_valid); - void setPointCloudTransform(const std::string & base_link_frame, const std::string & lidar_frame); + void setPointCloudTransform( + const std::string & base_link_frame, const std::string & lidar_frame) override; }; -class Undistorter3D : public Undistorter +class Undistorter3D : public Undistorter { public: // defined outside of for loop for performance reasons. @@ -118,13 +160,14 @@ class Undistorter3D : public Undistorter explicit Undistorter3D(tf2_ros::Buffer & buffer) : Undistorter(buffer) {} void initialize() override; - void undistortPoint( + void 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, float & time_offset, - bool & is_twist_valid, bool & is_imu_valid) override; - void setPointCloudTransform(const std::string & base_link_frame, const std::string & lidar_frame); + bool & is_twist_valid, bool & is_imu_valid); + void setPointCloudTransform( + const std::string & base_link_frame, const std::string & lidar_frame) override; }; } // namespace pointcloud_preprocessor diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index 2a92dc6d5acc3..6108d3aa5573a 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -45,7 +45,7 @@ DistortionCorrectorComponent::DistortionCorrectorComponent(const rclcpp::NodeOpt // Parameter time_stamp_field_name_ = declare_parameter("time_stamp_field_name", "time_stamp"); use_imu_ = declare_parameter("use_imu", true); - use_3d_distortion_correction_ = declare_parameter("use_3d_distortion_correction", false); + use_3d_distortion_correction_ = declare_parameter("use_3d_distortion_correction", true); // Publisher undistorted_points_pub_ = @@ -75,22 +75,7 @@ DistortionCorrectorComponent::DistortionCorrectorComponent(const rclcpp::NodeOpt void DistortionCorrectorComponent::onTwistWithCovarianceStamped( 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); - - 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(); - } - break; - } + undistorter_->processTwistMessage(twist_msg); } void DistortionCorrectorComponent::onImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) @@ -99,31 +84,7 @@ void DistortionCorrectorComponent::onImu(const sensor_msgs::msg::Imu::ConstShare return; } - if (!undistorter_->is_imu_transfrom_exist) - undistorter_->setIMUTransform(base_link_frame_, imu_msg->header.frame_id); - - geometry_msgs::msg::Vector3Stamped angular_velocity; - angular_velocity.vector = imu_msg->angular_velocity; - - geometry_msgs::msg::Vector3Stamped transformed_angular_velocity; - tf2::doTransform( - angular_velocity, transformed_angular_velocity, *undistorter_->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 ( - 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(); - } - break; - } + undistorter_->processIMUMessage(base_link_frame_, imu_msg); } void DistortionCorrectorComponent::onPointCloud(PointCloud2::UniquePtr pointcloud_msg) @@ -136,11 +97,10 @@ void DistortionCorrectorComponent::onPointCloud(PointCloud2::UniquePtr pointclou return; } - if (!undistorter_->is_pointcloud_transfrom_exist) - undistorter_->setPointCloudTransform(base_link_frame_, pointcloud_msg->header.frame_id); + undistorter_->setPointCloudTransform(base_link_frame_, pointcloud_msg->header.frame_id); undistorter_->initialize(); - undistortPointCloud(*pointcloud_msg); + undistorter_->undistortPointCloud(use_imu_, *pointcloud_msg); if (debug_publisher_) { auto pipeline_latency_ms = @@ -165,148 +125,6 @@ void DistortionCorrectorComponent::onPointCloud(PointCloud2::UniquePtr pointclou } } -bool DistortionCorrectorComponent::isInputValid(PointCloud2 & pointcloud) -{ - if (pointcloud.data.empty() || twist_queue_.empty()) { - RCLCPP_WARN_STREAM_THROTTLE( - get_logger(), *get_clock(), 10000 /* ms */, "input pointcloud or twist_queue_ is empty."); - return false; - } - - auto time_stamp_field_it = std::find_if( - std::cbegin(pointcloud.fields), std::cend(pointcloud.fields), - [this](const sensor_msgs::msg::PointField & field) { - return field.name == time_stamp_field_name_; - }); - if (time_stamp_field_it == pointcloud.fields.cend()) { - RCLCPP_WARN_STREAM_THROTTLE( - get_logger(), *get_clock(), 10000 /* ms */, - "Required field time stamp doesn't exist in the point cloud."); - return false; - } - return true; -} - -void DistortionCorrectorComponent::warnIfTimestampsTooLate( - bool is_twist_time_stamp_too_late, bool is_imu_time_stamp_is_too_late) -{ - if (is_twist_time_stamp_too_late) { - RCLCPP_WARN_STREAM_THROTTLE( - get_logger(), *get_clock(), 10000 /* ms */, - "twist time_stamp is too late. Could not interpolate."); - } - - if (is_imu_time_stamp_is_too_late) { - RCLCPP_WARN_STREAM_THROTTLE( - get_logger(), *get_clock(), 10000 /* ms */, - "imu time_stamp is too late. Could not interpolate."); - } -} - -void DistortionCorrectorComponent::getIteratorOfTwistAndIMU( - double first_point_time_stamp_sec, - std::deque::iterator & it_twist, - std::deque::iterator & it_imu) -{ - it_twist = std::lower_bound( - std::begin(twist_queue_), std::end(twist_queue_), first_point_time_stamp_sec, - [](const geometry_msgs::msg::TwistStamped & x, const double t) { - return rclcpp::Time(x.header.stamp).seconds() < t; - }); - it_twist = it_twist == std::end(twist_queue_) ? std::end(twist_queue_) - 1 : it_twist; - - if (use_imu_ && !angular_velocity_queue_.empty()) { - it_imu = std::lower_bound( - std::begin(angular_velocity_queue_), std::end(angular_velocity_queue_), - first_point_time_stamp_sec, [](const geometry_msgs::msg::Vector3Stamped & x, const double t) { - return rclcpp::Time(x.header.stamp).seconds() < t; - }); - it_imu = - it_imu == std::end(angular_velocity_queue_) ? std::end(angular_velocity_queue_) - 1 : it_imu; - } -} - -void DistortionCorrectorComponent::undistortPointCloud(PointCloud2 & pointcloud) -{ - if (!isInputValid(pointcloud)) return; - - sensor_msgs::PointCloud2Iterator it_x(pointcloud, "x"); - sensor_msgs::PointCloud2Iterator it_y(pointcloud, "y"); - sensor_msgs::PointCloud2Iterator it_z(pointcloud, "z"); - sensor_msgs::PointCloud2ConstIterator it_time_stamp(pointcloud, time_stamp_field_name_); - - double prev_time_stamp_sec{*it_time_stamp}; - const double first_point_time_stamp_sec{*it_time_stamp}; - - std::deque::iterator it_twist; - std::deque::iterator it_imu; - getIteratorOfTwistAndIMU(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(); - double imu_stamp{0.0}; - if (use_imu_ && !angular_velocity_queue_.empty()) { - imu_stamp = rclcpp::Time(it_imu->header.stamp).seconds(); - } - - // If there is a point in a pointlcoud that cannot be associated, record it to issue a warning - bool is_twist_time_stamp_too_late = false; - bool is_imu_time_stamp_is_too_late = false; - bool is_twist_valid = true; - bool is_imu_valid = true; - - for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_time_stamp) { - auto start = std::chrono::high_resolution_clock::now(); - is_twist_valid = true; - is_imu_valid = true; - - // Get closest twist information - while (it_twist != std::end(twist_queue_) - 1 && *it_time_stamp > twist_stamp) { - ++it_twist; - twist_stamp = rclcpp::Time(it_twist->header.stamp).seconds(); - } - if (std::abs(*it_time_stamp - twist_stamp) > 0.1) { - is_twist_time_stamp_too_late = true; - is_twist_valid = false; - } - - // Get closest IMU information - if (use_imu_ && !angular_velocity_queue_.empty()) { - while (it_imu != std::end(angular_velocity_queue_) - 1 && *it_time_stamp > imu_stamp) { - ++it_imu; - imu_stamp = rclcpp::Time(it_imu->header.stamp).seconds(); - } - - if (std::abs(*it_time_stamp - imu_stamp) > 0.1) { - is_imu_time_stamp_is_too_late = true; - is_imu_valid = false; - } - } else { - is_imu_valid = false; - } - - float time_offset = static_cast(*it_time_stamp - prev_time_stamp_sec); - - // std::cout << "before undistortPoint" << std::endl; - // std::cout << "it_x: " << *it_x << " it_y: " << *it_y << " it_z: " << *it_z << std::endl; - - // Undistorted a single point based on the strategy - undistorter_->undistortPoint( - it_x, it_y, it_z, it_twist, it_imu, time_offset, is_twist_valid, is_imu_valid); - - // std::cout << "after undistortPoint" << std::endl; - // std::cout << "it_x: " << *it_x << " it_y: " << *it_y << " it_z: " << *it_z << std::endl; - // std::cout << "//////////////////\n" << std::endl; - prev_time_stamp_sec = *it_time_stamp; - - auto end = std::chrono::high_resolution_clock::now(); - std::chrono::duration duration = end - start; - std::cout << "Function execution time: " << duration.count() << " seconds" << std::endl; - } - - warnIfTimestampsTooLate(is_twist_time_stamp_too_late, is_imu_time_stamp_is_too_late); -} - } // namespace pointcloud_preprocessor #include diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/undistorter.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/undistorter.cpp index 03d68ffb7cedf..e37e5f8c78804 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/undistorter.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/undistorter.cpp @@ -26,9 +26,47 @@ namespace pointcloud_preprocessor { -void Undistorter::setIMUTransform( - const std::string & base_link_frame, const std::string & imu_frame) +template +void Undistorter::processTwistMessage( + 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); + + 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(); + } + break; + } +} + +template +void Undistorter::processIMUMessage( + const std::string & base_link_frame, const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) +{ + geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr = + std::make_shared(); + getIMUTransformation(base_link_frame, imu_msg->header.frame_id, geometry_imu_to_base_link_ptr); + storeIMUToQueue(imu_msg, geometry_imu_to_base_link_ptr); +} + +template +void Undistorter::getIMUTransformation( + const std::string & base_link_frame, const std::string & imu_frame, + geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr) +{ + if (is_imu_transfrom_exist) { + return; + } + tf2::Transform tf2_imu_to_base_link; if (base_link_frame == imu_frame) { tf2_imu_to_base_link.setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); @@ -50,11 +88,183 @@ void Undistorter::setIMUTransform( } } - 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 Undistorter::storeIMUToQueue( + const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg, + geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr) +{ + geometry_msgs::msg::Vector3Stamped angular_velocity; + angular_velocity.vector = imu_msg->angular_velocity; + + 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 ( + 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(); + } + break; + } +} + +template +void Undistorter::getIteratorOfTwistAndIMU( + bool use_imu, double first_point_time_stamp_sec, + std::deque::iterator & it_twist, + std::deque::iterator & it_imu) +{ + it_twist = std::lower_bound( + std::begin(twist_queue), std::end(twist_queue), first_point_time_stamp_sec, + [](const geometry_msgs::msg::TwistStamped & x, const double t) { + return rclcpp::Time(x.header.stamp).seconds() < t; + }); + it_twist = it_twist == std::end(twist_queue) ? std::end(twist_queue) - 1 : it_twist; + + if (use_imu && !angular_velocity_queue.empty()) { + it_imu = std::lower_bound( + std::begin(angular_velocity_queue), std::end(angular_velocity_queue), + first_point_time_stamp_sec, [](const geometry_msgs::msg::Vector3Stamped & x, const double t) { + return rclcpp::Time(x.header.stamp).seconds() < t; + }); + it_imu = + it_imu == std::end(angular_velocity_queue) ? std::end(angular_velocity_queue) - 1 : it_imu; + } +} + +template +bool Undistorter::isInputValid(sensor_msgs::msg::PointCloud2 & pointcloud) +{ + if (pointcloud.data.empty() || twist_queue.empty()) { + // RCLCPP_WARN_STREAM_THROTTLE( + // get_logger(), *get_clock(), 10000 /* ms */, "input pointcloud or twist_queue_ is empty."); + return false; + } + + auto time_stamp_field_it = std::find_if( + std::cbegin(pointcloud.fields), std::cend(pointcloud.fields), + [](const sensor_msgs::msg::PointField & field) { return field.name == "time_stamp"; }); + if (time_stamp_field_it == pointcloud.fields.cend()) { + // RCLCPP_WARN_STREAM_THROTTLE( + // get_logger(), *get_clock(), 10000 /* ms */, + // "Required field time stamp doesn't exist in the point cloud."); + return false; + } + return true; +} + +template +void Undistorter::undistortPointCloud( + bool use_imu, sensor_msgs::msg::PointCloud2 & pointcloud) +{ + if (!isInputValid(pointcloud)) return; + + sensor_msgs::PointCloud2Iterator it_x(pointcloud, "x"); + sensor_msgs::PointCloud2Iterator it_y(pointcloud, "y"); + sensor_msgs::PointCloud2Iterator it_z(pointcloud, "z"); + sensor_msgs::PointCloud2ConstIterator it_time_stamp(pointcloud, "time_stamp"); + + double prev_time_stamp_sec{*it_time_stamp}; + const double first_point_time_stamp_sec{*it_time_stamp}; + + std::deque::iterator it_twist; + std::deque::iterator it_imu; + getIteratorOfTwistAndIMU(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(); + double imu_stamp{0.0}; + if (use_imu && !angular_velocity_queue.empty()) { + imu_stamp = rclcpp::Time(it_imu->header.stamp).seconds(); + } + + // If there is a point in a pointlcoud that cannot be associated, record it to issue a warning + bool is_twist_time_stamp_too_late = false; + bool is_imu_time_stamp_is_too_late = false; + bool is_twist_valid = true; + bool is_imu_valid = true; + + for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_time_stamp) { + is_twist_valid = true; + is_imu_valid = true; + + // Get closest twist information + while (it_twist != std::end(twist_queue) - 1 && *it_time_stamp > twist_stamp) { + ++it_twist; + twist_stamp = rclcpp::Time(it_twist->header.stamp).seconds(); + } + if (std::abs(*it_time_stamp - twist_stamp) > 0.1) { + is_twist_time_stamp_too_late = true; + is_twist_valid = false; + } + + // Get closest IMU information + if (use_imu && !angular_velocity_queue.empty()) { + while (it_imu != std::end(angular_velocity_queue) - 1 && *it_time_stamp > imu_stamp) { + ++it_imu; + imu_stamp = rclcpp::Time(it_imu->header.stamp).seconds(); + } + + if (std::abs(*it_time_stamp - imu_stamp) > 0.1) { + is_imu_time_stamp_is_too_late = true; + is_imu_valid = false; + } + } else { + is_imu_valid = false; + } + + float time_offset = static_cast(*it_time_stamp - prev_time_stamp_sec); + + // std::cout << "before undistortPoint" << std::endl; + // std::cout << "it_x: " << *it_x << " it_y: " << *it_y << " it_z: " << *it_z << std::endl; + + // Undistorted a single point based on the strategy + auto start = std::chrono::high_resolution_clock::now(); + undistortPoint(it_x, it_y, it_z, it_twist, it_imu, time_offset, is_twist_valid, is_imu_valid); + // std::cout << "after undistortPoint" << std::endl; + // std::cout << "it_x: " << *it_x << " it_y: " << *it_y << " it_z: " << *it_z << std::endl; + // std::cout << "//////////////////\n" << std::endl; + prev_time_stamp_sec = *it_time_stamp; + + auto end = std::chrono::high_resolution_clock::now(); + std::chrono::duration duration = end - start; + std::cout << "Function execution time: " << duration.count() << " seconds" << std::endl; + } + + warnIfTimestampsTooLate(is_twist_time_stamp_too_late, is_imu_time_stamp_is_too_late); +} + +template +void Undistorter::warnIfTimestampsTooLate( + bool is_twist_time_stamp_too_late, bool is_imu_time_stamp_is_too_late) +{ + if (is_twist_time_stamp_too_late) { + // RCLCPP_WARN_STREAM_THROTTLE( + // get_logger(), *get_clock(), 10000 /* ms */, + // "twist time_stamp is too late. Could not interpolate."); + std::cout << "twist time_stamp is too late. Could not interpolate." << std::endl; + } + + if (is_imu_time_stamp_is_too_late) { + // RCLCPP_WARN_STREAM_THROTTLE( + // get_logger(), *get_clock(), 10000 /* ms */, + // "imu time_stamp is too late. Could not interpolate."); + std::cout << "imu time_stamp is too late. Could not interpolate." << std::endl; + } +} + void Undistorter2D::initialize() { x = 0.0f; @@ -70,6 +280,10 @@ void Undistorter3D::initialize() void Undistorter2D::setPointCloudTransform( const std::string & base_link_frame, const std::string & lidar_frame) { + if (is_pointcloud_transfrom_exist) { + return; + } + if (base_link_frame == lidar_frame) { tf2_lidar_to_base_link.setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); tf2_lidar_to_base_link.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0)); @@ -98,6 +312,10 @@ void Undistorter2D::setPointCloudTransform( void Undistorter3D::setPointCloudTransform( const std::string & base_link_frame, const std::string & lidar_frame) { + if (is_pointcloud_transfrom_exist) { + return; + } + if (base_link_frame == lidar_frame) { eigen_lidar_to_base_link = Eigen::Matrix4f::Identity(); eigen_base_link_to_lidar = Eigen::Matrix4f::Identity(); @@ -121,7 +339,7 @@ void Undistorter3D::setPointCloudTransform( } } -void Undistorter2D::undistortPoint( +void Undistorter2D::implementation( sensor_msgs::PointCloud2Iterator & it_x, sensor_msgs::PointCloud2Iterator & it_y, sensor_msgs::PointCloud2Iterator & it_z, std::deque::iterator & it_twist, @@ -166,7 +384,7 @@ void Undistorter2D::undistortPoint( *it_z = static_cast(undistorted_point_tf.getZ()); } -void Undistorter3D::undistortPoint( +void Undistorter3D::implementation( sensor_msgs::PointCloud2Iterator & it_x, sensor_msgs::PointCloud2Iterator & it_y, sensor_msgs::PointCloud2Iterator & it_z, std::deque::iterator & it_twist, From b99018890ad4e8179bbc1a5a72267755a6b44c36 Mon Sep 17 00:00:00 2001 From: vividf Date: Fri, 24 May 2024 19:27:54 +0900 Subject: [PATCH 07/47] fix error Signed-off-by: vividf --- .../distortion_corrector/undistorter.hpp | 2 +- .../src/distortion_corrector/distortion_corrector.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/undistorter.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/undistorter.hpp index 9863791bd315d..64cd01356ecda 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/undistorter.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/undistorter.hpp @@ -145,7 +145,7 @@ class Undistorter2D : public Undistorter const std::string & base_link_frame, const std::string & lidar_frame) override; }; -class Undistorter3D : public Undistorter +class Undistorter3D : public Undistorter { public: // defined outside of for loop for performance reasons. diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index 6108d3aa5573a..31b7c15807230 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -45,7 +45,7 @@ DistortionCorrectorComponent::DistortionCorrectorComponent(const rclcpp::NodeOpt // Parameter time_stamp_field_name_ = declare_parameter("time_stamp_field_name", "time_stamp"); use_imu_ = declare_parameter("use_imu", true); - use_3d_distortion_correction_ = declare_parameter("use_3d_distortion_correction", true); + use_3d_distortion_correction_ = declare_parameter("use_3d_distortion_correction", false); // Publisher undistorted_points_pub_ = From 23e4ab945f2c75f493087f5b39be5bf00c2ade3f Mon Sep 17 00:00:00 2001 From: vividf Date: Sat, 25 May 2024 02:53:03 +0900 Subject: [PATCH 08/47] temporaily save --- .../src/distortion_corrector/undistorter.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/undistorter.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/undistorter.cpp index e37e5f8c78804..60d884669a144 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/undistorter.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/undistorter.cpp @@ -231,16 +231,16 @@ void Undistorter::undistortPointCloud( // std::cout << "it_x: " << *it_x << " it_y: " << *it_y << " it_z: " << *it_z << std::endl; // Undistorted a single point based on the strategy - auto start = std::chrono::high_resolution_clock::now(); + // auto start = std::chrono::high_resolution_clock::now(); undistortPoint(it_x, it_y, it_z, it_twist, it_imu, time_offset, is_twist_valid, is_imu_valid); // std::cout << "after undistortPoint" << std::endl; // std::cout << "it_x: " << *it_x << " it_y: " << *it_y << " it_z: " << *it_z << std::endl; // std::cout << "//////////////////\n" << std::endl; prev_time_stamp_sec = *it_time_stamp; - auto end = std::chrono::high_resolution_clock::now(); - std::chrono::duration duration = end - start; - std::cout << "Function execution time: " << duration.count() << " seconds" << std::endl; + // auto end = std::chrono::high_resolution_clock::now(); + // std::chrono::duration duration = end - start; + // std::cout << "Function execution time: " << duration.count() << " seconds" << std::endl; } warnIfTimestampsTooLate(is_twist_time_stamp_too_late, is_imu_time_stamp_is_too_late); From 3830fe64c42d079b72cdbefee627cc126932d3a7 Mon Sep 17 00:00:00 2001 From: vividf Date: Mon, 27 May 2024 14:44:28 +0900 Subject: [PATCH 09/47] clean code Signed-off-by: vividf --- .../distortion_corrector.hpp | 11 +- .../distortion_corrector/undistorter.hpp | 72 +++--- .../distortion_corrector.cpp | 16 +- .../src/distortion_corrector/undistorter.cpp | 244 +++++++++--------- 4 files changed, 158 insertions(+), 185 deletions(-) diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp index 90bde06aa841d..13d0254d4894c 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2024 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -15,15 +15,12 @@ #ifndef POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_HPP_ #define POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_HPP_ -#include #include -#include #include #include #include #include -#include // Include tier4 autoware utils #include "pointcloud_preprocessor/distortion_corrector/undistorter.hpp" @@ -31,12 +28,8 @@ #include #include -#include -#include #include -#include #include -#include namespace pointcloud_preprocessor { @@ -57,8 +50,6 @@ class DistortionCorrectorComponent : public rclcpp::Node std::unique_ptr> stop_watch_ptr_; std::unique_ptr debug_publisher_; - tf2_ros::Buffer tf2_buffer{get_clock()}; - std::string base_link_frame_ = "base_link"; std::string time_stamp_field_name_; bool use_imu_; diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/undistorter.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/undistorter.hpp index 64cd01356ecda..48d3960aa7c7a 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/undistorter.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/undistorter.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2024 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -35,19 +35,10 @@ #endif #include -#include - -// Include tier4 autoware utils -// delete unused -#include -#include #include -#include #include -#include #include -#include namespace pointcloud_preprocessor { @@ -69,15 +60,16 @@ template class Undistorter : public UndistorterBase { public: - bool is_pointcloud_transform_needed = false; - bool is_pointcloud_transfrom_exist = false; - bool is_imu_transfrom_exist = false; - tf2_ros::Buffer & tf2_buffer; + bool is_pointcloud_transform_needed_{false}; + bool is_pointcloud_transfrom_exist_{false}; + bool is_imu_transfrom_exist_{false}; + rclcpp::Node * node_; + std::unique_ptr tf2_buffer_ptr_; - std::deque twist_queue; - std::deque angular_velocity_queue; + std::deque twist_queue_; + std::deque angular_velocity_queue_; - explicit Undistorter(tf2_ros::Buffer & buffer) : tf2_buffer(buffer) {} + explicit Undistorter(rclcpp::Node * node); void processTwistMessage( const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) override; @@ -108,7 +100,7 @@ class Undistorter : public UndistorterBase std::deque::iterator & it_imu, float & time_offset, bool & is_twist_valid, bool & is_imu_valid) { - static_cast(this)->implementation( + static_cast(this)->undistortPointImplemenation( it_x, it_y, it_z, it_twist, it_imu, time_offset, is_twist_valid, is_imu_valid); }; @@ -118,23 +110,24 @@ class Undistorter : public UndistorterBase class Undistorter2D : public Undistorter { -public: +private: // defined outside of for loop for performance reasons. - tf2::Quaternion baselink_quat; - tf2::Transform baselink_tf_odom; - tf2::Vector3 point_tf; - tf2::Vector3 undistorted_point_tf; - float theta; - float x; - float y; + tf2::Quaternion baselink_quat_; + tf2::Transform baselink_tf_odom_; + tf2::Vector3 point_tf_; + tf2::Vector3 undistorted_point_tf_; + float theta_; + float x_; + float y_; // TF - tf2::Transform tf2_lidar_to_base_link; - tf2::Transform tf2_base_link_to_lidar; + tf2::Transform tf2_lidar_to_base_link_; + tf2::Transform tf2_base_link_to_lidar_; - explicit Undistorter2D(tf2_ros::Buffer & buffer) : Undistorter(buffer) {} +public: + explicit Undistorter2D(rclcpp::Node * node) : Undistorter(node) {} void initialize() override; - void implementation( + void undistortPointImplemenation( sensor_msgs::PointCloud2Iterator & it_x, sensor_msgs::PointCloud2Iterator & it_y, sensor_msgs::PointCloud2Iterator & it_z, std::deque::iterator & it_twist, @@ -147,20 +140,21 @@ class Undistorter2D : public Undistorter class Undistorter3D : public Undistorter { -public: +private: // defined outside of for loop for performance reasons. - Eigen::Vector4f point_eigen; - Eigen::Vector4f undistorted_point_eigen; - Eigen::Matrix4f transformation_matrix; - Eigen::Matrix4f prev_transformation_matrix; + Eigen::Vector4f point_eigen_; + Eigen::Vector4f undistorted_point_eigen_; + Eigen::Matrix4f transformation_matrix_; + Eigen::Matrix4f prev_transformation_matrix_; // TF - Eigen::Matrix4f eigen_lidar_to_base_link; - Eigen::Matrix4f eigen_base_link_to_lidar; + Eigen::Matrix4f eigen_lidar_to_base_link_; + Eigen::Matrix4f eigen_base_link_to_lidar_; - explicit Undistorter3D(tf2_ros::Buffer & buffer) : Undistorter(buffer) {} +public: + explicit Undistorter3D(rclcpp::Node * node) : Undistorter(node) {} void initialize() override; - void implementation( + void undistortPointImplemenation( sensor_msgs::PointCloud2Iterator & it_x, sensor_msgs::PointCloud2Iterator & it_y, sensor_msgs::PointCloud2Iterator & it_z, std::deque::iterator & it_twist, diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index 31b7c15807230..2202efb951ec9 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2024 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -15,16 +15,6 @@ #include "pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp" #include "pointcloud_preprocessor/distortion_corrector/undistorter.hpp" -#include "tier4_autoware_utils/math/trigonometry.hpp" - -#include - -// delete this -#include -#include -#include -#include -#include namespace pointcloud_preprocessor { @@ -66,9 +56,9 @@ DistortionCorrectorComponent::DistortionCorrectorComponent(const rclcpp::NodeOpt // Setup the undistortor if (use_3d_distortion_correction_) { - undistorter_ = std::make_unique(tf2_buffer); + undistorter_ = std::make_unique(this); } else { - undistorter_ = std::make_unique(tf2_buffer); + undistorter_ = std::make_unique(this); } } diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/undistorter.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/undistorter.cpp index 60d884669a144..d5268a1a46c00 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/undistorter.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/undistorter.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2024 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -18,14 +18,16 @@ #include -#include -#include -#include -#include - namespace pointcloud_preprocessor { +template +Undistorter::Undistorter(rclcpp::Node * node) +{ + node_ = node; + tf2_buffer_ptr_ = std::make_unique(node->get_clock()); +} + template void Undistorter::processTwistMessage( const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) @@ -33,16 +35,16 @@ void Undistorter::processTwistMessage( geometry_msgs::msg::TwistStamped msg; msg.header = twist_msg->header; msg.twist = twist_msg->twist.twist; - twist_queue.push_back(msg); + twist_queue_.push_back(msg); - while (!twist_queue.empty()) { + 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(); + 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_queue_.front().header.stamp) < rclcpp::Time(twist_msg->header.stamp) - rclcpp::Duration::from_seconds(1.0)) { - twist_queue.pop_front(); + twist_queue_.pop_front(); } break; } @@ -63,7 +65,7 @@ void Undistorter::getIMUTransformation( const std::string & base_link_frame, const std::string & imu_frame, geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr) { - if (is_imu_transfrom_exist) { + if (is_imu_transfrom_exist_) { return; } @@ -71,17 +73,18 @@ void Undistorter::getIMUTransformation( if (base_link_frame == imu_frame) { tf2_imu_to_base_link.setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); tf2_imu_to_base_link.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0)); - is_imu_transfrom_exist = true; + is_imu_transfrom_exist_ = true; } else { try { const auto transform_msg = - tf2_buffer.lookupTransform(base_link_frame, imu_frame, tf2::TimePointZero); + tf2_buffer_ptr_->lookupTransform(base_link_frame, imu_frame, tf2::TimePointZero); tf2::convert(transform_msg.transform, tf2_imu_to_base_link); - is_imu_transfrom_exist = true; + is_imu_transfrom_exist_ = true; } catch (const tf2::TransformException & ex) { - // RCLCPP_WARN(get_logger(), "%s", ex.what()); - // RCLCPP_ERROR( - // get_logger(), "Please publish TF %s to %s", target_frame.c_str(), source_frame.c_str()); + RCLCPP_WARN(node_->get_logger(), "%s", ex.what()); + RCLCPP_ERROR( + node_->get_logger(), "Please publish TF %s to %s", base_link_frame.c_str(), + imu_frame.c_str()); tf2_imu_to_base_link.setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); tf2_imu_to_base_link.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0)); @@ -103,18 +106,18 @@ void Undistorter::storeIMUToQueue( 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); + angular_velocity_queue_.push_back(transformed_angular_velocity); - while (!angular_velocity_queue.empty()) { + while (!angular_velocity_queue_.empty()) { // for replay rosbag if ( - rclcpp::Time(angular_velocity_queue.front().header.stamp) > + rclcpp::Time(angular_velocity_queue_.front().header.stamp) > rclcpp::Time(imu_msg->header.stamp)) { - angular_velocity_queue.pop_front(); + angular_velocity_queue_.pop_front(); } else if ( // NOLINT - rclcpp::Time(angular_velocity_queue.front().header.stamp) < + 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(); + angular_velocity_queue_.pop_front(); } break; } @@ -127,29 +130,30 @@ void Undistorter::getIteratorOfTwistAndIMU( std::deque::iterator & it_imu) { it_twist = std::lower_bound( - std::begin(twist_queue), std::end(twist_queue), first_point_time_stamp_sec, + std::begin(twist_queue_), std::end(twist_queue_), first_point_time_stamp_sec, [](const geometry_msgs::msg::TwistStamped & x, const double t) { return rclcpp::Time(x.header.stamp).seconds() < t; }); - it_twist = it_twist == std::end(twist_queue) ? std::end(twist_queue) - 1 : it_twist; + it_twist = it_twist == std::end(twist_queue_) ? std::end(twist_queue_) - 1 : it_twist; - if (use_imu && !angular_velocity_queue.empty()) { + if (use_imu && !angular_velocity_queue_.empty()) { it_imu = std::lower_bound( - std::begin(angular_velocity_queue), std::end(angular_velocity_queue), + std::begin(angular_velocity_queue_), std::end(angular_velocity_queue_), first_point_time_stamp_sec, [](const geometry_msgs::msg::Vector3Stamped & x, const double t) { return rclcpp::Time(x.header.stamp).seconds() < t; }); it_imu = - it_imu == std::end(angular_velocity_queue) ? std::end(angular_velocity_queue) - 1 : it_imu; + it_imu == std::end(angular_velocity_queue_) ? std::end(angular_velocity_queue_) - 1 : it_imu; } } template bool Undistorter::isInputValid(sensor_msgs::msg::PointCloud2 & pointcloud) { - if (pointcloud.data.empty() || twist_queue.empty()) { - // RCLCPP_WARN_STREAM_THROTTLE( - // get_logger(), *get_clock(), 10000 /* ms */, "input pointcloud or twist_queue_ is empty."); + if (pointcloud.data.empty() || twist_queue_.empty()) { + RCLCPP_WARN_STREAM_THROTTLE( + node_->get_logger(), *node_->get_clock(), 10000 /* ms */, + "input pointcloud or twist_queue_ is empty."); return false; } @@ -157,9 +161,9 @@ bool Undistorter::isInputValid(sensor_msgs::msg::PointCloud2 & pointclo std::cbegin(pointcloud.fields), std::cend(pointcloud.fields), [](const sensor_msgs::msg::PointField & field) { return field.name == "time_stamp"; }); if (time_stamp_field_it == pointcloud.fields.cend()) { - // RCLCPP_WARN_STREAM_THROTTLE( - // get_logger(), *get_clock(), 10000 /* ms */, - // "Required field time stamp doesn't exist in the point cloud."); + RCLCPP_WARN_STREAM_THROTTLE( + node_->get_logger(), *node_->get_clock(), 10000 /* ms */, + "Required field time stamp doesn't exist in the point cloud."); return false; } return true; @@ -186,7 +190,7 @@ void Undistorter::undistortPointCloud( // For performance, do not instantiate `rclcpp::Time` inside of the for-loop double twist_stamp = rclcpp::Time(it_twist->header.stamp).seconds(); double imu_stamp{0.0}; - if (use_imu && !angular_velocity_queue.empty()) { + if (use_imu && !angular_velocity_queue_.empty()) { imu_stamp = rclcpp::Time(it_imu->header.stamp).seconds(); } @@ -201,7 +205,7 @@ void Undistorter::undistortPointCloud( is_imu_valid = true; // Get closest twist information - while (it_twist != std::end(twist_queue) - 1 && *it_time_stamp > twist_stamp) { + while (it_twist != std::end(twist_queue_) - 1 && *it_time_stamp > twist_stamp) { ++it_twist; twist_stamp = rclcpp::Time(it_twist->header.stamp).seconds(); } @@ -211,8 +215,8 @@ void Undistorter::undistortPointCloud( } // Get closest IMU information - if (use_imu && !angular_velocity_queue.empty()) { - while (it_imu != std::end(angular_velocity_queue) - 1 && *it_time_stamp > imu_stamp) { + if (use_imu && !angular_velocity_queue_.empty()) { + while (it_imu != std::end(angular_velocity_queue_) - 1 && *it_time_stamp > imu_stamp) { ++it_imu; imu_stamp = rclcpp::Time(it_imu->header.stamp).seconds(); } @@ -227,20 +231,10 @@ void Undistorter::undistortPointCloud( float time_offset = static_cast(*it_time_stamp - prev_time_stamp_sec); - // std::cout << "before undistortPoint" << std::endl; - // std::cout << "it_x: " << *it_x << " it_y: " << *it_y << " it_z: " << *it_z << std::endl; - // Undistorted a single point based on the strategy - // auto start = std::chrono::high_resolution_clock::now(); undistortPoint(it_x, it_y, it_z, it_twist, it_imu, time_offset, is_twist_valid, is_imu_valid); - // std::cout << "after undistortPoint" << std::endl; - // std::cout << "it_x: " << *it_x << " it_y: " << *it_y << " it_z: " << *it_z << std::endl; - // std::cout << "//////////////////\n" << std::endl; - prev_time_stamp_sec = *it_time_stamp; - // auto end = std::chrono::high_resolution_clock::now(); - // std::chrono::duration duration = end - start; - // std::cout << "Function execution time: " << duration.count() << " seconds" << std::endl; + prev_time_stamp_sec = *it_time_stamp; } warnIfTimestampsTooLate(is_twist_time_stamp_too_late, is_imu_time_stamp_is_too_late); @@ -251,60 +245,63 @@ void Undistorter::warnIfTimestampsTooLate( bool is_twist_time_stamp_too_late, bool is_imu_time_stamp_is_too_late) { if (is_twist_time_stamp_too_late) { - // RCLCPP_WARN_STREAM_THROTTLE( - // get_logger(), *get_clock(), 10000 /* ms */, - // "twist time_stamp is too late. Could not interpolate."); + RCLCPP_WARN_STREAM_THROTTLE( + node_->get_logger(), *node_->get_clock(), 10000 /* ms */, + "twist time_stamp is too late. Could not interpolate."); std::cout << "twist time_stamp is too late. Could not interpolate." << std::endl; } if (is_imu_time_stamp_is_too_late) { - // RCLCPP_WARN_STREAM_THROTTLE( - // get_logger(), *get_clock(), 10000 /* ms */, - // "imu time_stamp is too late. Could not interpolate."); + RCLCPP_WARN_STREAM_THROTTLE( + node_->get_logger(), *node_->get_clock(), 10000 /* ms */, + "imu time_stamp is too late. Could not interpolate."); std::cout << "imu time_stamp is too late. Could not interpolate." << std::endl; } } +///////////////////////// Functions for different undistortion strategies ///////////////////////// + void Undistorter2D::initialize() { - x = 0.0f; - y = 0.0f; - theta = 0.0f; + x_ = 0.0f; + y_ = 0.0f; + theta_ = 0.0f; } void Undistorter3D::initialize() { - prev_transformation_matrix = Eigen::Matrix4f::Identity(); + prev_transformation_matrix_ = Eigen::Matrix4f::Identity(); } void Undistorter2D::setPointCloudTransform( const std::string & base_link_frame, const std::string & lidar_frame) { - if (is_pointcloud_transfrom_exist) { + if (is_pointcloud_transfrom_exist_) { return; } if (base_link_frame == lidar_frame) { - tf2_lidar_to_base_link.setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); - tf2_lidar_to_base_link.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0)); - tf2_base_link_to_lidar = tf2_lidar_to_base_link; - is_pointcloud_transfrom_exist = true; + tf2_lidar_to_base_link_.setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); + tf2_lidar_to_base_link_.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0)); + tf2_base_link_to_lidar_ = tf2_lidar_to_base_link_; + is_pointcloud_transfrom_exist_ = true; } else { try { const auto transform_msg = - tf2_buffer.lookupTransform(base_link_frame, lidar_frame, tf2::TimePointZero); - tf2::convert(transform_msg.transform, tf2_lidar_to_base_link); - tf2_base_link_to_lidar = tf2_lidar_to_base_link.inverse(); - is_pointcloud_transfrom_exist = true; - is_pointcloud_transform_needed = true; + tf2_buffer_ptr_->lookupTransform(base_link_frame, lidar_frame, tf2::TimePointZero); + tf2::convert(transform_msg.transform, tf2_lidar_to_base_link_); + tf2_base_link_to_lidar_ = tf2_lidar_to_base_link_.inverse(); + is_pointcloud_transfrom_exist_ = true; + is_pointcloud_transform_needed_ = true; } catch (const tf2::TransformException & ex) { - // RCLCPP_WARN(get_logger(), "%s", ex.what()); - // RCLCPP_ERROR( - // get_logger(), "Please publish TF %s to %s", target_frame.c_str(), source_frame.c_str()); - - tf2_lidar_to_base_link.setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); - tf2_lidar_to_base_link.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0)); - tf2_base_link_to_lidar = tf2_lidar_to_base_link; + RCLCPP_WARN(node_->get_logger(), "%s", ex.what()); + RCLCPP_ERROR( + node_->get_logger(), "Please publish TF %s to %s", base_link_frame.c_str(), + lidar_frame.c_str()); + + tf2_lidar_to_base_link_.setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); + tf2_lidar_to_base_link_.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0)); + tf2_base_link_to_lidar_ = tf2_lidar_to_base_link_; } } } @@ -312,34 +309,35 @@ void Undistorter2D::setPointCloudTransform( void Undistorter3D::setPointCloudTransform( const std::string & base_link_frame, const std::string & lidar_frame) { - if (is_pointcloud_transfrom_exist) { + if (is_pointcloud_transfrom_exist_) { return; } if (base_link_frame == lidar_frame) { - eigen_lidar_to_base_link = Eigen::Matrix4f::Identity(); - eigen_base_link_to_lidar = Eigen::Matrix4f::Identity(); - is_pointcloud_transfrom_exist = true; + eigen_lidar_to_base_link_ = Eigen::Matrix4f::Identity(); + eigen_base_link_to_lidar_ = Eigen::Matrix4f::Identity(); + is_pointcloud_transfrom_exist_ = true; } try { const auto transform_msg = - tf2_buffer.lookupTransform(base_link_frame, lidar_frame, tf2::TimePointZero); - eigen_lidar_to_base_link = + tf2_buffer_ptr_->lookupTransform(base_link_frame, lidar_frame, tf2::TimePointZero); + eigen_lidar_to_base_link_ = tf2::transformToEigen(transform_msg.transform).matrix().cast(); - eigen_base_link_to_lidar = eigen_lidar_to_base_link.inverse(); - is_pointcloud_transfrom_exist = true; - is_pointcloud_transform_needed = true; + eigen_base_link_to_lidar_ = eigen_lidar_to_base_link_.inverse(); + is_pointcloud_transfrom_exist_ = true; + is_pointcloud_transform_needed_ = true; } catch (const tf2::TransformException & ex) { - // RCLCPP_WARN(get_logger(), "%s", ex.what()); - // RCLCPP_ERROR( - // get_logger(), "Please publish TF %s to %s", target_frame.c_str(), source_frame.c_str()); - eigen_lidar_to_base_link = Eigen::Matrix4f::Identity(); - eigen_base_link_to_lidar = Eigen::Matrix4f::Identity(); + RCLCPP_WARN(node_->get_logger(), "%s", ex.what()); + RCLCPP_ERROR( + node_->get_logger(), "Please publish TF %s to %s", base_link_frame.c_str(), + lidar_frame.c_str()); + eigen_lidar_to_base_link_ = Eigen::Matrix4f::Identity(); + eigen_base_link_to_lidar_ = Eigen::Matrix4f::Identity(); } } -void Undistorter2D::implementation( +void Undistorter2D::undistortPointImplemenation( sensor_msgs::PointCloud2Iterator & it_x, sensor_msgs::PointCloud2Iterator & it_y, sensor_msgs::PointCloud2Iterator & it_z, std::deque::iterator & it_twist, @@ -357,34 +355,34 @@ void Undistorter2D::implementation( } // Undistort point - point_tf.setValue(*it_x, *it_y, *it_z); + point_tf_.setValue(*it_x, *it_y, *it_z); - if (is_pointcloud_transform_needed) { - point_tf = tf2_lidar_to_base_link * point_tf; + if (is_pointcloud_transform_needed_) { + point_tf_ = tf2_lidar_to_base_link_ * point_tf_; } - theta += w * time_offset; - baselink_quat.setValue( - 0, 0, tier4_autoware_utils::sin(theta * 0.5f), - tier4_autoware_utils::cos(theta * 0.5f)); // baselink_quat.setRPY(0.0, 0.0, theta); + theta_ += w * time_offset; + baselink_quat_.setValue( + 0, 0, tier4_autoware_utils::sin(theta_ * 0.5f), + tier4_autoware_utils::cos(theta_ * 0.5f)); // baselink_quat.setRPY(0.0, 0.0, theta); const float dis = v * time_offset; - x += dis * tier4_autoware_utils::cos(theta); - y += dis * tier4_autoware_utils::sin(theta); + x_ += dis * tier4_autoware_utils::cos(theta_); + y_ += dis * tier4_autoware_utils::sin(theta_); - baselink_tf_odom.setOrigin(tf2::Vector3(x, y, 0.0)); - baselink_tf_odom.setRotation(baselink_quat); + baselink_tf_odom_.setOrigin(tf2::Vector3(x_, y_, 0.0)); + baselink_tf_odom_.setRotation(baselink_quat_); - undistorted_point_tf = baselink_tf_odom * point_tf; + undistorted_point_tf_ = baselink_tf_odom_ * point_tf_; - if (is_pointcloud_transform_needed) { - undistorted_point_tf = tf2_base_link_to_lidar * undistorted_point_tf; + if (is_pointcloud_transform_needed_) { + undistorted_point_tf_ = tf2_base_link_to_lidar_ * undistorted_point_tf_; } - *it_x = static_cast(undistorted_point_tf.getX()); - *it_y = static_cast(undistorted_point_tf.getY()); - *it_z = static_cast(undistorted_point_tf.getZ()); + *it_x = static_cast(undistorted_point_tf_.getX()); + *it_y = static_cast(undistorted_point_tf_.getY()); + *it_z = static_cast(undistorted_point_tf_.getZ()); } -void Undistorter3D::implementation( +void Undistorter3D::undistortPointImplemenation( sensor_msgs::PointCloud2Iterator & it_x, sensor_msgs::PointCloud2Iterator & it_y, sensor_msgs::PointCloud2Iterator & it_z, std::deque::iterator & it_twist, @@ -408,25 +406,25 @@ void Undistorter3D::implementation( } // Undistort point - point_eigen << *it_x, *it_y, *it_z, 1.0; - if (is_pointcloud_transform_needed) { - point_eigen = eigen_lidar_to_base_link * point_eigen; + point_eigen_ << *it_x, *it_y, *it_z, 1.0; + if (is_pointcloud_transform_needed_) { + 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_); twist = twist * time_offset; - transformation_matrix = Sophus::SE3f::exp(twist).matrix(); - transformation_matrix = transformation_matrix * prev_transformation_matrix; - undistorted_point_eigen = transformation_matrix * point_eigen; + transformation_matrix_ = Sophus::SE3f::exp(twist).matrix(); + transformation_matrix_ = transformation_matrix_ * prev_transformation_matrix_; + undistorted_point_eigen_ = transformation_matrix_ * point_eigen_; - if (is_pointcloud_transform_needed) { - undistorted_point_eigen = eigen_base_link_to_lidar * undistorted_point_eigen; + if (is_pointcloud_transform_needed_) { + undistorted_point_eigen_ = eigen_base_link_to_lidar_ * undistorted_point_eigen_; } - *it_x = undistorted_point_eigen[0]; - *it_y = undistorted_point_eigen[1]; - *it_z = undistorted_point_eigen[2]; + *it_x = undistorted_point_eigen_[0]; + *it_y = undistorted_point_eigen_[1]; + *it_z = undistorted_point_eigen_[2]; - prev_transformation_matrix = transformation_matrix; + prev_transformation_matrix_ = transformation_matrix_; } } // namespace pointcloud_preprocessor From bf07078c86506e40cd653b0c787dfddc15783c64 Mon Sep 17 00:00:00 2001 From: vividf Date: Mon, 27 May 2024 15:01:35 +0900 Subject: [PATCH 10/47] remove unused parameters Signed-off-by: vividf --- .../distortion_corrector/distortion_corrector.hpp | 11 ++++------- .../src/distortion_corrector/distortion_corrector.cpp | 1 - 2 files changed, 4 insertions(+), 8 deletions(-) diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp index 13d0254d4894c..a8f9e2652881c 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp @@ -15,19 +15,17 @@ #ifndef POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_HPP_ #define POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_HPP_ +#include "pointcloud_preprocessor/distortion_corrector/undistorter.hpp" + #include +#include +#include #include #include #include #include -// Include tier4 autoware utils -#include "pointcloud_preprocessor/distortion_corrector/undistorter.hpp" - -#include -#include - #include #include @@ -51,7 +49,6 @@ class DistortionCorrectorComponent : public rclcpp::Node std::unique_ptr debug_publisher_; std::string base_link_frame_ = "base_link"; - std::string time_stamp_field_name_; bool use_imu_; bool use_3d_distortion_correction_; diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index 2202efb951ec9..2ad245b2b602d 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -33,7 +33,6 @@ DistortionCorrectorComponent::DistortionCorrectorComponent(const rclcpp::NodeOpt } // Parameter - time_stamp_field_name_ = declare_parameter("time_stamp_field_name", "time_stamp"); use_imu_ = declare_parameter("use_imu", true); use_3d_distortion_correction_ = declare_parameter("use_3d_distortion_correction", false); From 93fa49f36406ca96168abe8cce580925173415cd Mon Sep 17 00:00:00 2001 From: vividf Date: Mon, 27 May 2024 16:04:08 +0900 Subject: [PATCH 11/47] fix constructor error Signed-off-by: vividf --- .../distortion_corrector/undistorter.hpp | 10 ++++++++-- .../src/distortion_corrector/undistorter.cpp | 16 +++------------- 2 files changed, 11 insertions(+), 15 deletions(-) diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/undistorter.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/undistorter.hpp index 48d3960aa7c7a..9a5b89c77e3fd 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/undistorter.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/undistorter.hpp @@ -35,6 +35,8 @@ #endif #include +#include +#include #include #include @@ -64,12 +66,16 @@ class Undistorter : public UndistorterBase bool is_pointcloud_transfrom_exist_{false}; bool is_imu_transfrom_exist_{false}; rclcpp::Node * node_; - std::unique_ptr tf2_buffer_ptr_; + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; std::deque twist_queue_; std::deque angular_velocity_queue_; - explicit Undistorter(rclcpp::Node * node); + explicit Undistorter(rclcpp::Node * node) + : node_(node), tf_buffer_(node_->get_clock()), tf_listener_(tf_buffer_) + { + } void processTwistMessage( const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) override; diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/undistorter.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/undistorter.cpp index d5268a1a46c00..a5eff81527b10 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/undistorter.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/undistorter.cpp @@ -20,14 +20,6 @@ namespace pointcloud_preprocessor { - -template -Undistorter::Undistorter(rclcpp::Node * node) -{ - node_ = node; - tf2_buffer_ptr_ = std::make_unique(node->get_clock()); -} - template void Undistorter::processTwistMessage( const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) @@ -77,7 +69,7 @@ void Undistorter::getIMUTransformation( } else { try { const auto transform_msg = - tf2_buffer_ptr_->lookupTransform(base_link_frame, imu_frame, tf2::TimePointZero); + tf_buffer_.lookupTransform(base_link_frame, imu_frame, tf2::TimePointZero); tf2::convert(transform_msg.transform, tf2_imu_to_base_link); is_imu_transfrom_exist_ = true; } catch (const tf2::TransformException & ex) { @@ -248,14 +240,12 @@ void Undistorter::warnIfTimestampsTooLate( RCLCPP_WARN_STREAM_THROTTLE( node_->get_logger(), *node_->get_clock(), 10000 /* ms */, "twist time_stamp is too late. Could not interpolate."); - std::cout << "twist time_stamp is too late. Could not interpolate." << std::endl; } if (is_imu_time_stamp_is_too_late) { RCLCPP_WARN_STREAM_THROTTLE( node_->get_logger(), *node_->get_clock(), 10000 /* ms */, "imu time_stamp is too late. Could not interpolate."); - std::cout << "imu time_stamp is too late. Could not interpolate." << std::endl; } } @@ -288,7 +278,7 @@ void Undistorter2D::setPointCloudTransform( } else { try { const auto transform_msg = - tf2_buffer_ptr_->lookupTransform(base_link_frame, lidar_frame, tf2::TimePointZero); + tf_buffer_.lookupTransform(base_link_frame, lidar_frame, tf2::TimePointZero); tf2::convert(transform_msg.transform, tf2_lidar_to_base_link_); tf2_base_link_to_lidar_ = tf2_lidar_to_base_link_.inverse(); is_pointcloud_transfrom_exist_ = true; @@ -321,7 +311,7 @@ void Undistorter3D::setPointCloudTransform( try { const auto transform_msg = - tf2_buffer_ptr_->lookupTransform(base_link_frame, lidar_frame, tf2::TimePointZero); + tf_buffer_.lookupTransform(base_link_frame, lidar_frame, tf2::TimePointZero); eigen_lidar_to_base_link_ = tf2::transformToEigen(transform_msg.transform).matrix().cast(); eigen_base_link_to_lidar_ = eigen_lidar_to_base_link_.inverse(); From 40234098c40931abefaa2dc11964924eaa91714c Mon Sep 17 00:00:00 2001 From: vividf Date: Mon, 27 May 2024 17:34:49 +0900 Subject: [PATCH 12/47] fix spell errors Signed-off-by: vividf --- .../distortion_corrector/undistorter.hpp | 10 ++++----- .../src/distortion_corrector/undistorter.cpp | 22 +++++++++---------- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/undistorter.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/undistorter.hpp index 9a5b89c77e3fd..565050b84f35f 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/undistorter.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/undistorter.hpp @@ -63,8 +63,8 @@ class Undistorter : public UndistorterBase { public: bool is_pointcloud_transform_needed_{false}; - bool is_pointcloud_transfrom_exist_{false}; - bool is_imu_transfrom_exist_{false}; + bool is_pointcloud_transform_exist_{false}; + bool is_imu_transform_exist_{false}; rclcpp::Node * node_; tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; @@ -106,7 +106,7 @@ class Undistorter : public UndistorterBase std::deque::iterator & it_imu, float & time_offset, bool & is_twist_valid, bool & is_imu_valid) { - static_cast(this)->undistortPointImplemenation( + static_cast(this)->undistortPointImplemention( it_x, it_y, it_z, it_twist, it_imu, time_offset, is_twist_valid, is_imu_valid); }; @@ -133,7 +133,7 @@ class Undistorter2D : public Undistorter public: explicit Undistorter2D(rclcpp::Node * node) : Undistorter(node) {} void initialize() override; - void undistortPointImplemenation( + void undistortPointImplemention( sensor_msgs::PointCloud2Iterator & it_x, sensor_msgs::PointCloud2Iterator & it_y, sensor_msgs::PointCloud2Iterator & it_z, std::deque::iterator & it_twist, @@ -160,7 +160,7 @@ class Undistorter3D : public Undistorter public: explicit Undistorter3D(rclcpp::Node * node) : Undistorter(node) {} void initialize() override; - void undistortPointImplemenation( + void undistortPointImplemention( sensor_msgs::PointCloud2Iterator & it_x, sensor_msgs::PointCloud2Iterator & it_y, sensor_msgs::PointCloud2Iterator & it_z, std::deque::iterator & it_twist, diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/undistorter.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/undistorter.cpp index a5eff81527b10..77a4a50317c7e 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/undistorter.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/undistorter.cpp @@ -57,7 +57,7 @@ void Undistorter::getIMUTransformation( const std::string & base_link_frame, const std::string & imu_frame, geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr) { - if (is_imu_transfrom_exist_) { + if (is_imu_transform_exist_) { return; } @@ -65,13 +65,13 @@ void Undistorter::getIMUTransformation( if (base_link_frame == imu_frame) { tf2_imu_to_base_link.setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); tf2_imu_to_base_link.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0)); - is_imu_transfrom_exist_ = true; + is_imu_transform_exist_ = true; } else { try { const auto transform_msg = tf_buffer_.lookupTransform(base_link_frame, imu_frame, tf2::TimePointZero); tf2::convert(transform_msg.transform, tf2_imu_to_base_link); - is_imu_transfrom_exist_ = true; + is_imu_transform_exist_ = true; } catch (const tf2::TransformException & ex) { RCLCPP_WARN(node_->get_logger(), "%s", ex.what()); RCLCPP_ERROR( @@ -266,7 +266,7 @@ void Undistorter3D::initialize() void Undistorter2D::setPointCloudTransform( const std::string & base_link_frame, const std::string & lidar_frame) { - if (is_pointcloud_transfrom_exist_) { + if (is_pointcloud_transform_exist_) { return; } @@ -274,14 +274,14 @@ void Undistorter2D::setPointCloudTransform( tf2_lidar_to_base_link_.setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); tf2_lidar_to_base_link_.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0)); tf2_base_link_to_lidar_ = tf2_lidar_to_base_link_; - is_pointcloud_transfrom_exist_ = true; + is_pointcloud_transform_exist_ = true; } else { try { const auto transform_msg = tf_buffer_.lookupTransform(base_link_frame, lidar_frame, tf2::TimePointZero); tf2::convert(transform_msg.transform, tf2_lidar_to_base_link_); tf2_base_link_to_lidar_ = tf2_lidar_to_base_link_.inverse(); - is_pointcloud_transfrom_exist_ = true; + is_pointcloud_transform_exist_ = true; is_pointcloud_transform_needed_ = true; } catch (const tf2::TransformException & ex) { RCLCPP_WARN(node_->get_logger(), "%s", ex.what()); @@ -299,14 +299,14 @@ void Undistorter2D::setPointCloudTransform( void Undistorter3D::setPointCloudTransform( const std::string & base_link_frame, const std::string & lidar_frame) { - if (is_pointcloud_transfrom_exist_) { + if (is_pointcloud_transform_exist_) { return; } if (base_link_frame == lidar_frame) { eigen_lidar_to_base_link_ = Eigen::Matrix4f::Identity(); eigen_base_link_to_lidar_ = Eigen::Matrix4f::Identity(); - is_pointcloud_transfrom_exist_ = true; + is_pointcloud_transform_exist_ = true; } try { @@ -315,7 +315,7 @@ void Undistorter3D::setPointCloudTransform( eigen_lidar_to_base_link_ = tf2::transformToEigen(transform_msg.transform).matrix().cast(); eigen_base_link_to_lidar_ = eigen_lidar_to_base_link_.inverse(); - is_pointcloud_transfrom_exist_ = true; + is_pointcloud_transform_exist_ = true; is_pointcloud_transform_needed_ = true; } catch (const tf2::TransformException & ex) { RCLCPP_WARN(node_->get_logger(), "%s", ex.what()); @@ -327,7 +327,7 @@ void Undistorter3D::setPointCloudTransform( } } -void Undistorter2D::undistortPointImplemenation( +void Undistorter2D::undistortPointImplemention( sensor_msgs::PointCloud2Iterator & it_x, sensor_msgs::PointCloud2Iterator & it_y, sensor_msgs::PointCloud2Iterator & it_z, std::deque::iterator & it_twist, @@ -372,7 +372,7 @@ void Undistorter2D::undistortPointImplemenation( *it_z = static_cast(undistorted_point_tf_.getZ()); } -void Undistorter3D::undistortPointImplemenation( +void Undistorter3D::undistortPointImplemention( sensor_msgs::PointCloud2Iterator & it_x, sensor_msgs::PointCloud2Iterator & it_y, sensor_msgs::PointCloud2Iterator & it_z, std::deque::iterator & it_twist, From eb420a2b948bc894ccb550ae04dfad8f7b096be4 Mon Sep 17 00:00:00 2001 From: vividf Date: Mon, 27 May 2024 17:43:20 +0900 Subject: [PATCH 13/47] fix more spell errors Signed-off-by: vividf --- .../distortion_corrector/undistorter.hpp | 6 +++--- .../src/distortion_corrector/undistorter.cpp | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/undistorter.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/undistorter.hpp index 565050b84f35f..131709647a3c7 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/undistorter.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/undistorter.hpp @@ -106,7 +106,7 @@ class Undistorter : public UndistorterBase std::deque::iterator & it_imu, float & time_offset, bool & is_twist_valid, bool & is_imu_valid) { - static_cast(this)->undistortPointImplemention( + static_cast(this)->undistortPointImplementation( it_x, it_y, it_z, it_twist, it_imu, time_offset, is_twist_valid, is_imu_valid); }; @@ -133,7 +133,7 @@ class Undistorter2D : public Undistorter public: explicit Undistorter2D(rclcpp::Node * node) : Undistorter(node) {} void initialize() override; - void undistortPointImplemention( + void undistortPointImplementation( sensor_msgs::PointCloud2Iterator & it_x, sensor_msgs::PointCloud2Iterator & it_y, sensor_msgs::PointCloud2Iterator & it_z, std::deque::iterator & it_twist, @@ -160,7 +160,7 @@ class Undistorter3D : public Undistorter public: explicit Undistorter3D(rclcpp::Node * node) : Undistorter(node) {} void initialize() override; - void undistortPointImplemention( + void undistortPointImplementation( sensor_msgs::PointCloud2Iterator & it_x, sensor_msgs::PointCloud2Iterator & it_y, sensor_msgs::PointCloud2Iterator & it_z, std::deque::iterator & it_twist, diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/undistorter.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/undistorter.cpp index 77a4a50317c7e..64eccbea0654a 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/undistorter.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/undistorter.cpp @@ -186,7 +186,7 @@ void Undistorter::undistortPointCloud( imu_stamp = rclcpp::Time(it_imu->header.stamp).seconds(); } - // If there is a point in a pointlcoud that cannot be associated, record it to issue a warning + // If there is a point in a pointcloud that cannot be associated, record it to issue a warning bool is_twist_time_stamp_too_late = false; bool is_imu_time_stamp_is_too_late = false; bool is_twist_valid = true; @@ -327,7 +327,7 @@ void Undistorter3D::setPointCloudTransform( } } -void Undistorter2D::undistortPointImplemention( +void Undistorter2D::undistortPointImplementation( sensor_msgs::PointCloud2Iterator & it_x, sensor_msgs::PointCloud2Iterator & it_y, sensor_msgs::PointCloud2Iterator & it_z, std::deque::iterator & it_twist, @@ -372,7 +372,7 @@ void Undistorter2D::undistortPointImplemention( *it_z = static_cast(undistorted_point_tf_.getZ()); } -void Undistorter3D::undistortPointImplemention( +void Undistorter3D::undistortPointImplementation( sensor_msgs::PointCloud2Iterator & it_x, sensor_msgs::PointCloud2Iterator & it_y, sensor_msgs::PointCloud2Iterator & it_z, std::deque::iterator & it_twist, From 8e8dfb968d6a4c0d2183e9e457b87fd3662e46bc Mon Sep 17 00:00:00 2001 From: vividf Date: Wed, 29 May 2024 10:42:56 +0900 Subject: [PATCH 14/47] add undistorter to library Signed-off-by: vividf --- .../pointcloud_preprocessor/CMakeLists.txt | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) diff --git a/sensing/pointcloud_preprocessor/CMakeLists.txt b/sensing/pointcloud_preprocessor/CMakeLists.txt index ca20b985fc7a0..2758ecc332c2b 100644 --- a/sensing/pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/pointcloud_preprocessor/CMakeLists.txt @@ -58,6 +58,23 @@ ament_target_dependencies(faster_voxel_grid_downsample_filter sensor_msgs ) + +add_library(undistorter SHARED + src/distortion_corrector/undistorter.cpp +) + +target_include_directories(undistorter PUBLIC + "$" + "$" +) + +ament_target_dependencies(undistorter + rclcpp + sensor_msgs + tf2_eigen + tier4_autoware_utils +) + ament_auto_add_library(pointcloud_preprocessor_filter SHARED src/utility/utilities.cpp src/concatenate_data/concatenate_and_time_sync_nodelet.cpp @@ -78,7 +95,6 @@ ament_auto_add_library(pointcloud_preprocessor_filter SHARED src/pointcloud_accumulator/pointcloud_accumulator_nodelet.cpp src/vector_map_filter/lanelet2_map_filter_nodelet.cpp src/distortion_corrector/distortion_corrector.cpp - src/distortion_corrector/undistorter.cpp src/blockage_diag/blockage_diag_nodelet.cpp src/polygon_remover/polygon_remover.cpp src/vector_map_filter/vector_map_inside_area_filter.cpp @@ -87,6 +103,7 @@ ament_auto_add_library(pointcloud_preprocessor_filter SHARED target_link_libraries(pointcloud_preprocessor_filter pointcloud_preprocessor_filter_base faster_voxel_grid_downsample_filter + undistorter ${Boost_LIBRARIES} ${OpenCV_LIBRARIES} ${Sophus_LIBRARIES} From d8bbd59f4293a571e965f84dc23af39a5af40fea Mon Sep 17 00:00:00 2001 From: vividf Date: Wed, 12 Jun 2024 20:30:31 +0900 Subject: [PATCH 15/47] fix: fix cmake and change class name Signed-off-by: vividf --- .../pointcloud_preprocessor/CMakeLists.txt | 19 +- .../distortion_corrector.hpp | 154 +++++- .../distortion_corrector_nodelet.hpp | 65 +++ .../distortion_corrector/undistorter.hpp | 175 ------- .../distortion_corrector.cpp | 448 +++++++++++++++--- .../distortion_corrector_nodelet.cpp | 120 +++++ .../src/distortion_corrector/undistorter.cpp | 420 ---------------- 7 files changed, 692 insertions(+), 709 deletions(-) create mode 100644 sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector_nodelet.hpp delete mode 100644 sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/undistorter.hpp create mode 100644 sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_nodelet.cpp delete mode 100644 sensing/pointcloud_preprocessor/src/distortion_corrector/undistorter.cpp diff --git a/sensing/pointcloud_preprocessor/CMakeLists.txt b/sensing/pointcloud_preprocessor/CMakeLists.txt index 01ebd3292a96b..73aa8ade80b19 100644 --- a/sensing/pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/pointcloud_preprocessor/CMakeLists.txt @@ -58,23 +58,6 @@ ament_target_dependencies(faster_voxel_grid_downsample_filter sensor_msgs ) - -add_library(undistorter SHARED - src/distortion_corrector/undistorter.cpp -) - -target_include_directories(undistorter PUBLIC - "$" - "$" -) - -ament_target_dependencies(undistorter - rclcpp - sensor_msgs - tf2_eigen - tier4_autoware_utils -) - ament_auto_add_library(pointcloud_preprocessor_filter SHARED src/utility/utilities.cpp src/concatenate_data/concatenate_and_time_sync_nodelet.cpp @@ -95,6 +78,7 @@ ament_auto_add_library(pointcloud_preprocessor_filter SHARED src/pointcloud_accumulator/pointcloud_accumulator_nodelet.cpp src/vector_map_filter/lanelet2_map_filter_nodelet.cpp src/distortion_corrector/distortion_corrector.cpp + src/distortion_corrector/distortion_corrector_nodelet.cpp src/blockage_diag/blockage_diag_nodelet.cpp src/polygon_remover/polygon_remover.cpp src/vector_map_filter/vector_map_inside_area_filter.cpp @@ -103,7 +87,6 @@ ament_auto_add_library(pointcloud_preprocessor_filter SHARED target_link_libraries(pointcloud_preprocessor_filter pointcloud_preprocessor_filter_base faster_voxel_grid_downsample_filter - undistorter ${Boost_LIBRARIES} ${OpenCV_LIBRARIES} ${Sophus_LIBRARIES} diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp index a8f9e2652881c..4afa430160280 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp @@ -15,49 +15,159 @@ #ifndef POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_HPP_ #define POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_HPP_ -#include "pointcloud_preprocessor/distortion_corrector/undistorter.hpp" - +#include #include -#include -#include +#include #include #include #include #include +#include + +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif +#include +#include +#include + +#include #include #include namespace pointcloud_preprocessor { -using rcl_interfaces::msg::SetParametersResult; -using sensor_msgs::msg::PointCloud2; -class DistortionCorrectorComponent : public rclcpp::Node +class DistortionCorrectorBase +{ +public: + virtual void processTwistMessage( + const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) = 0; + virtual void processIMUMessage( + const std::string & base_link_frame, const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) = 0; + virtual void setPointCloudTransform( + const std::string & base_link_frame, const std::string & lidar_frame) = 0; + virtual void initialize() = 0; + virtual void undistortPointCloud(bool use_imu, sensor_msgs::msg::PointCloud2 & pointcloud) = 0; +}; + +template +class DistortionCorrector : public DistortionCorrectorBase { public: - explicit DistortionCorrectorComponent(const rclcpp::NodeOptions & options); + bool is_pointcloud_transform_needed_{false}; + bool is_pointcloud_transform_exist_{false}; + bool is_imu_transform_exist_{false}; + rclcpp::Node * node_; + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + std::deque twist_queue_; + std::deque angular_velocity_queue_; + + explicit DistortionCorrector(rclcpp::Node * node) + : node_(node), tf_buffer_(node_->get_clock()), tf_listener_(tf_buffer_) + { + } + void processTwistMessage( + const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) override; + + void processIMUMessage( + const std::string & base_link_frame, + const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) override; + void getIMUTransformation( + const std::string & base_link_frame, const std::string & imu_frame, + geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr); + void storeIMUToQueue( + const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg, + geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr); + + bool isInputValid(sensor_msgs::msg::PointCloud2 & pointcloud); + void getIteratorOfTwistAndIMU( + bool use_imu, double first_point_time_stamp_sec, + std::deque::iterator & it_twist, + std::deque::iterator & it_imu); + void undistortPointCloud(bool use_imu, sensor_msgs::msg::PointCloud2 & pointcloud) override; + void warnIfTimestampsTooLate( + bool is_twist_time_stamp_too_late, bool is_imu_time_stamp_is_too_late); + virtual void initialize() = 0; + 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 & time_offset, + bool & is_twist_valid, 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); + }; + + virtual void setPointCloudTransform( + const std::string & base_link_frame, const std::string & lidar_frame) = 0; +}; + +class DistortionCorrector2D : public DistortionCorrector +{ private: - rclcpp::Subscription::SharedPtr input_points_sub_; - rclcpp::Subscription::SharedPtr imu_sub_; - rclcpp::Subscription::SharedPtr twist_sub_; - rclcpp::Publisher::SharedPtr undistorted_points_pub_; + // defined outside of for loop for performance reasons. + tf2::Quaternion baselink_quat_; + tf2::Transform baselink_tf_odom_; + tf2::Vector3 point_tf_; + tf2::Vector3 undistorted_point_tf_; + float theta_; + float x_; + float y_; - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr debug_publisher_; + // TF + tf2::Transform tf2_lidar_to_base_link_; + tf2::Transform tf2_base_link_to_lidar_; - std::string base_link_frame_ = "base_link"; - bool use_imu_; - bool use_3d_distortion_correction_; +public: + explicit DistortionCorrector2D(rclcpp::Node * node) : DistortionCorrector(node) {} + void initialize() override; + void undistortPointImplementation( + 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 & time_offset, + bool & is_twist_valid, bool & is_imu_valid); - std::unique_ptr undistorter_; + void setPointCloudTransform( + const std::string & base_link_frame, const std::string & lidar_frame) override; +}; - void onPointCloud(PointCloud2::UniquePtr points_msg); - void onTwistWithCovarianceStamped( - const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg); - void onImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg); +class DistortionCorrector3D : public DistortionCorrector +{ +private: + // defined outside of for loop for performance reasons. + Eigen::Vector4f point_eigen_; + Eigen::Vector4f undistorted_point_eigen_; + Eigen::Matrix4f transformation_matrix_; + Eigen::Matrix4f prev_transformation_matrix_; + + // TF + Eigen::Matrix4f eigen_lidar_to_base_link_; + Eigen::Matrix4f eigen_base_link_to_lidar_; + +public: + explicit DistortionCorrector3D(rclcpp::Node * node) : DistortionCorrector(node) {} + void initialize() override; + void undistortPointImplementation( + 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 & time_offset, + bool & is_twist_valid, bool & is_imu_valid); + void setPointCloudTransform( + const std::string & base_link_frame, const std::string & lidar_frame) override; }; } // namespace pointcloud_preprocessor diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector_nodelet.hpp new file mode 100644 index 0000000000000..efa137bd3ccf4 --- /dev/null +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector_nodelet.hpp @@ -0,0 +1,65 @@ +// Copyright 2024 Tier IV, Inc. +// +// 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. + +#ifndef POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_NODELET_HPP_ +#define POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_NODELET_HPP_ + +#include "pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp" + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +namespace pointcloud_preprocessor +{ +using rcl_interfaces::msg::SetParametersResult; +using sensor_msgs::msg::PointCloud2; + +class DistortionCorrectorComponent : public rclcpp::Node +{ +public: + explicit DistortionCorrectorComponent(const rclcpp::NodeOptions & options); + +private: + rclcpp::Subscription::SharedPtr input_points_sub_; + rclcpp::Subscription::SharedPtr imu_sub_; + rclcpp::Subscription::SharedPtr twist_sub_; + rclcpp::Publisher::SharedPtr undistorted_points_pub_; + + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; + + std::string base_link_frame_ = "base_link"; + bool use_imu_; + bool use_3d_distortion_correction_; + + std::unique_ptr DistortionCorrector_; + + void onPointCloud(PointCloud2::UniquePtr points_msg); + void onTwistWithCovarianceStamped( + const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg); + void onImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg); +}; + +} // namespace pointcloud_preprocessor + +#endif // POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_NODELET_HPP_ diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/undistorter.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/undistorter.hpp deleted file mode 100644 index 131709647a3c7..0000000000000 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/undistorter.hpp +++ /dev/null @@ -1,175 +0,0 @@ -// Copyright 2024 Tier IV, Inc. -// -// 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. - -#ifndef POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__UNDISTORTER_HPP_ -#define POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__UNDISTORTER_HPP_ - -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include - -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - -#include -#include -#include - -#include -#include -#include - -namespace pointcloud_preprocessor -{ - -class UndistorterBase -{ -public: - virtual void processTwistMessage( - const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) = 0; - virtual void processIMUMessage( - const std::string & base_link_frame, const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) = 0; - virtual void setPointCloudTransform( - const std::string & base_link_frame, const std::string & lidar_frame) = 0; - virtual void initialize() = 0; - virtual void undistortPointCloud(bool use_imu, sensor_msgs::msg::PointCloud2 & pointcloud) = 0; -}; - -template -class Undistorter : public UndistorterBase -{ -public: - bool is_pointcloud_transform_needed_{false}; - bool is_pointcloud_transform_exist_{false}; - bool is_imu_transform_exist_{false}; - rclcpp::Node * node_; - tf2_ros::Buffer tf_buffer_; - tf2_ros::TransformListener tf_listener_; - - std::deque twist_queue_; - std::deque angular_velocity_queue_; - - explicit Undistorter(rclcpp::Node * node) - : node_(node), tf_buffer_(node_->get_clock()), tf_listener_(tf_buffer_) - { - } - void processTwistMessage( - const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) override; - - void processIMUMessage( - const std::string & base_link_frame, - const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) override; - void getIMUTransformation( - const std::string & base_link_frame, const std::string & imu_frame, - geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr); - void storeIMUToQueue( - const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg, - geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr); - - bool isInputValid(sensor_msgs::msg::PointCloud2 & pointcloud); - void getIteratorOfTwistAndIMU( - bool use_imu, double first_point_time_stamp_sec, - std::deque::iterator & it_twist, - std::deque::iterator & it_imu); - void undistortPointCloud(bool use_imu, sensor_msgs::msg::PointCloud2 & pointcloud) override; - void warnIfTimestampsTooLate( - bool is_twist_time_stamp_too_late, bool is_imu_time_stamp_is_too_late); - - virtual void initialize() = 0; - 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 & time_offset, - bool & is_twist_valid, 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); - }; - - virtual void setPointCloudTransform( - const std::string & base_link_frame, const std::string & lidar_frame) = 0; -}; - -class Undistorter2D : public Undistorter -{ -private: - // defined outside of for loop for performance reasons. - tf2::Quaternion baselink_quat_; - tf2::Transform baselink_tf_odom_; - tf2::Vector3 point_tf_; - tf2::Vector3 undistorted_point_tf_; - float theta_; - float x_; - float y_; - - // TF - tf2::Transform tf2_lidar_to_base_link_; - tf2::Transform tf2_base_link_to_lidar_; - -public: - explicit Undistorter2D(rclcpp::Node * node) : Undistorter(node) {} - void initialize() override; - void undistortPointImplementation( - 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 & time_offset, - bool & is_twist_valid, bool & is_imu_valid); - - void setPointCloudTransform( - const std::string & base_link_frame, const std::string & lidar_frame) override; -}; - -class Undistorter3D : public Undistorter -{ -private: - // defined outside of for loop for performance reasons. - Eigen::Vector4f point_eigen_; - Eigen::Vector4f undistorted_point_eigen_; - Eigen::Matrix4f transformation_matrix_; - Eigen::Matrix4f prev_transformation_matrix_; - - // TF - Eigen::Matrix4f eigen_lidar_to_base_link_; - Eigen::Matrix4f eigen_base_link_to_lidar_; - -public: - explicit Undistorter3D(rclcpp::Node * node) : Undistorter(node) {} - void initialize() override; - void undistortPointImplementation( - 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 & time_offset, - bool & is_twist_valid, bool & is_imu_valid); - void setPointCloudTransform( - const std::string & base_link_frame, const std::string & lidar_frame) override; -}; - -} // namespace pointcloud_preprocessor - -#endif // POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__UNDISTORTER_HPP_ diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index 2ad245b2b602d..b4e033681ee3d 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -14,107 +14,407 @@ #include "pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp" -#include "pointcloud_preprocessor/distortion_corrector/undistorter.hpp" +#include "tier4_autoware_utils/math/trigonometry.hpp" + +#include namespace pointcloud_preprocessor { -/** @brief Constructor. */ -DistortionCorrectorComponent::DistortionCorrectorComponent(const rclcpp::NodeOptions & options) -: Node("distortion_corrector_node", options) +template +void DistortionCorrector::processTwistMessage( + const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) { - // initialize debug tool - { - using tier4_autoware_utils::DebugPublisher; - using tier4_autoware_utils::StopWatch; - stop_watch_ptr_ = std::make_unique>(); - debug_publisher_ = std::make_unique(this, "distortion_corrector"); - stop_watch_ptr_->tic("cyclic_time"); - stop_watch_ptr_->tic("processing_time"); - } - - // Parameter - use_imu_ = declare_parameter("use_imu", true); - use_3d_distortion_correction_ = declare_parameter("use_3d_distortion_correction", false); - - // Publisher - undistorted_points_pub_ = - this->create_publisher("~/output/pointcloud", rclcpp::SensorDataQoS()); - - // Subscriber - twist_sub_ = this->create_subscription( - "~/input/twist", 10, - std::bind( - &DistortionCorrectorComponent::onTwistWithCovarianceStamped, this, std::placeholders::_1)); - imu_sub_ = this->create_subscription( - "~/input/imu", 10, - std::bind(&DistortionCorrectorComponent::onImu, this, std::placeholders::_1)); - input_points_sub_ = this->create_subscription( - "~/input/pointcloud", rclcpp::SensorDataQoS(), - std::bind(&DistortionCorrectorComponent::onPointCloud, this, std::placeholders::_1)); - - // Setup the undistortor - - if (use_3d_distortion_correction_) { - undistorter_ = std::make_unique(this); - } else { - undistorter_ = std::make_unique(this); + geometry_msgs::msg::TwistStamped msg; + msg.header = twist_msg->header; + msg.twist = twist_msg->twist.twist; + twist_queue_.push_back(msg); + + 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(); + } + break; } } -void DistortionCorrectorComponent::onTwistWithCovarianceStamped( - const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) +template +void DistortionCorrector::processIMUMessage( + const std::string & base_link_frame, const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) { - undistorter_->processTwistMessage(twist_msg); + geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr = + std::make_shared(); + getIMUTransformation(base_link_frame, imu_msg->header.frame_id, geometry_imu_to_base_link_ptr); + storeIMUToQueue(imu_msg, geometry_imu_to_base_link_ptr); } -void DistortionCorrectorComponent::onImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) +template +void DistortionCorrector::getIMUTransformation( + const std::string & base_link_frame, const std::string & imu_frame, + geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr) { - if (!use_imu_) { + if (is_imu_transform_exist_) { return; } - undistorter_->processIMUMessage(base_link_frame_, imu_msg); + tf2::Transform tf2_imu_to_base_link; + if (base_link_frame == imu_frame) { + tf2_imu_to_base_link.setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); + tf2_imu_to_base_link.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0)); + is_imu_transform_exist_ = true; + } else { + try { + const auto transform_msg = + tf_buffer_.lookupTransform(base_link_frame, imu_frame, tf2::TimePointZero); + tf2::convert(transform_msg.transform, tf2_imu_to_base_link); + is_imu_transform_exist_ = true; + } catch (const tf2::TransformException & ex) { + RCLCPP_WARN(node_->get_logger(), "%s", ex.what()); + RCLCPP_ERROR( + node_->get_logger(), "Please publish TF %s to %s", base_link_frame.c_str(), + imu_frame.c_str()); + + tf2_imu_to_base_link.setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); + tf2_imu_to_base_link.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0)); + } + } + + geometry_imu_to_base_link_ptr->transform.rotation = + tf2::toMsg(tf2_imu_to_base_link.getRotation()); +} + +template +void DistortionCorrector::storeIMUToQueue( + const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg, + geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr) +{ + geometry_msgs::msg::Vector3Stamped angular_velocity; + angular_velocity.vector = imu_msg->angular_velocity; + + 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 ( + 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(); + } + break; + } +} + +template +void DistortionCorrector::getIteratorOfTwistAndIMU( + bool use_imu, double first_point_time_stamp_sec, + std::deque::iterator & it_twist, + std::deque::iterator & it_imu) +{ + it_twist = std::lower_bound( + std::begin(twist_queue_), std::end(twist_queue_), first_point_time_stamp_sec, + [](const geometry_msgs::msg::TwistStamped & x, const double t) { + return rclcpp::Time(x.header.stamp).seconds() < t; + }); + it_twist = it_twist == std::end(twist_queue_) ? std::end(twist_queue_) - 1 : it_twist; + + if (use_imu && !angular_velocity_queue_.empty()) { + it_imu = std::lower_bound( + std::begin(angular_velocity_queue_), std::end(angular_velocity_queue_), + first_point_time_stamp_sec, [](const geometry_msgs::msg::Vector3Stamped & x, const double t) { + return rclcpp::Time(x.header.stamp).seconds() < t; + }); + it_imu = + it_imu == std::end(angular_velocity_queue_) ? std::end(angular_velocity_queue_) - 1 : it_imu; + } +} + +template +bool DistortionCorrector::isInputValid(sensor_msgs::msg::PointCloud2 & pointcloud) +{ + if (pointcloud.data.empty() || twist_queue_.empty()) { + RCLCPP_WARN_STREAM_THROTTLE( + node_->get_logger(), *node_->get_clock(), 10000 /* ms */, + "input pointcloud or twist_queue_ is empty."); + return false; + } + + auto time_stamp_field_it = std::find_if( + std::cbegin(pointcloud.fields), std::cend(pointcloud.fields), + [](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 */, + "Required field time stamp doesn't exist in the point cloud."); + return false; + } + return true; } -void DistortionCorrectorComponent::onPointCloud(PointCloud2::UniquePtr pointcloud_msg) +template +void DistortionCorrector::undistortPointCloud( + bool use_imu, sensor_msgs::msg::PointCloud2 & pointcloud) { - stop_watch_ptr_->toc("processing_time", true); - const auto points_sub_count = undistorted_points_pub_->get_subscription_count() + - undistorted_points_pub_->get_intra_process_subscription_count(); + if (!isInputValid(pointcloud)) return; + + sensor_msgs::PointCloud2Iterator it_x(pointcloud, "x"); + sensor_msgs::PointCloud2Iterator it_y(pointcloud, "y"); + sensor_msgs::PointCloud2Iterator it_z(pointcloud, "z"); + sensor_msgs::PointCloud2ConstIterator it_time_stamp(pointcloud, "time_stamp"); + + double prev_time_stamp_sec{*it_time_stamp}; + const double first_point_time_stamp_sec{*it_time_stamp}; + + std::deque::iterator it_twist; + std::deque::iterator it_imu; + getIteratorOfTwistAndIMU(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(); + double imu_stamp{0.0}; + if (use_imu && !angular_velocity_queue_.empty()) { + imu_stamp = rclcpp::Time(it_imu->header.stamp).seconds(); + } + + // If there is a point in a pointcloud that cannot be associated, record it to issue a warning + bool is_twist_time_stamp_too_late = false; + bool is_imu_time_stamp_is_too_late = false; + bool is_twist_valid = true; + bool is_imu_valid = true; + + for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_time_stamp) { + is_twist_valid = true; + is_imu_valid = true; - if (points_sub_count < 1) { + // Get closest twist information + while (it_twist != std::end(twist_queue_) - 1 && *it_time_stamp > twist_stamp) { + ++it_twist; + twist_stamp = rclcpp::Time(it_twist->header.stamp).seconds(); + } + if (std::abs(*it_time_stamp - twist_stamp) > 0.1) { + is_twist_time_stamp_too_late = true; + is_twist_valid = false; + } + + // Get closest IMU information + if (use_imu && !angular_velocity_queue_.empty()) { + while (it_imu != std::end(angular_velocity_queue_) - 1 && *it_time_stamp > imu_stamp) { + ++it_imu; + imu_stamp = rclcpp::Time(it_imu->header.stamp).seconds(); + } + + if (std::abs(*it_time_stamp - imu_stamp) > 0.1) { + is_imu_time_stamp_is_too_late = true; + is_imu_valid = false; + } + } else { + is_imu_valid = false; + } + + float time_offset = static_cast(*it_time_stamp - prev_time_stamp_sec); + + // Undistorted 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 = *it_time_stamp; + } + + warnIfTimestampsTooLate(is_twist_time_stamp_too_late, is_imu_time_stamp_is_too_late); +} + +template +void DistortionCorrector::warnIfTimestampsTooLate( + bool is_twist_time_stamp_too_late, bool is_imu_time_stamp_is_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_is_too_late) { + RCLCPP_WARN_STREAM_THROTTLE( + node_->get_logger(), *node_->get_clock(), 10000 /* ms */, + "imu time_stamp is too late. Could not interpolate."); + } +} + +///////////////////////// Functions for different undistortion strategies ///////////////////////// + +void DistortionCorrector2D::initialize() +{ + x_ = 0.0f; + y_ = 0.0f; + theta_ = 0.0f; +} + +void DistortionCorrector3D::initialize() +{ + prev_transformation_matrix_ = Eigen::Matrix4f::Identity(); +} + +void DistortionCorrector2D::setPointCloudTransform( + const std::string & base_link_frame, const std::string & lidar_frame) +{ + if (is_pointcloud_transform_exist_) { return; } - undistorter_->setPointCloudTransform(base_link_frame_, pointcloud_msg->header.frame_id); + if (base_link_frame == lidar_frame) { + tf2_lidar_to_base_link_.setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); + tf2_lidar_to_base_link_.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0)); + tf2_base_link_to_lidar_ = tf2_lidar_to_base_link_; + is_pointcloud_transform_exist_ = true; + } else { + try { + const auto transform_msg = + tf_buffer_.lookupTransform(base_link_frame, lidar_frame, tf2::TimePointZero); + tf2::convert(transform_msg.transform, tf2_lidar_to_base_link_); + tf2_base_link_to_lidar_ = tf2_lidar_to_base_link_.inverse(); + is_pointcloud_transform_exist_ = true; + is_pointcloud_transform_needed_ = true; + } catch (const tf2::TransformException & ex) { + RCLCPP_WARN(node_->get_logger(), "%s", ex.what()); + RCLCPP_ERROR( + node_->get_logger(), "Please publish TF %s to %s", base_link_frame.c_str(), + lidar_frame.c_str()); - undistorter_->initialize(); - undistorter_->undistortPointCloud(use_imu_, *pointcloud_msg); + tf2_lidar_to_base_link_.setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); + tf2_lidar_to_base_link_.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0)); + tf2_base_link_to_lidar_ = tf2_lidar_to_base_link_; + } + } +} - if (debug_publisher_) { - auto pipeline_latency_ms = - std::chrono::duration( - std::chrono::nanoseconds( - (this->get_clock()->now() - pointcloud_msg->header.stamp).nanoseconds())) - .count(); - debug_publisher_->publish( - "debug/pipeline_latency_ms", pipeline_latency_ms); +void DistortionCorrector3D::setPointCloudTransform( + const std::string & base_link_frame, const std::string & lidar_frame) +{ + if (is_pointcloud_transform_exist_) { + return; } - undistorted_points_pub_->publish(std::move(pointcloud_msg)); + if (base_link_frame == lidar_frame) { + eigen_lidar_to_base_link_ = Eigen::Matrix4f::Identity(); + eigen_base_link_to_lidar_ = Eigen::Matrix4f::Identity(); + is_pointcloud_transform_exist_ = true; + } - // add processing time for debug - if (debug_publisher_) { - const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); - const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( - "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( - "debug/processing_time_ms", processing_time_ms); + try { + const auto transform_msg = + tf_buffer_.lookupTransform(base_link_frame, lidar_frame, tf2::TimePointZero); + eigen_lidar_to_base_link_ = + tf2::transformToEigen(transform_msg.transform).matrix().cast(); + eigen_base_link_to_lidar_ = eigen_lidar_to_base_link_.inverse(); + is_pointcloud_transform_exist_ = true; + is_pointcloud_transform_needed_ = true; + } catch (const tf2::TransformException & ex) { + RCLCPP_WARN(node_->get_logger(), "%s", ex.what()); + RCLCPP_ERROR( + node_->get_logger(), "Please publish TF %s to %s", base_link_frame.c_str(), + lidar_frame.c_str()); + eigen_lidar_to_base_link_ = Eigen::Matrix4f::Identity(); + eigen_base_link_to_lidar_ = Eigen::Matrix4f::Identity(); } } -} // namespace pointcloud_preprocessor +void DistortionCorrector2D::undistortPointImplementation( + 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 & time_offset, + bool & is_twist_valid, bool & is_imu_valid) +{ + // Initialize linear velocity and angular velocity + float v{0.0f}, w{0.0f}; + if (is_twist_valid) { + v = static_cast(it_twist->twist.linear.x); + w = static_cast(it_twist->twist.angular.z); + } + if (is_imu_valid) { + w = static_cast(it_imu->vector.z); + } -#include -RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_preprocessor::DistortionCorrectorComponent) + // Undistort point + point_tf_.setValue(*it_x, *it_y, *it_z); + + if (is_pointcloud_transform_needed_) { + point_tf_ = tf2_lidar_to_base_link_ * point_tf_; + } + theta_ += w * time_offset; + baselink_quat_.setValue( + 0, 0, tier4_autoware_utils::sin(theta_ * 0.5f), + tier4_autoware_utils::cos(theta_ * 0.5f)); // baselink_quat.setRPY(0.0, 0.0, theta); + const float dis = v * time_offset; + x_ += dis * tier4_autoware_utils::cos(theta_); + y_ += dis * tier4_autoware_utils::sin(theta_); + + baselink_tf_odom_.setOrigin(tf2::Vector3(x_, y_, 0.0)); + baselink_tf_odom_.setRotation(baselink_quat_); + + undistorted_point_tf_ = baselink_tf_odom_ * point_tf_; + + if (is_pointcloud_transform_needed_) { + undistorted_point_tf_ = tf2_base_link_to_lidar_ * undistorted_point_tf_; + } + + *it_x = static_cast(undistorted_point_tf_.getX()); + *it_y = static_cast(undistorted_point_tf_.getY()); + *it_z = static_cast(undistorted_point_tf_.getZ()); +} + +void DistortionCorrector3D::undistortPointImplementation( + 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 & time_offset, + bool & is_twist_valid, 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}; + 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); + } + 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); + } + + // Undistort point + point_eigen_ << *it_x, *it_y, *it_z, 1.0; + if (is_pointcloud_transform_needed_) { + 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_); + twist = twist * time_offset; + transformation_matrix_ = Sophus::SE3f::exp(twist).matrix(); + transformation_matrix_ = transformation_matrix_ * prev_transformation_matrix_; + undistorted_point_eigen_ = transformation_matrix_ * point_eigen_; + + if (is_pointcloud_transform_needed_) { + undistorted_point_eigen_ = eigen_base_link_to_lidar_ * undistorted_point_eigen_; + } + *it_x = undistorted_point_eigen_[0]; + *it_y = undistorted_point_eigen_[1]; + *it_z = undistorted_point_eigen_[2]; + + prev_transformation_matrix_ = transformation_matrix_; +} + +} // namespace pointcloud_preprocessor diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_nodelet.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_nodelet.cpp new file mode 100644 index 0000000000000..ffeae9d805441 --- /dev/null +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_nodelet.cpp @@ -0,0 +1,120 @@ +// Copyright 2024 Tier IV, Inc. +// +// 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. + +#include "pointcloud_preprocessor/distortion_corrector/distortion_corrector_nodelet.hpp" + +#include "pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp" + +namespace pointcloud_preprocessor +{ +/** @brief Constructor. */ +DistortionCorrectorComponent::DistortionCorrectorComponent(const rclcpp::NodeOptions & options) +: Node("distortion_corrector_node", options) +{ + // initialize debug tool + { + using tier4_autoware_utils::DebugPublisher; + using tier4_autoware_utils::StopWatch; + stop_watch_ptr_ = std::make_unique>(); + debug_publisher_ = std::make_unique(this, "distortion_corrector"); + stop_watch_ptr_->tic("cyclic_time"); + stop_watch_ptr_->tic("processing_time"); + } + + // Parameter + use_imu_ = declare_parameter("use_imu", true); + use_3d_distortion_correction_ = declare_parameter("use_3d_distortion_correction", false); + + // Publisher + undistorted_points_pub_ = + this->create_publisher("~/output/pointcloud", rclcpp::SensorDataQoS()); + + // Subscriber + twist_sub_ = this->create_subscription( + "~/input/twist", 10, + std::bind( + &DistortionCorrectorComponent::onTwistWithCovarianceStamped, this, std::placeholders::_1)); + imu_sub_ = this->create_subscription( + "~/input/imu", 10, + std::bind(&DistortionCorrectorComponent::onImu, this, std::placeholders::_1)); + input_points_sub_ = this->create_subscription( + "~/input/pointcloud", rclcpp::SensorDataQoS(), + std::bind(&DistortionCorrectorComponent::onPointCloud, this, std::placeholders::_1)); + + // Setup the undistortor + + if (use_3d_distortion_correction_) { + DistortionCorrector_ = std::make_unique(this); + } else { + DistortionCorrector_ = std::make_unique(this); + } +} + +void DistortionCorrectorComponent::onTwistWithCovarianceStamped( + const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) +{ + DistortionCorrector_->processTwistMessage(twist_msg); +} + +void DistortionCorrectorComponent::onImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) +{ + if (!use_imu_) { + return; + } + + DistortionCorrector_->processIMUMessage(base_link_frame_, imu_msg); +} + +void DistortionCorrectorComponent::onPointCloud(PointCloud2::UniquePtr pointcloud_msg) +{ + stop_watch_ptr_->toc("processing_time", true); + const auto points_sub_count = undistorted_points_pub_->get_subscription_count() + + undistorted_points_pub_->get_intra_process_subscription_count(); + + if (points_sub_count < 1) { + return; + } + + DistortionCorrector_->setPointCloudTransform(base_link_frame_, pointcloud_msg->header.frame_id); + + DistortionCorrector_->initialize(); + DistortionCorrector_->undistortPointCloud(use_imu_, *pointcloud_msg); + + if (debug_publisher_) { + auto pipeline_latency_ms = + std::chrono::duration( + std::chrono::nanoseconds( + (this->get_clock()->now() - pointcloud_msg->header.stamp).nanoseconds())) + .count(); + debug_publisher_->publish( + "debug/pipeline_latency_ms", pipeline_latency_ms); + } + + undistorted_points_pub_->publish(std::move(pointcloud_msg)); + + // add processing time for debug + if (debug_publisher_) { + const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); + const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); + debug_publisher_->publish( + "debug/cyclic_time_ms", cyclic_time_ms); + debug_publisher_->publish( + "debug/processing_time_ms", processing_time_ms); + } +} + +} // namespace pointcloud_preprocessor + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_preprocessor::DistortionCorrectorComponent) diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/undistorter.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/undistorter.cpp deleted file mode 100644 index 64eccbea0654a..0000000000000 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/undistorter.cpp +++ /dev/null @@ -1,420 +0,0 @@ -// Copyright 2024 Tier IV, Inc. -// -// 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. - -#include "pointcloud_preprocessor/distortion_corrector/undistorter.hpp" - -#include "tier4_autoware_utils/math/trigonometry.hpp" - -#include - -namespace pointcloud_preprocessor -{ -template -void Undistorter::processTwistMessage( - 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); - - 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(); - } - break; - } -} - -template -void Undistorter::processIMUMessage( - const std::string & base_link_frame, const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) -{ - geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr = - std::make_shared(); - getIMUTransformation(base_link_frame, imu_msg->header.frame_id, geometry_imu_to_base_link_ptr); - storeIMUToQueue(imu_msg, geometry_imu_to_base_link_ptr); -} - -template -void Undistorter::getIMUTransformation( - const std::string & base_link_frame, const std::string & imu_frame, - geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr) -{ - if (is_imu_transform_exist_) { - return; - } - - tf2::Transform tf2_imu_to_base_link; - if (base_link_frame == imu_frame) { - tf2_imu_to_base_link.setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); - tf2_imu_to_base_link.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0)); - is_imu_transform_exist_ = true; - } else { - try { - const auto transform_msg = - tf_buffer_.lookupTransform(base_link_frame, imu_frame, tf2::TimePointZero); - tf2::convert(transform_msg.transform, tf2_imu_to_base_link); - is_imu_transform_exist_ = true; - } catch (const tf2::TransformException & ex) { - RCLCPP_WARN(node_->get_logger(), "%s", ex.what()); - RCLCPP_ERROR( - node_->get_logger(), "Please publish TF %s to %s", base_link_frame.c_str(), - imu_frame.c_str()); - - tf2_imu_to_base_link.setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); - tf2_imu_to_base_link.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0)); - } - } - - geometry_imu_to_base_link_ptr->transform.rotation = - tf2::toMsg(tf2_imu_to_base_link.getRotation()); -} - -template -void Undistorter::storeIMUToQueue( - const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg, - geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr) -{ - geometry_msgs::msg::Vector3Stamped angular_velocity; - angular_velocity.vector = imu_msg->angular_velocity; - - 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 ( - 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(); - } - break; - } -} - -template -void Undistorter::getIteratorOfTwistAndIMU( - bool use_imu, double first_point_time_stamp_sec, - std::deque::iterator & it_twist, - std::deque::iterator & it_imu) -{ - it_twist = std::lower_bound( - std::begin(twist_queue_), std::end(twist_queue_), first_point_time_stamp_sec, - [](const geometry_msgs::msg::TwistStamped & x, const double t) { - return rclcpp::Time(x.header.stamp).seconds() < t; - }); - it_twist = it_twist == std::end(twist_queue_) ? std::end(twist_queue_) - 1 : it_twist; - - if (use_imu && !angular_velocity_queue_.empty()) { - it_imu = std::lower_bound( - std::begin(angular_velocity_queue_), std::end(angular_velocity_queue_), - first_point_time_stamp_sec, [](const geometry_msgs::msg::Vector3Stamped & x, const double t) { - return rclcpp::Time(x.header.stamp).seconds() < t; - }); - it_imu = - it_imu == std::end(angular_velocity_queue_) ? std::end(angular_velocity_queue_) - 1 : it_imu; - } -} - -template -bool Undistorter::isInputValid(sensor_msgs::msg::PointCloud2 & pointcloud) -{ - if (pointcloud.data.empty() || twist_queue_.empty()) { - RCLCPP_WARN_STREAM_THROTTLE( - node_->get_logger(), *node_->get_clock(), 10000 /* ms */, - "input pointcloud or twist_queue_ is empty."); - return false; - } - - auto time_stamp_field_it = std::find_if( - std::cbegin(pointcloud.fields), std::cend(pointcloud.fields), - [](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 */, - "Required field time stamp doesn't exist in the point cloud."); - return false; - } - return true; -} - -template -void Undistorter::undistortPointCloud( - bool use_imu, sensor_msgs::msg::PointCloud2 & pointcloud) -{ - if (!isInputValid(pointcloud)) return; - - sensor_msgs::PointCloud2Iterator it_x(pointcloud, "x"); - sensor_msgs::PointCloud2Iterator it_y(pointcloud, "y"); - sensor_msgs::PointCloud2Iterator it_z(pointcloud, "z"); - sensor_msgs::PointCloud2ConstIterator it_time_stamp(pointcloud, "time_stamp"); - - double prev_time_stamp_sec{*it_time_stamp}; - const double first_point_time_stamp_sec{*it_time_stamp}; - - std::deque::iterator it_twist; - std::deque::iterator it_imu; - getIteratorOfTwistAndIMU(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(); - double imu_stamp{0.0}; - if (use_imu && !angular_velocity_queue_.empty()) { - imu_stamp = rclcpp::Time(it_imu->header.stamp).seconds(); - } - - // If there is a point in a pointcloud that cannot be associated, record it to issue a warning - bool is_twist_time_stamp_too_late = false; - bool is_imu_time_stamp_is_too_late = false; - bool is_twist_valid = true; - bool is_imu_valid = true; - - for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_time_stamp) { - is_twist_valid = true; - is_imu_valid = true; - - // Get closest twist information - while (it_twist != std::end(twist_queue_) - 1 && *it_time_stamp > twist_stamp) { - ++it_twist; - twist_stamp = rclcpp::Time(it_twist->header.stamp).seconds(); - } - if (std::abs(*it_time_stamp - twist_stamp) > 0.1) { - is_twist_time_stamp_too_late = true; - is_twist_valid = false; - } - - // Get closest IMU information - if (use_imu && !angular_velocity_queue_.empty()) { - while (it_imu != std::end(angular_velocity_queue_) - 1 && *it_time_stamp > imu_stamp) { - ++it_imu; - imu_stamp = rclcpp::Time(it_imu->header.stamp).seconds(); - } - - if (std::abs(*it_time_stamp - imu_stamp) > 0.1) { - is_imu_time_stamp_is_too_late = true; - is_imu_valid = false; - } - } else { - is_imu_valid = false; - } - - float time_offset = static_cast(*it_time_stamp - prev_time_stamp_sec); - - // Undistorted 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 = *it_time_stamp; - } - - warnIfTimestampsTooLate(is_twist_time_stamp_too_late, is_imu_time_stamp_is_too_late); -} - -template -void Undistorter::warnIfTimestampsTooLate( - bool is_twist_time_stamp_too_late, bool is_imu_time_stamp_is_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_is_too_late) { - RCLCPP_WARN_STREAM_THROTTLE( - node_->get_logger(), *node_->get_clock(), 10000 /* ms */, - "imu time_stamp is too late. Could not interpolate."); - } -} - -///////////////////////// Functions for different undistortion strategies ///////////////////////// - -void Undistorter2D::initialize() -{ - x_ = 0.0f; - y_ = 0.0f; - theta_ = 0.0f; -} - -void Undistorter3D::initialize() -{ - prev_transformation_matrix_ = Eigen::Matrix4f::Identity(); -} - -void Undistorter2D::setPointCloudTransform( - const std::string & base_link_frame, const std::string & lidar_frame) -{ - if (is_pointcloud_transform_exist_) { - return; - } - - if (base_link_frame == lidar_frame) { - tf2_lidar_to_base_link_.setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); - tf2_lidar_to_base_link_.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0)); - tf2_base_link_to_lidar_ = tf2_lidar_to_base_link_; - is_pointcloud_transform_exist_ = true; - } else { - try { - const auto transform_msg = - tf_buffer_.lookupTransform(base_link_frame, lidar_frame, tf2::TimePointZero); - tf2::convert(transform_msg.transform, tf2_lidar_to_base_link_); - tf2_base_link_to_lidar_ = tf2_lidar_to_base_link_.inverse(); - is_pointcloud_transform_exist_ = true; - is_pointcloud_transform_needed_ = true; - } catch (const tf2::TransformException & ex) { - RCLCPP_WARN(node_->get_logger(), "%s", ex.what()); - RCLCPP_ERROR( - node_->get_logger(), "Please publish TF %s to %s", base_link_frame.c_str(), - lidar_frame.c_str()); - - tf2_lidar_to_base_link_.setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); - tf2_lidar_to_base_link_.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0)); - tf2_base_link_to_lidar_ = tf2_lidar_to_base_link_; - } - } -} - -void Undistorter3D::setPointCloudTransform( - const std::string & base_link_frame, const std::string & lidar_frame) -{ - if (is_pointcloud_transform_exist_) { - return; - } - - if (base_link_frame == lidar_frame) { - eigen_lidar_to_base_link_ = Eigen::Matrix4f::Identity(); - eigen_base_link_to_lidar_ = Eigen::Matrix4f::Identity(); - is_pointcloud_transform_exist_ = true; - } - - try { - const auto transform_msg = - tf_buffer_.lookupTransform(base_link_frame, lidar_frame, tf2::TimePointZero); - eigen_lidar_to_base_link_ = - tf2::transformToEigen(transform_msg.transform).matrix().cast(); - eigen_base_link_to_lidar_ = eigen_lidar_to_base_link_.inverse(); - is_pointcloud_transform_exist_ = true; - is_pointcloud_transform_needed_ = true; - } catch (const tf2::TransformException & ex) { - RCLCPP_WARN(node_->get_logger(), "%s", ex.what()); - RCLCPP_ERROR( - node_->get_logger(), "Please publish TF %s to %s", base_link_frame.c_str(), - lidar_frame.c_str()); - eigen_lidar_to_base_link_ = Eigen::Matrix4f::Identity(); - eigen_base_link_to_lidar_ = Eigen::Matrix4f::Identity(); - } -} - -void Undistorter2D::undistortPointImplementation( - 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 & time_offset, - bool & is_twist_valid, bool & is_imu_valid) -{ - // Initialize linear velocity and angular velocity - float v{0.0f}, w{0.0f}; - if (is_twist_valid) { - v = static_cast(it_twist->twist.linear.x); - w = static_cast(it_twist->twist.angular.z); - } - if (is_imu_valid) { - w = static_cast(it_imu->vector.z); - } - - // Undistort point - point_tf_.setValue(*it_x, *it_y, *it_z); - - if (is_pointcloud_transform_needed_) { - point_tf_ = tf2_lidar_to_base_link_ * point_tf_; - } - theta_ += w * time_offset; - baselink_quat_.setValue( - 0, 0, tier4_autoware_utils::sin(theta_ * 0.5f), - tier4_autoware_utils::cos(theta_ * 0.5f)); // baselink_quat.setRPY(0.0, 0.0, theta); - const float dis = v * time_offset; - x_ += dis * tier4_autoware_utils::cos(theta_); - y_ += dis * tier4_autoware_utils::sin(theta_); - - baselink_tf_odom_.setOrigin(tf2::Vector3(x_, y_, 0.0)); - baselink_tf_odom_.setRotation(baselink_quat_); - - undistorted_point_tf_ = baselink_tf_odom_ * point_tf_; - - if (is_pointcloud_transform_needed_) { - undistorted_point_tf_ = tf2_base_link_to_lidar_ * undistorted_point_tf_; - } - - *it_x = static_cast(undistorted_point_tf_.getX()); - *it_y = static_cast(undistorted_point_tf_.getY()); - *it_z = static_cast(undistorted_point_tf_.getZ()); -} - -void Undistorter3D::undistortPointImplementation( - 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 & time_offset, - bool & is_twist_valid, 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}; - 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); - } - 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); - } - - // Undistort point - point_eigen_ << *it_x, *it_y, *it_z, 1.0; - if (is_pointcloud_transform_needed_) { - 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_); - twist = twist * time_offset; - transformation_matrix_ = Sophus::SE3f::exp(twist).matrix(); - transformation_matrix_ = transformation_matrix_ * prev_transformation_matrix_; - undistorted_point_eigen_ = transformation_matrix_ * point_eigen_; - - if (is_pointcloud_transform_needed_) { - undistorted_point_eigen_ = eigen_base_link_to_lidar_ * undistorted_point_eigen_; - } - *it_x = undistorted_point_eigen_[0]; - *it_y = undistorted_point_eigen_[1]; - *it_z = undistorted_point_eigen_[2]; - - prev_transformation_matrix_ = transformation_matrix_; -} - -} // namespace pointcloud_preprocessor From 0891e2c5ac531541e5499e122610b3c41b09d115 Mon Sep 17 00:00:00 2001 From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com> Date: Thu, 13 Jun 2024 12:30:05 +0900 Subject: [PATCH 16/47] Update sensing/pointcloud_preprocessor/docs/distortion-corrector.md Co-authored-by: David Wong <33114676+drwnz@users.noreply.github.com> Signed-off-by: vividf --- sensing/pointcloud_preprocessor/docs/distortion-corrector.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md index ed44976cd47e4..49d45b2fa7240 100644 --- a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md +++ b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md @@ -2,7 +2,7 @@ ## Purpose -The `distortion_corrector` is a node that compensates for pointcloud distortion caused by the ego-vehicle's movement during 1 scan. +The `distortion_corrector` is a node that compensates for pointcloud distortion caused by the ego-vehicle's movement during one scan. Since the LiDAR sensor scans by rotating an internal laser, the resulting point cloud will be distorted if the ego-vehicle moves during a single scan (as shown by the figure below). The node corrects this by interpolating sensor data using the odometry of the ego-vehicle. From 9f48c471bea509205ca9900acab54a79b268790e Mon Sep 17 00:00:00 2001 From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com> Date: Thu, 13 Jun 2024 12:30:20 +0900 Subject: [PATCH 17/47] chore: update sensing/pointcloud_preprocessor/docs/distortion-corrector.md Co-authored-by: David Wong <33114676+drwnz@users.noreply.github.com> Signed-off-by: vividf --- sensing/pointcloud_preprocessor/docs/distortion-corrector.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md index 49d45b2fa7240..30380b372b74c 100644 --- a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md +++ b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md @@ -8,7 +8,7 @@ Since the LiDAR sensor scans by rotating an internal laser, the resulting point ## Inner-workings / Algorithms -The node utilizes twist information (linear velocity and angular velocity) from the `~/input/twist` topic. If the user sets `use_imu` to true, the node will replace the twist's angular velocity with the angular velocity from IMU. Afterward, the node will undistort all of the points one by one based on the velocity information. +The node utilizes twist information (linear velocity and angular velocity) from the `~/input/twist` topic. If the user sets `use_imu` to true, the node will replace the twist's angular velocity with the angular velocity from IMU. The node then moves every point in the pointcloud to compensate for the motion provided by the velocity information. The node also supports two different kinds of distortion methods: 2D distortion correction and 3D distortion correction. The main difference is that the 2D distortion corrector only utilizes the x-axis of linear velocity and the z-axis of angular velocity to correct the points. On the other hand, the 3D distortion corrector utilizes all linear and angular velocity components to correct the points. From a767831dceae08b301cacdd97afffb61f9a6cf83 Mon Sep 17 00:00:00 2001 From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com> Date: Thu, 13 Jun 2024 12:30:31 +0900 Subject: [PATCH 18/47] chore: update sensing/pointcloud_preprocessor/docs/distortion-corrector.md Co-authored-by: David Wong <33114676+drwnz@users.noreply.github.com> Signed-off-by: vividf --- sensing/pointcloud_preprocessor/docs/distortion-corrector.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md index 30380b372b74c..11be738001ad9 100644 --- a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md +++ b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md @@ -10,7 +10,7 @@ Since the LiDAR sensor scans by rotating an internal laser, the resulting point The node utilizes twist information (linear velocity and angular velocity) from the `~/input/twist` topic. If the user sets `use_imu` to true, the node will replace the twist's angular velocity with the angular velocity from IMU. The node then moves every point in the pointcloud to compensate for the motion provided by the velocity information. -The node also supports two different kinds of distortion methods: 2D distortion correction and 3D distortion correction. The main difference is that the 2D distortion corrector only utilizes the x-axis of linear velocity and the z-axis of angular velocity to correct the points. On the other hand, the 3D distortion corrector utilizes all linear and angular velocity components to correct the points. +The node also supports two different modes of distortion correction: 2D distortion correction and 3D distortion correction. The main difference is that the 2D distortion corrector only utilizes the x-axis of linear velocity and the z-axis of angular velocity to correct the point positions. On the other hand, the 3D distortion corrector utilizes all linear and angular velocity components to correct the point positions. Please note that the processing time difference between the two distortion methods is significant; the 3D corrector takes 50% more time than the 2D corrector. Therefore, it is recommended that in general cases, users should set `use_3d_distortion_correction` to `false`. However, in scenarios such as a vehicle going over speed bumps, using the 3D corrector can be beneficial. From ba9b04e37be3efb98899ed78bd04adc001f3058b Mon Sep 17 00:00:00 2001 From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com> Date: Thu, 13 Jun 2024 12:30:55 +0900 Subject: [PATCH 19/47] chore: fix company name Co-authored-by: David Wong <33114676+drwnz@users.noreply.github.com> Signed-off-by: vividf --- .../distortion_corrector/distortion_corrector.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp index 4afa430160280..cc4cbf0821efa 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. From b29bc0f8ac96d96147ee6341d3c7ad9926ed3a5e Mon Sep 17 00:00:00 2001 From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com> Date: Thu, 13 Jun 2024 12:31:01 +0900 Subject: [PATCH 20/47] chore: fix company name Co-authored-by: David Wong <33114676+drwnz@users.noreply.github.com> Signed-off-by: vividf --- .../distortion_corrector/distortion_corrector_nodelet.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector_nodelet.hpp index efa137bd3ccf4..412a78dff762b 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector_nodelet.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. From f57a2217c69c5448fae729aee0c8e2ee92eaa33e Mon Sep 17 00:00:00 2001 From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com> Date: Thu, 13 Jun 2024 12:31:10 +0900 Subject: [PATCH 21/47] chore: fix company name Co-authored-by: David Wong <33114676+drwnz@users.noreply.github.com> Signed-off-by: vividf --- .../src/distortion_corrector/distortion_corrector.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index b4e033681ee3d..5271ca7e5cd51 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. From 4ebf4fcda9ba72ca204f2b9b61f852edd4438363 Mon Sep 17 00:00:00 2001 From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com> Date: Thu, 13 Jun 2024 12:31:51 +0900 Subject: [PATCH 22/47] chore: fix imu to IMU Co-authored-by: David Wong <33114676+drwnz@users.noreply.github.com> Signed-off-by: vividf --- .../src/distortion_corrector/distortion_corrector.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index 5271ca7e5cd51..6c4c31d9aebe6 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -245,7 +245,7 @@ void DistortionCorrector::warnIfTimestampsTooLate( if (is_imu_time_stamp_is_too_late) { RCLCPP_WARN_STREAM_THROTTLE( node_->get_logger(), *node_->get_clock(), 10000 /* ms */, - "imu time_stamp is too late. Could not interpolate."); + "IMU time_stamp is too late. Could not interpolate."); } } From a62f2bdd242b99558aef5c515d65a157c95bf568 Mon Sep 17 00:00:00 2001 From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com> Date: Thu, 13 Jun 2024 12:32:04 +0900 Subject: [PATCH 23/47] chore: fix company name Co-authored-by: David Wong <33114676+drwnz@users.noreply.github.com> Signed-off-by: vividf --- .../src/distortion_corrector/distortion_corrector_nodelet.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_nodelet.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_nodelet.cpp index ffeae9d805441..2b5455afe8650 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_nodelet.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. From aececaf30a85acc5bc4498b03951ebb122f9e60a Mon Sep 17 00:00:00 2001 From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com> Date: Thu, 13 Jun 2024 12:33:00 +0900 Subject: [PATCH 24/47] chore: remove old naming Co-authored-by: David Wong <33114676+drwnz@users.noreply.github.com> Signed-off-by: vividf --- .../src/distortion_corrector/distortion_corrector_nodelet.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_nodelet.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_nodelet.cpp index 2b5455afe8650..e3e5d49a176fb 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_nodelet.cpp @@ -52,7 +52,7 @@ DistortionCorrectorComponent::DistortionCorrectorComponent(const rclcpp::NodeOpt "~/input/pointcloud", rclcpp::SensorDataQoS(), std::bind(&DistortionCorrectorComponent::onPointCloud, this, std::placeholders::_1)); - // Setup the undistortor + // Setup the distortion corrector if (use_3d_distortion_correction_) { DistortionCorrector_ = std::make_unique(this); From c5d7aeab81ffc83e6f704ec949330fd623f2556f Mon Sep 17 00:00:00 2001 From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com> Date: Thu, 13 Jun 2024 12:41:54 +0900 Subject: [PATCH 25/47] chore: change boolean variable naming Co-authored-by: David Wong <33114676+drwnz@users.noreply.github.com> Signed-off-by: vividf --- .../distortion_corrector/distortion_corrector.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp index cc4cbf0821efa..fe37ccd8c56e2 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp @@ -62,7 +62,7 @@ template class DistortionCorrector : public DistortionCorrectorBase { public: - bool is_pointcloud_transform_needed_{false}; + bool pointcloud_transform_needed_{false}; bool is_pointcloud_transform_exist_{false}; bool is_imu_transform_exist_{false}; rclcpp::Node * node_; From 776aeb0a5f6d17f00918a976c694dbcac87d13d5 Mon Sep 17 00:00:00 2001 From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com> Date: Thu, 13 Jun 2024 12:42:03 +0900 Subject: [PATCH 26/47] chore: change boolean variable naming Co-authored-by: David Wong <33114676+drwnz@users.noreply.github.com> Signed-off-by: vividf --- .../distortion_corrector/distortion_corrector.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp index fe37ccd8c56e2..610556b608ccc 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp @@ -64,7 +64,7 @@ class DistortionCorrector : public DistortionCorrectorBase public: bool pointcloud_transform_needed_{false}; bool is_pointcloud_transform_exist_{false}; - bool is_imu_transform_exist_{false}; + bool imu_transform_exists_{false}; rclcpp::Node * node_; tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; From d53f3ab6e3639686406756ef99fc21c0991590a2 Mon Sep 17 00:00:00 2001 From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com> Date: Thu, 13 Jun 2024 12:42:35 +0900 Subject: [PATCH 27/47] chore: change boolean variable naming Co-authored-by: David Wong <33114676+drwnz@users.noreply.github.com> Signed-off-by: vividf --- .../distortion_corrector/distortion_corrector.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp index 610556b608ccc..24d76a1e9404c 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp @@ -63,7 +63,7 @@ class DistortionCorrector : public DistortionCorrectorBase { public: bool pointcloud_transform_needed_{false}; - bool is_pointcloud_transform_exist_{false}; + bool pointcloud_transform_exists_{false}; bool imu_transform_exists_{false}; rclcpp::Node * node_; tf2_ros::Buffer tf_buffer_; From d0a2d613ec0bc0828e242655c155428f5805cae6 Mon Sep 17 00:00:00 2001 From: vividf Date: Thu, 13 Jun 2024 13:32:57 +0900 Subject: [PATCH 28/47] fix: fix file name and variable name Signed-off-by: vividf --- .../pointcloud_preprocessor/CMakeLists.txt | 2 +- .../distortion_corrector.hpp | 3 +- ...elet.hpp => distortion_corrector_node.hpp} | 16 +++---- .../distortion_corrector.cpp | 44 +++++++++---------- ...elet.cpp => distortion_corrector_node.cpp} | 23 +++++----- 5 files changed, 43 insertions(+), 45 deletions(-) rename sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/{distortion_corrector_nodelet.hpp => distortion_corrector_node.hpp} (86%) rename sensing/pointcloud_preprocessor/src/distortion_corrector/{distortion_corrector_nodelet.cpp => distortion_corrector_node.cpp} (83%) diff --git a/sensing/pointcloud_preprocessor/CMakeLists.txt b/sensing/pointcloud_preprocessor/CMakeLists.txt index 73aa8ade80b19..d00d137129449 100644 --- a/sensing/pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/pointcloud_preprocessor/CMakeLists.txt @@ -78,7 +78,7 @@ ament_auto_add_library(pointcloud_preprocessor_filter SHARED src/pointcloud_accumulator/pointcloud_accumulator_nodelet.cpp src/vector_map_filter/lanelet2_map_filter_nodelet.cpp src/distortion_corrector/distortion_corrector.cpp - src/distortion_corrector/distortion_corrector_nodelet.cpp + src/distortion_corrector/distortion_corrector_node.cpp src/blockage_diag/blockage_diag_nodelet.cpp src/polygon_remover/polygon_remover.cpp src/vector_map_filter/vector_map_inside_area_filter.cpp diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp index 24d76a1e9404c..4b7f93c349980 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp @@ -95,8 +95,7 @@ class DistortionCorrector : public DistortionCorrectorBase std::deque::iterator & it_twist, std::deque::iterator & it_imu); void undistortPointCloud(bool use_imu, sensor_msgs::msg::PointCloud2 & pointcloud) override; - void warnIfTimestampsTooLate( - bool is_twist_time_stamp_too_late, bool is_imu_time_stamp_is_too_late); + void warnIfTimestampsTooLate(bool is_twist_time_stamp_too_late, bool is_imu_time_stamp_too_late); virtual void initialize() = 0; void undistortPoint( diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector_node.hpp similarity index 86% rename from sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector_nodelet.hpp rename to sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector_node.hpp index 412a78dff762b..a211810330ff8 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_NODELET_HPP_ -#define POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_NODELET_HPP_ +#ifndef POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_NODE_HPP_ +#define POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_NODE_HPP_ #include "pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp" @@ -40,9 +40,10 @@ class DistortionCorrectorComponent : public rclcpp::Node explicit DistortionCorrectorComponent(const rclcpp::NodeOptions & options); private: - rclcpp::Subscription::SharedPtr input_points_sub_; - rclcpp::Subscription::SharedPtr imu_sub_; rclcpp::Subscription::SharedPtr twist_sub_; + rclcpp::Subscription::SharedPtr imu_sub_; + rclcpp::Subscription::SharedPtr pointcloud_sub_; + rclcpp::Publisher::SharedPtr undistorted_points_pub_; std::unique_ptr> stop_watch_ptr_; @@ -52,14 +53,13 @@ class DistortionCorrectorComponent : public rclcpp::Node bool use_imu_; bool use_3d_distortion_correction_; - std::unique_ptr DistortionCorrector_; + std::unique_ptr distortion_corrector_; void onPointCloud(PointCloud2::UniquePtr points_msg); - void onTwistWithCovarianceStamped( - const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg); + void onTwist(const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg); void onImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg); }; } // namespace pointcloud_preprocessor -#endif // POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_NODELET_HPP_ +#endif // POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_NODE_HPP_ diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index 6c4c31d9aebe6..ef5223d7bd8e8 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -57,7 +57,7 @@ void DistortionCorrector::getIMUTransformation( const std::string & base_link_frame, const std::string & imu_frame, geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr) { - if (is_imu_transform_exist_) { + if (imu_transform_exists_) { return; } @@ -65,13 +65,13 @@ void DistortionCorrector::getIMUTransformation( if (base_link_frame == imu_frame) { tf2_imu_to_base_link.setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); tf2_imu_to_base_link.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0)); - is_imu_transform_exist_ = true; + imu_transform_exists_ = true; } else { try { const auto transform_msg = tf_buffer_.lookupTransform(base_link_frame, imu_frame, tf2::TimePointZero); tf2::convert(transform_msg.transform, tf2_imu_to_base_link); - is_imu_transform_exist_ = true; + imu_transform_exists_ = true; } catch (const tf2::TransformException & ex) { RCLCPP_WARN(node_->get_logger(), "%s", ex.what()); RCLCPP_ERROR( @@ -188,7 +188,7 @@ void DistortionCorrector::undistortPointCloud( // If there is a point in a pointcloud that cannot be associated, record it to issue a warning bool is_twist_time_stamp_too_late = false; - bool is_imu_time_stamp_is_too_late = false; + bool is_imu_time_stamp_too_late = false; bool is_twist_valid = true; bool is_imu_valid = true; @@ -214,7 +214,7 @@ void DistortionCorrector::undistortPointCloud( } if (std::abs(*it_time_stamp - imu_stamp) > 0.1) { - is_imu_time_stamp_is_too_late = true; + is_imu_time_stamp_too_late = true; is_imu_valid = false; } } else { @@ -223,26 +223,26 @@ void DistortionCorrector::undistortPointCloud( float time_offset = static_cast(*it_time_stamp - prev_time_stamp_sec); - // Undistorted a single point based on the strategy + // 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 = *it_time_stamp; } - warnIfTimestampsTooLate(is_twist_time_stamp_too_late, is_imu_time_stamp_is_too_late); + warnIfTimestampsTooLate(is_twist_time_stamp_too_late, is_imu_time_stamp_too_late); } template void DistortionCorrector::warnIfTimestampsTooLate( - bool is_twist_time_stamp_too_late, bool is_imu_time_stamp_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."); + "Twist time_stamp is too late. Could not interpolate."); } - if (is_imu_time_stamp_is_too_late) { + 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."); @@ -266,7 +266,7 @@ void DistortionCorrector3D::initialize() void DistortionCorrector2D::setPointCloudTransform( const std::string & base_link_frame, const std::string & lidar_frame) { - if (is_pointcloud_transform_exist_) { + if (pointcloud_transform_exists_) { return; } @@ -274,15 +274,15 @@ void DistortionCorrector2D::setPointCloudTransform( tf2_lidar_to_base_link_.setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); tf2_lidar_to_base_link_.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0)); tf2_base_link_to_lidar_ = tf2_lidar_to_base_link_; - is_pointcloud_transform_exist_ = true; + pointcloud_transform_exists_ = true; } else { try { const auto transform_msg = tf_buffer_.lookupTransform(base_link_frame, lidar_frame, tf2::TimePointZero); tf2::convert(transform_msg.transform, tf2_lidar_to_base_link_); tf2_base_link_to_lidar_ = tf2_lidar_to_base_link_.inverse(); - is_pointcloud_transform_exist_ = true; - is_pointcloud_transform_needed_ = true; + pointcloud_transform_exists_ = true; + pointcloud_transform_needed_ = true; } catch (const tf2::TransformException & ex) { RCLCPP_WARN(node_->get_logger(), "%s", ex.what()); RCLCPP_ERROR( @@ -299,14 +299,14 @@ void DistortionCorrector2D::setPointCloudTransform( void DistortionCorrector3D::setPointCloudTransform( const std::string & base_link_frame, const std::string & lidar_frame) { - if (is_pointcloud_transform_exist_) { + if (pointcloud_transform_exists_) { return; } if (base_link_frame == lidar_frame) { eigen_lidar_to_base_link_ = Eigen::Matrix4f::Identity(); eigen_base_link_to_lidar_ = Eigen::Matrix4f::Identity(); - is_pointcloud_transform_exist_ = true; + pointcloud_transform_exists_ = true; } try { @@ -315,8 +315,8 @@ void DistortionCorrector3D::setPointCloudTransform( eigen_lidar_to_base_link_ = tf2::transformToEigen(transform_msg.transform).matrix().cast(); eigen_base_link_to_lidar_ = eigen_lidar_to_base_link_.inverse(); - is_pointcloud_transform_exist_ = true; - is_pointcloud_transform_needed_ = true; + pointcloud_transform_exists_ = true; + pointcloud_transform_needed_ = true; } catch (const tf2::TransformException & ex) { RCLCPP_WARN(node_->get_logger(), "%s", ex.what()); RCLCPP_ERROR( @@ -347,7 +347,7 @@ void DistortionCorrector2D::undistortPointImplementation( // Undistort point point_tf_.setValue(*it_x, *it_y, *it_z); - if (is_pointcloud_transform_needed_) { + if (pointcloud_transform_needed_) { point_tf_ = tf2_lidar_to_base_link_ * point_tf_; } theta_ += w * time_offset; @@ -363,7 +363,7 @@ void DistortionCorrector2D::undistortPointImplementation( undistorted_point_tf_ = baselink_tf_odom_ * point_tf_; - if (is_pointcloud_transform_needed_) { + if (pointcloud_transform_needed_) { undistorted_point_tf_ = tf2_base_link_to_lidar_ * undistorted_point_tf_; } @@ -397,7 +397,7 @@ void DistortionCorrector3D::undistortPointImplementation( // Undistort point point_eigen_ << *it_x, *it_y, *it_z, 1.0; - if (is_pointcloud_transform_needed_) { + if (pointcloud_transform_needed_) { point_eigen_ = eigen_lidar_to_base_link_ * point_eigen_; } @@ -407,7 +407,7 @@ void DistortionCorrector3D::undistortPointImplementation( transformation_matrix_ = transformation_matrix_ * prev_transformation_matrix_; undistorted_point_eigen_ = transformation_matrix_ * point_eigen_; - if (is_pointcloud_transform_needed_) { + if (pointcloud_transform_needed_) { undistorted_point_eigen_ = eigen_base_link_to_lidar_ * undistorted_point_eigen_; } *it_x = undistorted_point_eigen_[0]; diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_nodelet.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp similarity index 83% rename from sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_nodelet.cpp rename to sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp index e3e5d49a176fb..7e125e320f15c 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pointcloud_preprocessor/distortion_corrector/distortion_corrector_nodelet.hpp" +#include "pointcloud_preprocessor/distortion_corrector/distortion_corrector_node.hpp" #include "pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp" @@ -43,28 +43,27 @@ DistortionCorrectorComponent::DistortionCorrectorComponent(const rclcpp::NodeOpt // Subscriber twist_sub_ = this->create_subscription( "~/input/twist", 10, - std::bind( - &DistortionCorrectorComponent::onTwistWithCovarianceStamped, this, std::placeholders::_1)); + std::bind(&DistortionCorrectorComponent::onTwist, this, std::placeholders::_1)); imu_sub_ = this->create_subscription( "~/input/imu", 10, std::bind(&DistortionCorrectorComponent::onImu, this, std::placeholders::_1)); - input_points_sub_ = this->create_subscription( + pointcloud_sub_ = this->create_subscription( "~/input/pointcloud", rclcpp::SensorDataQoS(), std::bind(&DistortionCorrectorComponent::onPointCloud, this, std::placeholders::_1)); // Setup the distortion corrector if (use_3d_distortion_correction_) { - DistortionCorrector_ = std::make_unique(this); + distortion_corrector_ = std::make_unique(this); } else { - DistortionCorrector_ = std::make_unique(this); + distortion_corrector_ = std::make_unique(this); } } -void DistortionCorrectorComponent::onTwistWithCovarianceStamped( +void DistortionCorrectorComponent::onTwist( const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) { - DistortionCorrector_->processTwistMessage(twist_msg); + distortion_corrector_->processTwistMessage(twist_msg); } void DistortionCorrectorComponent::onImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) @@ -73,7 +72,7 @@ void DistortionCorrectorComponent::onImu(const sensor_msgs::msg::Imu::ConstShare return; } - DistortionCorrector_->processIMUMessage(base_link_frame_, imu_msg); + distortion_corrector_->processIMUMessage(base_link_frame_, imu_msg); } void DistortionCorrectorComponent::onPointCloud(PointCloud2::UniquePtr pointcloud_msg) @@ -86,10 +85,10 @@ void DistortionCorrectorComponent::onPointCloud(PointCloud2::UniquePtr pointclou return; } - DistortionCorrector_->setPointCloudTransform(base_link_frame_, pointcloud_msg->header.frame_id); + distortion_corrector_->setPointCloudTransform(base_link_frame_, pointcloud_msg->header.frame_id); - DistortionCorrector_->initialize(); - DistortionCorrector_->undistortPointCloud(use_imu_, *pointcloud_msg); + distortion_corrector_->initialize(); + distortion_corrector_->undistortPointCloud(use_imu_, *pointcloud_msg); if (debug_publisher_) { auto pipeline_latency_ms = From 32fcd04373e15d227b25b3144a75081a616c74a2 Mon Sep 17 00:00:00 2001 From: vividf Date: Thu, 13 Jun 2024 13:55:48 +0900 Subject: [PATCH 29/47] chore: fix invlaid virtual function definitions Signed-off-by: vividf --- .../distortion_corrector/distortion_corrector.hpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp index 4b7f93c349980..16595134f6447 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp @@ -96,8 +96,6 @@ class DistortionCorrector : public DistortionCorrectorBase std::deque::iterator & it_imu); void undistortPointCloud(bool use_imu, sensor_msgs::msg::PointCloud2 & pointcloud) override; void warnIfTimestampsTooLate(bool is_twist_time_stamp_too_late, bool is_imu_time_stamp_too_late); - - virtual void initialize() = 0; void undistortPoint( sensor_msgs::PointCloud2Iterator & it_x, sensor_msgs::PointCloud2Iterator & it_y, sensor_msgs::PointCloud2Iterator & it_z, @@ -108,9 +106,6 @@ class DistortionCorrector : public DistortionCorrectorBase static_cast(this)->undistortPointImplementation( it_x, it_y, it_z, it_twist, it_imu, time_offset, is_twist_valid, is_imu_valid); }; - - virtual void setPointCloudTransform( - const std::string & base_link_frame, const std::string & lidar_frame) = 0; }; class DistortionCorrector2D : public DistortionCorrector From 3e3c644d16788fa75cd323078aca078232639443 Mon Sep 17 00:00:00 2001 From: vividf Date: Thu, 13 Jun 2024 17:51:47 +0900 Subject: [PATCH 30/47] fix: add sophus in package dependency Signed-off-by: vividf --- sensing/pointcloud_preprocessor/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/sensing/pointcloud_preprocessor/package.xml b/sensing/pointcloud_preprocessor/package.xml index 619bf3180591d..f2c0bcd23f68b 100644 --- a/sensing/pointcloud_preprocessor/package.xml +++ b/sensing/pointcloud_preprocessor/package.xml @@ -41,6 +41,7 @@ rclcpp rclcpp_components sensor_msgs + sophus std_msgs tf2 tf2_eigen From 68d5cb9a1bbe87933110cac969b71536f4bffc6a Mon Sep 17 00:00:00 2001 From: vividf Date: Fri, 14 Jun 2024 11:22:53 +0900 Subject: [PATCH 31/47] chore: remove brackets Signed-off-by: vividf --- .../distortion_corrector_node.cpp | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp index 7e125e320f15c..29d65dc817699 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp @@ -23,14 +23,13 @@ DistortionCorrectorComponent::DistortionCorrectorComponent(const rclcpp::NodeOpt : Node("distortion_corrector_node", options) { // initialize debug tool - { - using tier4_autoware_utils::DebugPublisher; - using tier4_autoware_utils::StopWatch; - stop_watch_ptr_ = std::make_unique>(); - debug_publisher_ = std::make_unique(this, "distortion_corrector"); - stop_watch_ptr_->tic("cyclic_time"); - stop_watch_ptr_->tic("processing_time"); - } + + using tier4_autoware_utils::DebugPublisher; + using tier4_autoware_utils::StopWatch; + stop_watch_ptr_ = std::make_unique>(); + debug_publisher_ = std::make_unique(this, "distortion_corrector"); + stop_watch_ptr_->tic("cyclic_time"); + stop_watch_ptr_->tic("processing_time"); // Parameter use_imu_ = declare_parameter("use_imu", true); From 2aad8c080fd8b1463de9e9b215f9947f8e9674db Mon Sep 17 00:00:00 2001 From: vividf Date: Fri, 14 Jun 2024 11:47:29 +0900 Subject: [PATCH 32/47] chore: fix algorithm Signed-off-by: vividf --- .../pointcloud_preprocessor/docs/distortion-corrector.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md index 11be738001ad9..9ce45047414e6 100644 --- a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md +++ b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md @@ -8,9 +8,9 @@ Since the LiDAR sensor scans by rotating an internal laser, the resulting point ## Inner-workings / Algorithms -The node utilizes twist information (linear velocity and angular velocity) from the `~/input/twist` topic. If the user sets `use_imu` to true, the node will replace the twist's angular velocity with the angular velocity from IMU. The node then moves every point in the pointcloud to compensate for the motion provided by the velocity information. +The node uses twist information (linear and angular velocity) from the `~/input/twist` topic to correct each point in the point cloud. If the user sets `use_imu` to true, the node will replace the twist's angular velocity with the angular velocity from IMU. -The node also supports two different modes of distortion correction: 2D distortion correction and 3D distortion correction. The main difference is that the 2D distortion corrector only utilizes the x-axis of linear velocity and the z-axis of angular velocity to correct the point positions. On the other hand, the 3D distortion corrector utilizes all linear and angular velocity components to correct the point positions. +The node supports two different modes of distortion correction: 2D distortion correction and 3D distortion correction. The main difference is that the 2D distortion corrector only utilizes the x-axis of linear velocity and the z-axis of angular velocity to correct the point positions. On the other hand, the 3D distortion corrector utilizes all linear and angular velocity components to correct the point positions. Please note that the processing time difference between the two distortion methods is significant; the 3D corrector takes 50% more time than the 2D corrector. Therefore, it is recommended that in general cases, users should set `use_3d_distortion_correction` to `false`. However, in scenarios such as a vehicle going over speed bumps, using the 3D corrector can be beneficial. @@ -45,4 +45,4 @@ Please note that the processing time difference between the two distortion metho ## Assumptions / Known limits - The node requires that time synchronization works well between the pointcloud, twist, and IMU. -- If you want to use 3D distortion corrector without IMU, please check that the linear and angular velocity fields of your twist message are not empty. +- 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. From 6d24eba7fc81b4605501d7d38bc401fb53e8716f Mon Sep 17 00:00:00 2001 From: vividf Date: Fri, 14 Jun 2024 11:53:14 +0900 Subject: [PATCH 33/47] chore: remove timestamp_field_name and add default parameter for 3d distortion Signed-off-by: vividf --- .../pointcloud_preprocessor/docs/distortion-corrector.md | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md index 9ce45047414e6..62df08b3190dc 100644 --- a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md +++ b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md @@ -36,11 +36,10 @@ Please note that the processing time difference between the two distortion metho ### Core Parameters -| Name | Type | Default Value | Description | -| ------------------------------ | -------- | ------------- | ----------------------------------------------------------- | -| `timestamp_field_name` | `string` | `time_stamp` | Name of time stamp field. | -| `use_imu` | `bool` | `true` | Use gyroscope for yaw rate if true, else use vehicle status | -| `use_3d_distortion_correction` | `bool` | | Use 3d correction if true, otherwise use 2d correction | +| Name | Type | Default Value | Description | +| ------------------------------ | ------ | ------------- | ----------------------------------------------------------- | +| `use_imu` | `bool` | `true` | Use gyroscope for yaw rate if true, else use vehicle status | +| `use_3d_distortion_correction` | `bool` | `false` | Use 3d correction if true, otherwise use 2d correction | ## Assumptions / Known limits From 7d890acdb657e1f2799ab27e0de421d8e1ea8194 Mon Sep 17 00:00:00 2001 From: vividf Date: Fri, 14 Jun 2024 11:57:36 +0900 Subject: [PATCH 34/47] chore: fix known limits explanation Signed-off-by: vividf --- sensing/pointcloud_preprocessor/docs/distortion-corrector.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md index 62df08b3190dc..b0ab82294602c 100644 --- a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md +++ b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md @@ -43,5 +43,5 @@ Please note that the processing time difference between the two distortion metho ## Assumptions / Known limits -- The node requires that time synchronization works well between the pointcloud, twist, and IMU. +- The node requires that 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. From 4a828867f93aad70a4d6a2c125c161c98dc4f639 Mon Sep 17 00:00:00 2001 From: vividf Date: Fri, 14 Jun 2024 14:29:23 +0900 Subject: [PATCH 35/47] feat: add parameter schema and launch file for distortion correction node Signed-off-by: vividf --- .../distortion_corrector_node.param.yaml | 5 +++ .../docs/distortion-corrector.md | 27 +++++++------ .../distortion_corrector.hpp | 13 +++--- .../distortion_corrector_node.hpp | 4 +- .../distortion_corrector_node.launch.xml | 16 ++++++++ .../distortion_corrector_node.schema.json | 40 +++++++++++++++++++ .../distortion_corrector.cpp | 31 +++++++------- .../distortion_corrector_node.cpp | 17 ++++---- 8 files changed, 107 insertions(+), 46 deletions(-) create mode 100644 sensing/pointcloud_preprocessor/config/distortion_corrector_node.param.yaml create mode 100644 sensing/pointcloud_preprocessor/launch/distortion_corrector_node.launch.xml create mode 100644 sensing/pointcloud_preprocessor/schema/distortion_corrector_node.schema.json diff --git a/sensing/pointcloud_preprocessor/config/distortion_corrector_node.param.yaml b/sensing/pointcloud_preprocessor/config/distortion_corrector_node.param.yaml new file mode 100644 index 0000000000000..3afa4816271cb --- /dev/null +++ b/sensing/pointcloud_preprocessor/config/distortion_corrector_node.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + base_frame: base_link + use_imu: true + use_3d_distortion_correction: false diff --git a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md index b0ab82294602c..81a87d8bc0cdc 100644 --- a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md +++ b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md @@ -20,26 +20,29 @@ Please note that the processing time difference between the two distortion metho ### Input -| Name | Type | Description | -| ---------------- | ------------------------------------------------ | ---------------------------------- | -| `~/input/points` | `sensor_msgs::msg::PointCloud2` | Topic of the distorted pointcloud. | -| `~/input/twist` | `geometry_msgs::msg::TwistWithCovarianceStamped` | Topic of the twist information. | -| `~/input/imu` | `sensor_msgs::msg::Imu` | Topic of the IMU data. | +| Name | Type | Description | +| -------------------- | ------------------------------------------------ | ---------------------------------- | +| `~/input/pointcloud` | `sensor_msgs::msg::PointCloud2` | Topic of the distorted pointcloud. | +| `~/input/twist` | `geometry_msgs::msg::TwistWithCovarianceStamped` | Topic of the twist information. | +| `~/input/imu` | `sensor_msgs::msg::Imu` | Topic of the IMU data. | ### Output -| Name | Type | Description | -| ----------------- | ------------------------------- | ----------------------------------- | -| `~/output/points` | `sensor_msgs::msg::PointCloud2` | Topic of the undistorted pointcloud | +| Name | Type | Description | +| --------------------- | ------------------------------- | ----------------------------------- | +| `~/output/pointcloud` | `sensor_msgs::msg::PointCloud2` | Topic of the undistorted pointcloud | ## Parameters ### Core Parameters -| Name | Type | Default Value | Description | -| ------------------------------ | ------ | ------------- | ----------------------------------------------------------- | -| `use_imu` | `bool` | `true` | Use gyroscope for yaw rate if true, else use vehicle status | -| `use_3d_distortion_correction` | `bool` | `false` | Use 3d correction if true, otherwise use 2d correction | +{{ json_to_markdown("sensing/pointcloud_preprocessor/schema/distortion_corrector.schema.json") }} + +## Launch + +```bash +ros2 launch pointcloud_preprocessor distortion_corrector.launch.xml +``` ## Assumptions / Known limits diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp index 16595134f6447..16247b79872d1 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp @@ -51,9 +51,9 @@ class DistortionCorrectorBase virtual void processTwistMessage( const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) = 0; virtual void processIMUMessage( - const std::string & base_link_frame, const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) = 0; + const std::string & base_frame, const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) = 0; virtual void setPointCloudTransform( - const std::string & base_link_frame, const std::string & lidar_frame) = 0; + 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; }; @@ -80,10 +80,9 @@ class DistortionCorrector : public DistortionCorrectorBase const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) override; void processIMUMessage( - const std::string & base_link_frame, - const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) override; + const std::string & base_frame, const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) override; void getIMUTransformation( - const std::string & base_link_frame, const std::string & imu_frame, + const std::string & base_frame, const std::string & imu_frame, geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr); void storeIMUToQueue( const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg, @@ -135,7 +134,7 @@ class DistortionCorrector2D : public DistortionCorrector bool & is_twist_valid, bool & is_imu_valid); void setPointCloudTransform( - const std::string & base_link_frame, const std::string & lidar_frame) override; + const std::string & base_frame, const std::string & lidar_frame) override; }; class DistortionCorrector3D : public DistortionCorrector @@ -161,7 +160,7 @@ class DistortionCorrector3D : public DistortionCorrector std::deque::iterator & it_imu, float & time_offset, bool & is_twist_valid, bool & is_imu_valid); void setPointCloudTransform( - const std::string & base_link_frame, const std::string & lidar_frame) override; + const std::string & base_frame, const std::string & lidar_frame) override; }; } // namespace pointcloud_preprocessor diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector_node.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector_node.hpp index a211810330ff8..c6b1008c19c8d 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector_node.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector_node.hpp @@ -44,12 +44,12 @@ class DistortionCorrectorComponent : public rclcpp::Node rclcpp::Subscription::SharedPtr imu_sub_; rclcpp::Subscription::SharedPtr pointcloud_sub_; - rclcpp::Publisher::SharedPtr undistorted_points_pub_; + rclcpp::Publisher::SharedPtr undistorted_pointcloud_pub_; std::unique_ptr> stop_watch_ptr_; std::unique_ptr debug_publisher_; - std::string base_link_frame_ = "base_link"; + std::string base_frame_; bool use_imu_; bool use_3d_distortion_correction_; diff --git a/sensing/pointcloud_preprocessor/launch/distortion_corrector_node.launch.xml b/sensing/pointcloud_preprocessor/launch/distortion_corrector_node.launch.xml new file mode 100644 index 0000000000000..af532b0bb80f8 --- /dev/null +++ b/sensing/pointcloud_preprocessor/launch/distortion_corrector_node.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/sensing/pointcloud_preprocessor/schema/distortion_corrector_node.schema.json b/sensing/pointcloud_preprocessor/schema/distortion_corrector_node.schema.json new file mode 100644 index 0000000000000..acf67fd2746c4 --- /dev/null +++ b/sensing/pointcloud_preprocessor/schema/distortion_corrector_node.schema.json @@ -0,0 +1,40 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Distortion Corrector Node", + "type": "object", + "definitions": { + "distortion_corrector": { + "type": "object", + "properties": { + "base_frame": { + "type": "string", + "description": "The undistortion algorithm is based on a base frame, which must be the same as the twist frame.", + "default": "base_link" + }, + "use_imu": { + "type": "boolean", + "description": "Use IMU angular velocity, otherwise, use twist angular velocity.", + "default": "true" + }, + "use_3d_distortion_correction": { + "type": "boolean", + "description": "Use 3d distortion correction algorithm, otherwise, use 2d distortion correction algorithm.", + "default": "false" + } + }, + "required": ["base_frame", "use_imu", "use_3d_distortion_correction"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/distortion_corrector" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index ef5223d7bd8e8..fe6524b758c0f 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -44,17 +44,17 @@ void DistortionCorrector::processTwistMessage( template void DistortionCorrector::processIMUMessage( - const std::string & base_link_frame, const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) + const std::string & base_frame, const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) { geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr = std::make_shared(); - getIMUTransformation(base_link_frame, imu_msg->header.frame_id, geometry_imu_to_base_link_ptr); + getIMUTransformation(base_frame, imu_msg->header.frame_id, geometry_imu_to_base_link_ptr); storeIMUToQueue(imu_msg, geometry_imu_to_base_link_ptr); } template void DistortionCorrector::getIMUTransformation( - const std::string & base_link_frame, const std::string & imu_frame, + const std::string & base_frame, const std::string & imu_frame, geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr) { if (imu_transform_exists_) { @@ -62,21 +62,20 @@ void DistortionCorrector::getIMUTransformation( } tf2::Transform tf2_imu_to_base_link; - if (base_link_frame == imu_frame) { + if (base_frame == imu_frame) { tf2_imu_to_base_link.setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); tf2_imu_to_base_link.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0)); imu_transform_exists_ = true; } else { try { const auto transform_msg = - tf_buffer_.lookupTransform(base_link_frame, imu_frame, tf2::TimePointZero); + tf_buffer_.lookupTransform(base_frame, imu_frame, tf2::TimePointZero); tf2::convert(transform_msg.transform, tf2_imu_to_base_link); imu_transform_exists_ = true; } catch (const tf2::TransformException & ex) { RCLCPP_WARN(node_->get_logger(), "%s", ex.what()); RCLCPP_ERROR( - node_->get_logger(), "Please publish TF %s to %s", base_link_frame.c_str(), - imu_frame.c_str()); + node_->get_logger(), "Please publish TF %s to %s", base_frame.c_str(), imu_frame.c_str()); tf2_imu_to_base_link.setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); tf2_imu_to_base_link.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0)); @@ -264,13 +263,13 @@ void DistortionCorrector3D::initialize() } void DistortionCorrector2D::setPointCloudTransform( - const std::string & base_link_frame, const std::string & lidar_frame) + const std::string & base_frame, const std::string & lidar_frame) { if (pointcloud_transform_exists_) { return; } - if (base_link_frame == lidar_frame) { + if (base_frame == lidar_frame) { tf2_lidar_to_base_link_.setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); tf2_lidar_to_base_link_.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0)); tf2_base_link_to_lidar_ = tf2_lidar_to_base_link_; @@ -278,7 +277,7 @@ void DistortionCorrector2D::setPointCloudTransform( } else { try { const auto transform_msg = - tf_buffer_.lookupTransform(base_link_frame, lidar_frame, tf2::TimePointZero); + tf_buffer_.lookupTransform(base_frame, lidar_frame, tf2::TimePointZero); tf2::convert(transform_msg.transform, tf2_lidar_to_base_link_); tf2_base_link_to_lidar_ = tf2_lidar_to_base_link_.inverse(); pointcloud_transform_exists_ = true; @@ -286,8 +285,7 @@ void DistortionCorrector2D::setPointCloudTransform( } catch (const tf2::TransformException & ex) { RCLCPP_WARN(node_->get_logger(), "%s", ex.what()); RCLCPP_ERROR( - node_->get_logger(), "Please publish TF %s to %s", base_link_frame.c_str(), - lidar_frame.c_str()); + node_->get_logger(), "Please publish TF %s to %s", base_frame.c_str(), lidar_frame.c_str()); tf2_lidar_to_base_link_.setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); tf2_lidar_to_base_link_.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0)); @@ -297,13 +295,13 @@ void DistortionCorrector2D::setPointCloudTransform( } void DistortionCorrector3D::setPointCloudTransform( - const std::string & base_link_frame, const std::string & lidar_frame) + const std::string & base_frame, const std::string & lidar_frame) { if (pointcloud_transform_exists_) { return; } - if (base_link_frame == lidar_frame) { + if (base_frame == lidar_frame) { eigen_lidar_to_base_link_ = Eigen::Matrix4f::Identity(); eigen_base_link_to_lidar_ = Eigen::Matrix4f::Identity(); pointcloud_transform_exists_ = true; @@ -311,7 +309,7 @@ void DistortionCorrector3D::setPointCloudTransform( try { const auto transform_msg = - tf_buffer_.lookupTransform(base_link_frame, lidar_frame, tf2::TimePointZero); + tf_buffer_.lookupTransform(base_frame, lidar_frame, tf2::TimePointZero); eigen_lidar_to_base_link_ = tf2::transformToEigen(transform_msg.transform).matrix().cast(); eigen_base_link_to_lidar_ = eigen_lidar_to_base_link_.inverse(); @@ -320,8 +318,7 @@ void DistortionCorrector3D::setPointCloudTransform( } catch (const tf2::TransformException & ex) { RCLCPP_WARN(node_->get_logger(), "%s", ex.what()); RCLCPP_ERROR( - node_->get_logger(), "Please publish TF %s to %s", base_link_frame.c_str(), - lidar_frame.c_str()); + node_->get_logger(), "Please publish TF %s to %s", base_frame.c_str(), lidar_frame.c_str()); eigen_lidar_to_base_link_ = Eigen::Matrix4f::Identity(); eigen_base_link_to_lidar_ = Eigen::Matrix4f::Identity(); } diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp index 29d65dc817699..35cbbdf2acda7 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp @@ -32,11 +32,12 @@ DistortionCorrectorComponent::DistortionCorrectorComponent(const rclcpp::NodeOpt stop_watch_ptr_->tic("processing_time"); // Parameter - use_imu_ = declare_parameter("use_imu", true); - use_3d_distortion_correction_ = declare_parameter("use_3d_distortion_correction", false); + base_frame_ = declare_parameter("base_frame"); + use_imu_ = declare_parameter("use_imu"); + use_3d_distortion_correction_ = declare_parameter("use_3d_distortion_correction"); // Publisher - undistorted_points_pub_ = + undistorted_pointcloud_pub_ = this->create_publisher("~/output/pointcloud", rclcpp::SensorDataQoS()); // Subscriber @@ -71,20 +72,20 @@ void DistortionCorrectorComponent::onImu(const sensor_msgs::msg::Imu::ConstShare return; } - distortion_corrector_->processIMUMessage(base_link_frame_, imu_msg); + distortion_corrector_->processIMUMessage(base_frame_, imu_msg); } void DistortionCorrectorComponent::onPointCloud(PointCloud2::UniquePtr pointcloud_msg) { stop_watch_ptr_->toc("processing_time", true); - const auto points_sub_count = undistorted_points_pub_->get_subscription_count() + - undistorted_points_pub_->get_intra_process_subscription_count(); + const auto points_sub_count = undistorted_pointcloud_pub_->get_subscription_count() + + undistorted_pointcloud_pub_->get_intra_process_subscription_count(); if (points_sub_count < 1) { return; } - distortion_corrector_->setPointCloudTransform(base_link_frame_, pointcloud_msg->header.frame_id); + distortion_corrector_->setPointCloudTransform(base_frame_, pointcloud_msg->header.frame_id); distortion_corrector_->initialize(); distortion_corrector_->undistortPointCloud(use_imu_, *pointcloud_msg); @@ -99,7 +100,7 @@ void DistortionCorrectorComponent::onPointCloud(PointCloud2::UniquePtr pointclou "debug/pipeline_latency_ms", pipeline_latency_ms); } - undistorted_points_pub_->publish(std::move(pointcloud_msg)); + undistorted_pointcloud_pub_->publish(std::move(pointcloud_msg)); // add processing time for debug if (debug_publisher_) { From e4322261116724ee23d7a66ad3b8c0da63ee992e Mon Sep 17 00:00:00 2001 From: vividf Date: Fri, 14 Jun 2024 15:38:32 +0900 Subject: [PATCH 36/47] chore: fix function name Signed-off-by: vividf --- .../distortion_corrector/distortion_corrector.hpp | 2 +- .../src/distortion_corrector/distortion_corrector.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp index 16247b79872d1..bdd3a8d6928d0 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp @@ -94,7 +94,7 @@ class DistortionCorrector : public DistortionCorrectorBase std::deque::iterator & it_twist, std::deque::iterator & it_imu); void undistortPointCloud(bool use_imu, sensor_msgs::msg::PointCloud2 & pointcloud) override; - void warnIfTimestampsTooLate(bool is_twist_time_stamp_too_late, bool is_imu_time_stamp_too_late); + 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, diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index fe6524b758c0f..9c7888096468e 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -228,11 +228,11 @@ void DistortionCorrector::undistortPointCloud( prev_time_stamp_sec = *it_time_stamp; } - warnIfTimestampsTooLate(is_twist_time_stamp_too_late, is_imu_time_stamp_too_late); + warnIfTimestampIsTooLate(is_twist_time_stamp_too_late, is_imu_time_stamp_too_late); } template -void DistortionCorrector::warnIfTimestampsTooLate( +void DistortionCorrector::warnIfTimestampIsTooLate( bool is_twist_time_stamp_too_late, bool is_imu_time_stamp_too_late) { if (is_twist_time_stamp_too_late) { From c6d9cc16fe5eeb934fa47a329484e39202a52bc6 Mon Sep 17 00:00:00 2001 From: vividf Date: Fri, 14 Jun 2024 15:41:30 +0900 Subject: [PATCH 37/47] chore: fix IMU function name Signed-off-by: vividf --- .../distortion_corrector/distortion_corrector.hpp | 2 +- .../src/distortion_corrector/distortion_corrector.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp index bdd3a8d6928d0..afdf228a473f8 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp @@ -84,7 +84,7 @@ class DistortionCorrector : public DistortionCorrectorBase void getIMUTransformation( const std::string & base_frame, const std::string & imu_frame, geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr); - void storeIMUToQueue( + void enqueueIMU( const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg, geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr); diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index 9c7888096468e..b6db13f3ee713 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -49,7 +49,7 @@ void DistortionCorrector::processIMUMessage( geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr = std::make_shared(); getIMUTransformation(base_frame, imu_msg->header.frame_id, geometry_imu_to_base_link_ptr); - storeIMUToQueue(imu_msg, geometry_imu_to_base_link_ptr); + enqueueIMU(imu_msg, geometry_imu_to_base_link_ptr); } template @@ -87,7 +87,7 @@ void DistortionCorrector::getIMUTransformation( } template -void DistortionCorrector::storeIMUToQueue( +void DistortionCorrector::enqueueIMU( const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg, geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr) { From ae6ae5c3f30f0a10550dab21c7fee73b77cf483f Mon Sep 17 00:00:00 2001 From: vividf Date: Fri, 14 Jun 2024 15:43:00 +0900 Subject: [PATCH 38/47] chore: fix twist and imu iterator function name Signed-off-by: vividf --- .../distortion_corrector/distortion_corrector.hpp | 2 +- .../src/distortion_corrector/distortion_corrector.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp index afdf228a473f8..fbe015d4975f7 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp @@ -89,7 +89,7 @@ class DistortionCorrector : public DistortionCorrectorBase geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr); bool isInputValid(sensor_msgs::msg::PointCloud2 & pointcloud); - void getIteratorOfTwistAndIMU( + void getTwistAndIMUIterator( bool use_imu, double first_point_time_stamp_sec, std::deque::iterator & it_twist, std::deque::iterator & it_imu); diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index b6db13f3ee713..1bbdf777f7c4e 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -115,7 +115,7 @@ void DistortionCorrector::enqueueIMU( } template -void DistortionCorrector::getIteratorOfTwistAndIMU( +void DistortionCorrector::getTwistAndIMUIterator( bool use_imu, double first_point_time_stamp_sec, std::deque::iterator & it_twist, std::deque::iterator & it_imu) @@ -176,7 +176,7 @@ void DistortionCorrector::undistortPointCloud( std::deque::iterator it_twist; std::deque::iterator it_imu; - getIteratorOfTwistAndIMU(use_imu, first_point_time_stamp_sec, it_twist, it_imu); + getTwistAndIMUIterator(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(); From 39f4f629a3ac624bc8e89e3ee72b0f085946f105 Mon Sep 17 00:00:00 2001 From: vividf Date: Fri, 14 Jun 2024 17:11:08 +0900 Subject: [PATCH 39/47] fix: add inline for undistortPointcloud function Signed-off-by: vividf --- .../distortion_corrector/distortion_corrector.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp index fbe015d4975f7..b298eb25f3cc6 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp @@ -95,7 +95,7 @@ class DistortionCorrector : public DistortionCorrectorBase std::deque::iterator & it_imu); void undistortPointCloud(bool use_imu, sensor_msgs::msg::PointCloud2 & pointcloud) override; void warnIfTimestampIsTooLate(bool is_twist_time_stamp_too_late, bool is_imu_time_stamp_too_late); - void undistortPoint( + inline void undistortPoint( sensor_msgs::PointCloud2Iterator & it_x, sensor_msgs::PointCloud2Iterator & it_y, sensor_msgs::PointCloud2Iterator & it_z, std::deque::iterator & it_twist, @@ -126,7 +126,7 @@ class DistortionCorrector2D : public DistortionCorrector public: explicit DistortionCorrector2D(rclcpp::Node * node) : DistortionCorrector(node) {} void initialize() override; - void undistortPointImplementation( + inline void undistortPointImplementation( sensor_msgs::PointCloud2Iterator & it_x, sensor_msgs::PointCloud2Iterator & it_y, sensor_msgs::PointCloud2Iterator & it_z, std::deque::iterator & it_twist, @@ -153,7 +153,7 @@ class DistortionCorrector3D : public DistortionCorrector public: explicit DistortionCorrector3D(rclcpp::Node * node) : DistortionCorrector(node) {} void initialize() override; - void undistortPointImplementation( + inline void undistortPointImplementation( sensor_msgs::PointCloud2Iterator & it_x, sensor_msgs::PointCloud2Iterator & it_y, sensor_msgs::PointCloud2Iterator & it_z, std::deque::iterator & it_twist, From b5d74cadb33c54c937e89e7ea1c05b37001dc446 Mon Sep 17 00:00:00 2001 From: vividf Date: Fri, 14 Jun 2024 17:30:59 +0900 Subject: [PATCH 40/47] chore: move varialbe to const Signed-off-by: vividf --- .../distortion_corrector/distortion_corrector.hpp | 12 ++++++------ .../distortion_corrector/distortion_corrector.cpp | 8 ++++---- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp index b298eb25f3cc6..0ec646d69f85a 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp @@ -99,8 +99,8 @@ class DistortionCorrector : public DistortionCorrectorBase 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 & time_offset, - bool & is_twist_valid, bool & is_imu_valid) + 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); @@ -130,8 +130,8 @@ class DistortionCorrector2D : public DistortionCorrector 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 & time_offset, - bool & is_twist_valid, bool & is_imu_valid); + 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; @@ -157,8 +157,8 @@ class DistortionCorrector3D : public DistortionCorrector 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 & time_offset, - bool & is_twist_valid, bool & is_imu_valid); + 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; }; diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index 1bbdf777f7c4e..0c5b9cb1966b1 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -328,8 +328,8 @@ void DistortionCorrector2D::undistortPointImplementation( 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 & time_offset, - bool & is_twist_valid, bool & is_imu_valid) + std::deque::iterator & it_imu, const float & time_offset, + const bool & is_twist_valid, const bool & is_imu_valid) { // Initialize linear velocity and angular velocity float v{0.0f}, w{0.0f}; @@ -373,8 +373,8 @@ void DistortionCorrector3D::undistortPointImplementation( 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 & time_offset, - bool & is_twist_valid, bool & is_imu_valid) + std::deque::iterator & it_imu, const float & time_offset, + 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}; From 51e96be55b4cd07b28240379f2694500eb1dc3dc Mon Sep 17 00:00:00 2001 From: vividf Date: Mon, 17 Jun 2024 15:01:58 +0900 Subject: [PATCH 41/47] chore: fix grammar error Signed-off-by: vividf --- sensing/pointcloud_preprocessor/docs/distortion-corrector.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md index 81a87d8bc0cdc..96124f473f825 100644 --- a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md +++ b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md @@ -46,5 +46,5 @@ ros2 launch pointcloud_preprocessor distortion_corrector.launch.xml ## Assumptions / Known limits -- The node requires that time synchronization between the topics from lidars, twist, and IMU. +- 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. From 76603ebb366e817f51b33d32141a831d4a926f6d Mon Sep 17 00:00:00 2001 From: vividf Date: Mon, 17 Jun 2024 16:39:35 +0900 Subject: [PATCH 42/47] fix: fix inline function Signed-off-by: vividf --- .../distortion_corrector/distortion_corrector.hpp | 6 +++--- .../launch/distortion_corrector_node.launch.xml | 2 +- .../src/distortion_corrector/distortion_corrector.cpp | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp index 0ec646d69f85a..90377bd83a93c 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp @@ -95,7 +95,7 @@ class DistortionCorrector : public DistortionCorrectorBase std::deque::iterator & it_imu); void undistortPointCloud(bool use_imu, sensor_msgs::msg::PointCloud2 & pointcloud) override; void warnIfTimestampIsTooLate(bool is_twist_time_stamp_too_late, bool is_imu_time_stamp_too_late); - inline void undistortPoint( + void undistortPoint( sensor_msgs::PointCloud2Iterator & it_x, sensor_msgs::PointCloud2Iterator & it_y, sensor_msgs::PointCloud2Iterator & it_z, std::deque::iterator & it_twist, @@ -126,7 +126,7 @@ class DistortionCorrector2D : public DistortionCorrector public: explicit DistortionCorrector2D(rclcpp::Node * node) : DistortionCorrector(node) {} void initialize() override; - inline void undistortPointImplementation( + void undistortPointImplementation( sensor_msgs::PointCloud2Iterator & it_x, sensor_msgs::PointCloud2Iterator & it_y, sensor_msgs::PointCloud2Iterator & it_z, std::deque::iterator & it_twist, @@ -153,7 +153,7 @@ class DistortionCorrector3D : public DistortionCorrector public: explicit DistortionCorrector3D(rclcpp::Node * node) : DistortionCorrector(node) {} void initialize() override; - inline void undistortPointImplementation( + void undistortPointImplementation( sensor_msgs::PointCloud2Iterator & it_x, sensor_msgs::PointCloud2Iterator & it_y, sensor_msgs::PointCloud2Iterator & it_z, std::deque::iterator & it_twist, diff --git a/sensing/pointcloud_preprocessor/launch/distortion_corrector_node.launch.xml b/sensing/pointcloud_preprocessor/launch/distortion_corrector_node.launch.xml index af532b0bb80f8..d970aae102264 100644 --- a/sensing/pointcloud_preprocessor/launch/distortion_corrector_node.launch.xml +++ b/sensing/pointcloud_preprocessor/launch/distortion_corrector_node.launch.xml @@ -2,7 +2,7 @@ - + diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index 0c5b9cb1966b1..eb11315e5e809 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -324,7 +324,7 @@ void DistortionCorrector3D::setPointCloudTransform( } } -void DistortionCorrector2D::undistortPointImplementation( +inline void DistortionCorrector2D::undistortPointImplementation( sensor_msgs::PointCloud2Iterator & it_x, sensor_msgs::PointCloud2Iterator & it_y, sensor_msgs::PointCloud2Iterator & it_z, std::deque::iterator & it_twist, @@ -369,7 +369,7 @@ void DistortionCorrector2D::undistortPointImplementation( *it_z = static_cast(undistorted_point_tf_.getZ()); } -void DistortionCorrector3D::undistortPointImplementation( +inline void DistortionCorrector3D::undistortPointImplementation( sensor_msgs::PointCloud2Iterator & it_x, sensor_msgs::PointCloud2Iterator & it_y, sensor_msgs::PointCloud2Iterator & it_z, std::deque::iterator & it_twist, From 6e3215228d2dfc2acd2f3c78705a79ab877147b7 Mon Sep 17 00:00:00 2001 From: vividf Date: Tue, 25 Jun 2024 20:04:17 +0900 Subject: [PATCH 43/47] chore: solve conflicts Signed-off-by: vividf --- .../distortion_corrector/distortion_corrector_node.hpp | 8 ++++---- .../src/distortion_corrector/distortion_corrector.cpp | 10 +++++----- .../distortion_corrector/distortion_corrector_node.cpp | 4 ++-- 3 files changed, 11 insertions(+), 11 deletions(-) diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector_node.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector_node.hpp index c6b1008c19c8d..162170b193827 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector_node.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector_node.hpp @@ -17,9 +17,9 @@ #include "pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp" +#include +#include #include -#include -#include #include #include @@ -46,8 +46,8 @@ class DistortionCorrectorComponent : public rclcpp::Node rclcpp::Publisher::SharedPtr undistorted_pointcloud_pub_; - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr debug_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; std::string base_frame_; bool use_imu_; diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index eb11315e5e809..75e52b175cb83 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -14,7 +14,7 @@ #include "pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp" -#include "tier4_autoware_utils/math/trigonometry.hpp" +#include "autoware/universe_utils/math/trigonometry.hpp" #include @@ -349,11 +349,11 @@ inline void DistortionCorrector2D::undistortPointImplementation( } theta_ += w * time_offset; baselink_quat_.setValue( - 0, 0, tier4_autoware_utils::sin(theta_ * 0.5f), - tier4_autoware_utils::cos(theta_ * 0.5f)); // baselink_quat.setRPY(0.0, 0.0, theta); + 0, 0, autoware::universe_utils::sin(theta_ * 0.5f), + autoware::universe_utils::cos(theta_ * 0.5f)); // baselink_quat.setRPY(0.0, 0.0, theta); const float dis = v * time_offset; - x_ += dis * tier4_autoware_utils::cos(theta_); - y_ += dis * tier4_autoware_utils::sin(theta_); + x_ += dis * autoware::universe_utils::cos(theta_); + y_ += dis * autoware::universe_utils::sin(theta_); baselink_tf_odom_.setOrigin(tf2::Vector3(x_, y_, 0.0)); baselink_tf_odom_.setRotation(baselink_quat_); diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp index 35cbbdf2acda7..2e9581acea845 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp @@ -24,8 +24,8 @@ DistortionCorrectorComponent::DistortionCorrectorComponent(const rclcpp::NodeOpt { // initialize debug tool - using tier4_autoware_utils::DebugPublisher; - using tier4_autoware_utils::StopWatch; + using autoware::universe_utils::DebugPublisher; + using autoware::universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, "distortion_corrector"); stop_watch_ptr_->tic("cyclic_time"); From 9fef37bb6835844e8e816281e922379123c7840c Mon Sep 17 00:00:00 2001 From: vividf Date: Thu, 27 Jun 2024 21:28:38 +0900 Subject: [PATCH 44/47] fix: fix bug in previous code Signed-off-by: vividf --- .../src/distortion_corrector/distortion_corrector.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index 75e52b175cb83..a21261f1f0a1b 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -37,8 +37,9 @@ void DistortionCorrector::processTwistMessage( rclcpp::Time(twist_queue_.front().header.stamp) < rclcpp::Time(twist_msg->header.stamp) - rclcpp::Duration::from_seconds(1.0)) { twist_queue_.pop_front(); + } else { + break; } - break; } } @@ -109,8 +110,9 @@ void DistortionCorrector::enqueueIMU( 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 { + break; } - break; } } From 3c3ca353935b65cc9ae8065e21104317a209e56c Mon Sep 17 00:00:00 2001 From: vividf Date: Wed, 3 Jul 2024 14:40:14 +0900 Subject: [PATCH 45/47] fix: fix the template naming Signed-off-by: vividf --- .../distortion_corrector.hpp | 4 +-- .../distortion_corrector.cpp | 32 +++++++++---------- 2 files changed, 18 insertions(+), 18 deletions(-) diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp index 90377bd83a93c..6f372f0e74646 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp @@ -58,7 +58,7 @@ class DistortionCorrectorBase virtual void undistortPointCloud(bool use_imu, sensor_msgs::msg::PointCloud2 & pointcloud) = 0; }; -template +template class DistortionCorrector : public DistortionCorrectorBase { public: @@ -102,7 +102,7 @@ class DistortionCorrector : public DistortionCorrectorBase std::deque::iterator & it_imu, float const & time_offset, const bool & is_twist_valid, const bool & is_imu_valid) { - static_cast(this)->undistortPointImplementation( + static_cast(this)->undistortPointImplementation( it_x, it_y, it_z, it_twist, it_imu, time_offset, is_twist_valid, is_imu_valid); }; }; diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index a21261f1f0a1b..81c29a9f6202a 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -20,8 +20,8 @@ namespace pointcloud_preprocessor { -template -void DistortionCorrector::processTwistMessage( +template +void DistortionCorrector::processTwistMessage( const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) { geometry_msgs::msg::TwistStamped msg; @@ -43,8 +43,8 @@ void DistortionCorrector::processTwistMessage( } } -template -void DistortionCorrector::processIMUMessage( +template +void DistortionCorrector::processIMUMessage( const std::string & base_frame, const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) { geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr = @@ -53,8 +53,8 @@ void DistortionCorrector::processIMUMessage( enqueueIMU(imu_msg, geometry_imu_to_base_link_ptr); } -template -void DistortionCorrector::getIMUTransformation( +template +void DistortionCorrector::getIMUTransformation( const std::string & base_frame, const std::string & imu_frame, geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr) { @@ -87,8 +87,8 @@ void DistortionCorrector::getIMUTransformation( tf2::toMsg(tf2_imu_to_base_link.getRotation()); } -template -void DistortionCorrector::enqueueIMU( +template +void DistortionCorrector::enqueueIMU( const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg, geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr) { @@ -116,8 +116,8 @@ void DistortionCorrector::enqueueIMU( } } -template -void DistortionCorrector::getTwistAndIMUIterator( +template +void DistortionCorrector::getTwistAndIMUIterator( bool use_imu, double first_point_time_stamp_sec, std::deque::iterator & it_twist, std::deque::iterator & it_imu) @@ -140,8 +140,8 @@ void DistortionCorrector::getTwistAndIMUIterator( } } -template -bool DistortionCorrector::isInputValid(sensor_msgs::msg::PointCloud2 & pointcloud) +template +bool DistortionCorrector::isInputValid(sensor_msgs::msg::PointCloud2 & pointcloud) { if (pointcloud.data.empty() || twist_queue_.empty()) { RCLCPP_WARN_STREAM_THROTTLE( @@ -162,8 +162,8 @@ bool DistortionCorrector::isInputValid(sensor_msgs::msg::PointCloud2 & return true; } -template -void DistortionCorrector::undistortPointCloud( +template +void DistortionCorrector::undistortPointCloud( bool use_imu, sensor_msgs::msg::PointCloud2 & pointcloud) { if (!isInputValid(pointcloud)) return; @@ -233,8 +233,8 @@ void DistortionCorrector::undistortPointCloud( warnIfTimestampIsTooLate(is_twist_time_stamp_too_late, is_imu_time_stamp_too_late); } -template -void DistortionCorrector::warnIfTimestampIsTooLate( +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) { From d9c8b656889ca20473f82c0c31e65e6289be5b58 Mon Sep 17 00:00:00 2001 From: vividf Date: Wed, 3 Jul 2024 15:54:03 +0900 Subject: [PATCH 46/47] fix: fix the component test Signed-off-by: vividf --- .../test/test_distortion_corrector.py | 2 +- .../test/test_distortion_corrector_use_imu_false.py | 6 +++++- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/sensing/pointcloud_preprocessor/test/test_distortion_corrector.py b/sensing/pointcloud_preprocessor/test/test_distortion_corrector.py index 5fdf434123c36..cc80da29e3c13 100644 --- a/sensing/pointcloud_preprocessor/test/test_distortion_corrector.py +++ b/sensing/pointcloud_preprocessor/test/test_distortion_corrector.py @@ -47,7 +47,7 @@ def generate_test_description(): plugin="pointcloud_preprocessor::DistortionCorrectorComponent", name="distortion_corrector_node", parameters=[ - {"use_imu": True}, + {"base_frame": "base_link", "use_imu": True, "use_3d_distortion_correction": False}, ], remappings=[ ("~/input/twist", "/test/sensing/vehicle_velocity_converter/twist_with_covariance"), diff --git a/sensing/pointcloud_preprocessor/test/test_distortion_corrector_use_imu_false.py b/sensing/pointcloud_preprocessor/test/test_distortion_corrector_use_imu_false.py index 5943aec13bca6..35eb41f9dbc17 100644 --- a/sensing/pointcloud_preprocessor/test/test_distortion_corrector_use_imu_false.py +++ b/sensing/pointcloud_preprocessor/test/test_distortion_corrector_use_imu_false.py @@ -46,7 +46,11 @@ def generate_test_description(): plugin="pointcloud_preprocessor::DistortionCorrectorComponent", name="distortion_corrector_node", parameters=[ - {"use_imu": False}, + { + "base_frame": "base_link", + "use_imu": False, + "use_3d_distortion_correction": False, + }, ], remappings=[ ("~/input/twist", "/test/sensing/vehicle_velocity_converter/twist_with_covariance"), From 7f76efba0698c73643b836a7d56e0e71d43651e9 Mon Sep 17 00:00:00 2001 From: vividf Date: Wed, 3 Jul 2024 16:03:33 +0900 Subject: [PATCH 47/47] fix: solve conflicts Signed-off-by: vividf --- .../distortion_corrector/distortion_corrector_node.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp index 2e9581acea845..b5cf581301a23 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp @@ -37,8 +37,13 @@ DistortionCorrectorComponent::DistortionCorrectorComponent(const rclcpp::NodeOpt use_3d_distortion_correction_ = declare_parameter("use_3d_distortion_correction"); // Publisher - undistorted_pointcloud_pub_ = - this->create_publisher("~/output/pointcloud", rclcpp::SensorDataQoS()); + { + rclcpp::PublisherOptions pub_options; + pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); + // Publisher + undistorted_pointcloud_pub_ = this->create_publisher( + "~/output/pointcloud", rclcpp::SensorDataQoS(), pub_options); + } // Subscriber twist_sub_ = this->create_subscription(