From f354a5aaba90e861138b85d75e393c06379e1132 Mon Sep 17 00:00:00 2001 From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com> Date: Wed, 3 Jul 2024 16:42:25 +0900 Subject: [PATCH] feat(pointcloud_preprocessor): support for 3d distortion correction algorithm and refactor distortion correction node (#7137) * add support for 3d distortion correction Signed-off-by: vividf * change parameter back to default and do small refactor Signed-off-by: vividf * init version, need to double check Signed-off-by: vividf * fix the logic error Signed-off-by: vividf * temporary save, need to delete some code * init done, need to check for 3d as time is high Signed-off-by: vividf * fix error Signed-off-by: vividf * temporaily save * clean code Signed-off-by: vividf * remove unused parameters Signed-off-by: vividf * fix constructor error Signed-off-by: vividf * fix spell errors Signed-off-by: vividf * fix more spell errors Signed-off-by: vividf * add undistorter to library Signed-off-by: vividf * fix: fix cmake and change class name Signed-off-by: vividf * Update sensing/pointcloud_preprocessor/docs/distortion-corrector.md Co-authored-by: David Wong <33114676+drwnz@users.noreply.github.com> Signed-off-by: vividf * chore: update sensing/pointcloud_preprocessor/docs/distortion-corrector.md Co-authored-by: David Wong <33114676+drwnz@users.noreply.github.com> Signed-off-by: vividf * chore: update sensing/pointcloud_preprocessor/docs/distortion-corrector.md Co-authored-by: David Wong <33114676+drwnz@users.noreply.github.com> Signed-off-by: vividf * chore: fix company name Co-authored-by: David Wong <33114676+drwnz@users.noreply.github.com> Signed-off-by: vividf * chore: fix company name Co-authored-by: David Wong <33114676+drwnz@users.noreply.github.com> Signed-off-by: vividf * chore: fix company name Co-authored-by: David Wong <33114676+drwnz@users.noreply.github.com> Signed-off-by: vividf * chore: fix imu to IMU Co-authored-by: David Wong <33114676+drwnz@users.noreply.github.com> Signed-off-by: vividf * chore: fix company name Co-authored-by: David Wong <33114676+drwnz@users.noreply.github.com> Signed-off-by: vividf * chore: remove old naming Co-authored-by: David Wong <33114676+drwnz@users.noreply.github.com> Signed-off-by: vividf * chore: change boolean variable naming Co-authored-by: David Wong <33114676+drwnz@users.noreply.github.com> Signed-off-by: vividf * chore: change boolean variable naming Co-authored-by: David Wong <33114676+drwnz@users.noreply.github.com> Signed-off-by: vividf * chore: change boolean variable naming Co-authored-by: David Wong <33114676+drwnz@users.noreply.github.com> Signed-off-by: vividf * fix: fix file name and variable name Signed-off-by: vividf * chore: fix invlaid virtual function definitions Signed-off-by: vividf * fix: add sophus in package dependency Signed-off-by: vividf * chore: remove brackets Signed-off-by: vividf * chore: fix algorithm Signed-off-by: vividf * chore: remove timestamp_field_name and add default parameter for 3d distortion Signed-off-by: vividf * chore: fix known limits explanation Signed-off-by: vividf * feat: add parameter schema and launch file for distortion correction node Signed-off-by: vividf * chore: fix function name Signed-off-by: vividf * chore: fix IMU function name Signed-off-by: vividf * chore: fix twist and imu iterator function name Signed-off-by: vividf * fix: add inline for undistortPointcloud function Signed-off-by: vividf * chore: move varialbe to const Signed-off-by: vividf * chore: fix grammar error Signed-off-by: vividf * fix: fix inline function Signed-off-by: vividf * chore: solve conflicts Signed-off-by: vividf * fix: fix bug in previous code Signed-off-by: vividf * fix: fix the template naming Signed-off-by: vividf * fix: fix the component test Signed-off-by: vividf * fix: solve conflicts Signed-off-by: vividf --------- Signed-off-by: vividf Co-authored-by: David Wong <33114676+drwnz@users.noreply.github.com> --- .../pointcloud_preprocessor/CMakeLists.txt | 4 + .../distortion_corrector_node.param.yaml | 5 + .../docs/distortion-corrector.md | 43 +- .../distortion_corrector.hpp | 142 +++-- .../distortion_corrector_node.hpp | 65 +++ .../distortion_corrector_node.launch.xml | 16 + sensing/pointcloud_preprocessor/package.xml | 1 + .../distortion_corrector_node.schema.json | 40 ++ .../distortion_corrector.cpp | 515 ++++++++++-------- .../distortion_corrector_node.cpp | 124 +++++ .../test/test_distortion_corrector.py | 2 +- ...test_distortion_corrector_use_imu_false.py | 6 +- 12 files changed, 694 insertions(+), 269 deletions(-) create mode 100644 sensing/pointcloud_preprocessor/config/distortion_corrector_node.param.yaml create mode 100644 sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector_node.hpp create mode 100644 sensing/pointcloud_preprocessor/launch/distortion_corrector_node.launch.xml create mode 100644 sensing/pointcloud_preprocessor/schema/distortion_corrector_node.schema.json create mode 100644 sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp diff --git a/sensing/pointcloud_preprocessor/CMakeLists.txt b/sensing/pointcloud_preprocessor/CMakeLists.txt index df62f538e9964..dd73f28a41f3c 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} ) @@ -76,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_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 @@ -86,6 +89,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/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 dcb03cc792cae..96124f473f825 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 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 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 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 offset equation is given by -$ TimeOffset = (55.296 \mu s _SequenceIndex) + (2.304 \mu s_ DataPointIndex) $ +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. -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,31 @@ $ 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/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` | filtered points | +| Name | Type | Description | +| --------------------- | ------------------------------- | ----------------------------------- | +| `~/output/pointcloud` | `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 | +{{ json_to_markdown("sensing/pointcloud_preprocessor/schema/distortion_corrector.schema.json") }} + +## Launch + +```bash +ros2 launch pointcloud_preprocessor distortion_corrector.launch.xml +``` ## Assumptions / Known limits + +- 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. 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 80abba9b5d1a5..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 @@ -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,7 +15,9 @@ #ifndef POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_HPP_ #define POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_HPP_ +#include #include +#include #include #include @@ -33,54 +35,132 @@ #endif #include +#include #include -// Include tier4 autoware utils -#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: - explicit DistortionCorrectorComponent(const rclcpp::NodeOptions & options); + virtual void processTwistMessage( + const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) = 0; + virtual void processIMUMessage( + const std::string & base_frame, const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) = 0; + virtual void setPointCloudTransform( + const std::string & base_frame, const std::string & lidar_frame) = 0; + virtual void initialize() = 0; + virtual void undistortPointCloud(bool use_imu, sensor_msgs::msg::PointCloud2 & pointcloud) = 0; +}; -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); +template +class DistortionCorrector : public DistortionCorrectorBase +{ +public: + bool pointcloud_transform_needed_{false}; + bool pointcloud_transform_exists_{false}; + bool imu_transform_exists_{false}; + rclcpp::Node * node_; + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; - bool undistortPointCloud(const tf2::Transform & tf2_base_link_to_sensor, PointCloud2 & points); + 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_frame, const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) override; + void getIMUTransformation( + const std::string & base_frame, const std::string & imu_frame, + geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr); + void enqueueIMU( + 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 getTwistAndIMUIterator( + 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 warnIfTimestampIsTooLate(bool is_twist_time_stamp_too_late, bool is_imu_time_stamp_too_late); + void undistortPoint( + sensor_msgs::PointCloud2Iterator & it_x, sensor_msgs::PointCloud2Iterator & it_y, + sensor_msgs::PointCloud2Iterator & it_z, + std::deque::iterator & it_twist, + std::deque::iterator & it_imu, float const & time_offset, + const bool & is_twist_valid, const bool & is_imu_valid) + { + static_cast(this)->undistortPointImplementation( + it_x, it_y, it_z, it_twist, it_imu, time_offset, is_twist_valid, is_imu_valid); + }; +}; - rclcpp::Subscription::SharedPtr input_points_sub_; - rclcpp::Subscription::SharedPtr imu_sub_; - rclcpp::Subscription::SharedPtr twist_sub_; - rclcpp::Publisher::SharedPtr undistorted_points_pub_; +class DistortionCorrector2D : public DistortionCorrector +{ +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_; - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr debug_publisher_; +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, 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; +}; - tf2_ros::Buffer tf2_buffer_{get_clock()}; - tf2_ros::TransformListener tf2_listener_{tf2_buffer_}; +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_; - std::deque twist_queue_; - std::deque angular_velocity_queue_; + // TF + Eigen::Matrix4f eigen_lidar_to_base_link_; + Eigen::Matrix4f eigen_base_link_to_lidar_; - std::string base_link_frame_ = "base_link"; - std::string time_stamp_field_name_; - bool use_imu_; +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, const float & time_offset, + const bool & is_twist_valid, const bool & is_imu_valid); + void setPointCloudTransform( + const std::string & base_frame, const std::string & lidar_frame) override; }; } // namespace 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 new file mode 100644 index 0000000000000..162170b193827 --- /dev/null +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector_node.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_NODE_HPP_ +#define POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_NODE_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 twist_sub_; + rclcpp::Subscription::SharedPtr imu_sub_; + rclcpp::Subscription::SharedPtr pointcloud_sub_; + + rclcpp::Publisher::SharedPtr undistorted_pointcloud_pub_; + + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; + + std::string base_frame_; + bool use_imu_; + bool use_3d_distortion_correction_; + + std::unique_ptr distortion_corrector_; + + void onPointCloud(PointCloud2::UniquePtr points_msg); + void onTwist(const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg); + void onImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg); +}; + +} // namespace pointcloud_preprocessor + +#endif // POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_NODE_HPP_ 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..d970aae102264 --- /dev/null +++ b/sensing/pointcloud_preprocessor/launch/distortion_corrector_node.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/sensing/pointcloud_preprocessor/package.xml b/sensing/pointcloud_preprocessor/package.xml index c4398ab8d01ac..3eea0f3e2eb9a 100644 --- a/sensing/pointcloud_preprocessor/package.xml +++ b/sensing/pointcloud_preprocessor/package.xml @@ -42,6 +42,7 @@ rclcpp rclcpp_components sensor_msgs + sophus std_msgs tf2 tf2_eigen 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 8bf511637371a..81c29a9f6202a 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. @@ -16,51 +16,12 @@ #include "autoware/universe_utils/math/trigonometry.hpp" -#include -#include -#include +#include namespace pointcloud_preprocessor { -/** @brief Constructor. */ -DistortionCorrectorComponent::DistortionCorrectorComponent(const rclcpp::NodeOptions & options) -: Node("distortion_corrector_node", options) -{ - // initialize debug tool - { - 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"); - stop_watch_ptr_->tic("processing_time"); - } - - // Parameter - time_stamp_field_name_ = declare_parameter("time_stamp_field_name", "time_stamp"); - use_imu_ = declare_parameter("use_imu", true); - - { - rclcpp::PublisherOptions pub_options; - pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); - // Publisher - undistorted_points_pub_ = this->create_publisher( - "~/output/pointcloud", rclcpp::SensorDataQoS(), pub_options); - } - // 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)); -} - -void DistortionCorrectorComponent::onTwistWithCovarianceStamped( +template +void DistortionCorrector::processTwistMessage( const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) { geometry_msgs::msg::TwistStamped msg; @@ -76,28 +37,66 @@ void DistortionCorrectorComponent::onTwistWithCovarianceStamped( 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; } } -void DistortionCorrectorComponent::onImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) +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 = + std::make_shared(); + getIMUTransformation(base_frame, imu_msg->header.frame_id, geometry_imu_to_base_link_ptr); + enqueueIMU(imu_msg, geometry_imu_to_base_link_ptr); +} + +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) { - if (!use_imu_) { + if (imu_transform_exists_) { 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()); + tf2::Transform tf2_imu_to_base_link; + 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_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_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::enqueueIMU( + 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, *tf_base2imu_ptr); + 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); @@ -111,226 +110,310 @@ void DistortionCorrectorComponent::onImu(const sensor_msgs::msg::Imu::ConstShare 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; - } -} - -void DistortionCorrectorComponent::onPointCloud(PointCloud2::UniquePtr points_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; - } - - tf2::Transform tf2_base_link_to_sensor{}; - getTransform(points_msg->header.frame_id, base_link_frame_, &tf2_base_link_to_sensor); - - undistortPointCloud(tf2_base_link_to_sensor, *points_msg); - - if (debug_publisher_) { - auto pipeline_latency_ms = - std::chrono::duration( - std::chrono::nanoseconds( - (this->get_clock()->now() - points_msg->header.stamp).nanoseconds())) - .count(); - debug_publisher_->publish( - "debug/pipeline_latency_ms", pipeline_latency_ms); - } - - undistorted_points_pub_->publish(std::move(points_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); } } -bool DistortionCorrectorComponent::getTransform( - const std::string & target_frame, const std::string & source_frame, - tf2::Transform * tf2_transform_ptr) +template +void DistortionCorrector::getTwistAndIMUIterator( + bool use_imu, double first_point_time_stamp_sec, + std::deque::iterator & it_twist, + std::deque::iterator & it_imu) { - 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()); + 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; - 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 false; + 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; } - return true; } -bool DistortionCorrectorComponent::undistortPointCloud( - const tf2::Transform & tf2_base_link_to_sensor, PointCloud2 & points) +template +bool DistortionCorrector::isInputValid(sensor_msgs::msg::PointCloud2 & pointcloud) { - if (points.data.empty() || twist_queue_.empty()) { + if (pointcloud.data.empty() || twist_queue_.empty()) { RCLCPP_WARN_STREAM_THROTTLE( - get_logger(), *get_clock(), 10000 /* ms */, - "input_pointcloud->points or twist_queue_ is empty."); + 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(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()) { + 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 */, + 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 DistortionCorrector::undistortPointCloud( + bool use_imu, sensor_msgs::msg::PointCloud2 & pointcloud) +{ + if (!isInputValid(pointcloud)) return; - 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_); + 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"); - 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}; - auto twist_it = 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; - - decltype(angular_velocity_queue_)::iterator imu_it; - if (use_imu_ && !angular_velocity_queue_.empty()) { - imu_it = 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; - } - - const tf2::Transform tf2_base_link_to_sensor_inv{tf2_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(); - - // 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_; + std::deque::iterator it_twist; + std::deque::iterator 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(); double imu_stamp{0.0}; - if (use_imu_ && !angular_velocity_queue_.empty()) { - imu_stamp = rclcpp::Time(imu_it->header.stamp).seconds(); + if (use_imu && !angular_velocity_queue_.empty()) { + 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; + // 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_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) { - 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)}; + 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) { - twist_time_stamp_is_too_late = true; - v = 0.0f; - w = 0.0f; + is_twist_time_stamp_too_late = true; + is_twist_valid = false; } - 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(); + // 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) { - imu_time_stamp_is_too_late = true; - } else { - w = static_cast(imu_it->vector.z); + is_imu_time_stamp_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); + float time_offset = static_cast(*it_time_stamp - prev_time_stamp_sec); - point.setValue(*it_x, *it_y, *it_z); + // 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); - if (need_transform) { - point = tf2_base_link_to_sensor_inv * point; - } + prev_time_stamp_sec = *it_time_stamp; + } - theta += w * time_offset; - baselink_quat.setValue( - 0, 0, autoware::universe_utils::sin(theta * 0.5f), - autoware::universe_utils::cos(theta * 0.5f)); // baselink_quat.setRPY(0.0, 0.0, theta); - const float dis = v * time_offset; - x += dis * autoware::universe_utils::cos(theta); - y += dis * autoware::universe_utils::sin(theta); + warnIfTimestampIsTooLate(is_twist_time_stamp_too_late, is_imu_time_stamp_too_late); +} - baselink_tf_odom.setOrigin(tf2::Vector3(x, y, 0.0)); - baselink_tf_odom.setRotation(baselink_quat); +template +void DistortionCorrector::warnIfTimestampIsTooLate( + bool is_twist_time_stamp_too_late, bool is_imu_time_stamp_too_late) +{ + if (is_twist_time_stamp_too_late) { + RCLCPP_WARN_STREAM_THROTTLE( + node_->get_logger(), *node_->get_clock(), 10000 /* ms */, + "Twist time_stamp is too late. Could not interpolate."); + } - undistorted_point = baselink_tf_odom * point; + 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."); + } +} - if (need_transform) { - undistorted_point = tf2_base_link_to_sensor * undistorted_point; +///////////////////////// 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_frame, const std::string & lidar_frame) +{ + if (pointcloud_transform_exists_) { + return; + } + + 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_; + pointcloud_transform_exists_ = true; + } else { + try { + const auto transform_msg = + 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; + 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_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 DistortionCorrector3D::setPointCloudTransform( + const std::string & base_frame, const std::string & lidar_frame) +{ + if (pointcloud_transform_exists_) { + return; + } - *it_x = static_cast(undistorted_point.getX()); - *it_y = static_cast(undistorted_point.getY()); - *it_z = static_cast(undistorted_point.getZ()); + 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; + } - prev_time_stamp_sec = *it_time_stamp; + try { + const auto transform_msg = + 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(); + pointcloud_transform_exists_ = true; + 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_frame.c_str(), lidar_frame.c_str()); + eigen_lidar_to_base_link_ = Eigen::Matrix4f::Identity(); + eigen_base_link_to_lidar_ = Eigen::Matrix4f::Identity(); } +} - 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."); +inline 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, 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}; + 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); } - 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."); + // Undistort point + point_tf_.setValue(*it_x, *it_y, *it_z); + + if (pointcloud_transform_needed_) { + point_tf_ = tf2_lidar_to_base_link_ * point_tf_; } + theta_ += w * time_offset; + baselink_quat_.setValue( + 0, 0, autoware::universe_utils::sin(theta_ * 0.5f), + autoware::universe_utils::cos(theta_ * 0.5f)); // baselink_quat.setRPY(0.0, 0.0, theta); + const float dis = v * time_offset; + x_ += dis * autoware::universe_utils::cos(theta_); + y_ += dis * autoware::universe_utils::sin(theta_); - return true; + 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 (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()); } -} // namespace pointcloud_preprocessor +inline 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, 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}; + 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); + } -#include -RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_preprocessor::DistortionCorrectorComponent) + // Undistort point + point_eigen_ << *it_x, *it_y, *it_z, 1.0; + if (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 (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_node.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp new file mode 100644 index 0000000000000..b5cf581301a23 --- /dev/null +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp @@ -0,0 +1,124 @@ +// 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_node.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 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"); + stop_watch_ptr_->tic("processing_time"); + + // Parameter + base_frame_ = declare_parameter("base_frame"); + use_imu_ = declare_parameter("use_imu"); + use_3d_distortion_correction_ = declare_parameter("use_3d_distortion_correction"); + + // Publisher + { + 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( + "~/input/twist", 10, + std::bind(&DistortionCorrectorComponent::onTwist, this, std::placeholders::_1)); + imu_sub_ = this->create_subscription( + "~/input/imu", 10, + std::bind(&DistortionCorrectorComponent::onImu, this, std::placeholders::_1)); + 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_) { + distortion_corrector_ = std::make_unique(this); + } else { + distortion_corrector_ = std::make_unique(this); + } +} + +void DistortionCorrectorComponent::onTwist( + const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) +{ + distortion_corrector_->processTwistMessage(twist_msg); +} + +void DistortionCorrectorComponent::onImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) +{ + if (!use_imu_) { + return; + } + + 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_pointcloud_pub_->get_subscription_count() + + undistorted_pointcloud_pub_->get_intra_process_subscription_count(); + + if (points_sub_count < 1) { + return; + } + + distortion_corrector_->setPointCloudTransform(base_frame_, pointcloud_msg->header.frame_id); + + distortion_corrector_->initialize(); + distortion_corrector_->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_pointcloud_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/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"),