|
1 | | -// Copyright 2020 Tier IV, Inc. |
| 1 | +// Copyright 2024 TIER IV, Inc. |
2 | 2 | // |
3 | 3 | // Licensed under the Apache License, Version 2.0 (the "License"); |
4 | 4 | // you may not use this file except in compliance with the License. |
|
15 | 15 | #ifndef POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_HPP_ |
16 | 16 | #define POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_HPP_ |
17 | 17 |
|
| 18 | +#include <Eigen/Core> |
18 | 19 | #include <rclcpp/rclcpp.hpp> |
| 20 | +#include <sophus/se3.hpp> |
19 | 21 |
|
20 | 22 | #include <geometry_msgs/msg/twist_stamped.hpp> |
21 | 23 | #include <geometry_msgs/msg/twist_with_covariance_stamped.hpp> |
|
33 | 35 | #endif |
34 | 36 |
|
35 | 37 | #include <tf2_ros/buffer.h> |
| 38 | +#include <tf2_ros/buffer_interface.h> |
36 | 39 | #include <tf2_ros/transform_listener.h> |
37 | 40 |
|
38 | | -// Include tier4 autoware utils |
39 | | -#include <autoware/universe_utils/ros/debug_publisher.hpp> |
40 | | -#include <autoware/universe_utils/system/stop_watch.hpp> |
41 | | - |
42 | 41 | #include <deque> |
43 | 42 | #include <memory> |
44 | 43 | #include <string> |
45 | 44 |
|
46 | 45 | namespace pointcloud_preprocessor |
47 | 46 | { |
48 | | -using rcl_interfaces::msg::SetParametersResult; |
49 | | -using sensor_msgs::msg::PointCloud2; |
50 | 47 |
|
51 | | -class DistortionCorrectorComponent : public rclcpp::Node |
| 48 | +class DistortionCorrectorBase |
52 | 49 | { |
53 | 50 | public: |
54 | | - explicit DistortionCorrectorComponent(const rclcpp::NodeOptions & options); |
| 51 | + virtual void processTwistMessage( |
| 52 | + const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) = 0; |
| 53 | + virtual void processIMUMessage( |
| 54 | + const std::string & base_frame, const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) = 0; |
| 55 | + virtual void setPointCloudTransform( |
| 56 | + const std::string & base_frame, const std::string & lidar_frame) = 0; |
| 57 | + virtual void initialize() = 0; |
| 58 | + virtual void undistortPointCloud(bool use_imu, sensor_msgs::msg::PointCloud2 & pointcloud) = 0; |
| 59 | +}; |
55 | 60 |
|
56 | | -private: |
57 | | - void onPointCloud(PointCloud2::UniquePtr points_msg); |
58 | | - void onTwistWithCovarianceStamped( |
59 | | - const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg); |
60 | | - void onImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg); |
61 | | - bool getTransform( |
62 | | - const std::string & target_frame, const std::string & source_frame, |
63 | | - tf2::Transform * tf2_transform_ptr); |
| 61 | +template <class T> |
| 62 | +class DistortionCorrector : public DistortionCorrectorBase |
| 63 | +{ |
| 64 | +public: |
| 65 | + bool pointcloud_transform_needed_{false}; |
| 66 | + bool pointcloud_transform_exists_{false}; |
| 67 | + bool imu_transform_exists_{false}; |
| 68 | + rclcpp::Node * node_; |
| 69 | + tf2_ros::Buffer tf_buffer_; |
| 70 | + tf2_ros::TransformListener tf_listener_; |
64 | 71 |
|
65 | | - bool undistortPointCloud(const tf2::Transform & tf2_base_link_to_sensor, PointCloud2 & points); |
| 72 | + std::deque<geometry_msgs::msg::TwistStamped> twist_queue_; |
| 73 | + std::deque<geometry_msgs::msg::Vector3Stamped> angular_velocity_queue_; |
| 74 | + |
| 75 | + explicit DistortionCorrector(rclcpp::Node * node) |
| 76 | + : node_(node), tf_buffer_(node_->get_clock()), tf_listener_(tf_buffer_) |
| 77 | + { |
| 78 | + } |
| 79 | + void processTwistMessage( |
| 80 | + const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) override; |
| 81 | + |
| 82 | + void processIMUMessage( |
| 83 | + const std::string & base_frame, const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) override; |
| 84 | + void getIMUTransformation( |
| 85 | + const std::string & base_frame, const std::string & imu_frame, |
| 86 | + geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr); |
| 87 | + void enqueueIMU( |
| 88 | + const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg, |
| 89 | + geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr); |
| 90 | + |
| 91 | + bool isInputValid(sensor_msgs::msg::PointCloud2 & pointcloud); |
| 92 | + void getTwistAndIMUIterator( |
| 93 | + bool use_imu, double first_point_time_stamp_sec, |
| 94 | + std::deque<geometry_msgs::msg::TwistStamped>::iterator & it_twist, |
| 95 | + std::deque<geometry_msgs::msg::Vector3Stamped>::iterator & it_imu); |
| 96 | + void undistortPointCloud(bool use_imu, sensor_msgs::msg::PointCloud2 & pointcloud) override; |
| 97 | + void warnIfTimestampIsTooLate(bool is_twist_time_stamp_too_late, bool is_imu_time_stamp_too_late); |
| 98 | + void undistortPoint( |
| 99 | + sensor_msgs::PointCloud2Iterator<float> & it_x, sensor_msgs::PointCloud2Iterator<float> & it_y, |
| 100 | + sensor_msgs::PointCloud2Iterator<float> & it_z, |
| 101 | + std::deque<geometry_msgs::msg::TwistStamped>::iterator & it_twist, |
| 102 | + std::deque<geometry_msgs::msg::Vector3Stamped>::iterator & it_imu, float const & time_offset, |
| 103 | + const bool & is_twist_valid, const bool & is_imu_valid) |
| 104 | + { |
| 105 | + static_cast<T *>(this)->undistortPointImplementation( |
| 106 | + it_x, it_y, it_z, it_twist, it_imu, time_offset, is_twist_valid, is_imu_valid); |
| 107 | + }; |
| 108 | +}; |
66 | 109 |
|
67 | | - rclcpp::Subscription<PointCloud2>::SharedPtr input_points_sub_; |
68 | | - rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_; |
69 | | - rclcpp::Subscription<geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr twist_sub_; |
70 | | - rclcpp::Publisher<PointCloud2>::SharedPtr undistorted_points_pub_; |
| 110 | +class DistortionCorrector2D : public DistortionCorrector<DistortionCorrector2D> |
| 111 | +{ |
| 112 | +private: |
| 113 | + // defined outside of for loop for performance reasons. |
| 114 | + tf2::Quaternion baselink_quat_; |
| 115 | + tf2::Transform baselink_tf_odom_; |
| 116 | + tf2::Vector3 point_tf_; |
| 117 | + tf2::Vector3 undistorted_point_tf_; |
| 118 | + float theta_; |
| 119 | + float x_; |
| 120 | + float y_; |
| 121 | + |
| 122 | + // TF |
| 123 | + tf2::Transform tf2_lidar_to_base_link_; |
| 124 | + tf2::Transform tf2_base_link_to_lidar_; |
71 | 125 |
|
72 | | - std::unique_ptr<autoware::universe_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_; |
73 | | - std::unique_ptr<autoware::universe_utils::DebugPublisher> debug_publisher_; |
| 126 | +public: |
| 127 | + explicit DistortionCorrector2D(rclcpp::Node * node) : DistortionCorrector(node) {} |
| 128 | + void initialize() override; |
| 129 | + void undistortPointImplementation( |
| 130 | + sensor_msgs::PointCloud2Iterator<float> & it_x, sensor_msgs::PointCloud2Iterator<float> & it_y, |
| 131 | + sensor_msgs::PointCloud2Iterator<float> & it_z, |
| 132 | + std::deque<geometry_msgs::msg::TwistStamped>::iterator & it_twist, |
| 133 | + std::deque<geometry_msgs::msg::Vector3Stamped>::iterator & it_imu, const float & time_offset, |
| 134 | + const bool & is_twist_valid, const bool & is_imu_valid); |
| 135 | + |
| 136 | + void setPointCloudTransform( |
| 137 | + const std::string & base_frame, const std::string & lidar_frame) override; |
| 138 | +}; |
74 | 139 |
|
75 | | - tf2_ros::Buffer tf2_buffer_{get_clock()}; |
76 | | - tf2_ros::TransformListener tf2_listener_{tf2_buffer_}; |
| 140 | +class DistortionCorrector3D : public DistortionCorrector<DistortionCorrector3D> |
| 141 | +{ |
| 142 | +private: |
| 143 | + // defined outside of for loop for performance reasons. |
| 144 | + Eigen::Vector4f point_eigen_; |
| 145 | + Eigen::Vector4f undistorted_point_eigen_; |
| 146 | + Eigen::Matrix4f transformation_matrix_; |
| 147 | + Eigen::Matrix4f prev_transformation_matrix_; |
77 | 148 |
|
78 | | - std::deque<geometry_msgs::msg::TwistStamped> twist_queue_; |
79 | | - std::deque<geometry_msgs::msg::Vector3Stamped> angular_velocity_queue_; |
| 149 | + // TF |
| 150 | + Eigen::Matrix4f eigen_lidar_to_base_link_; |
| 151 | + Eigen::Matrix4f eigen_base_link_to_lidar_; |
80 | 152 |
|
81 | | - std::string base_link_frame_ = "base_link"; |
82 | | - std::string time_stamp_field_name_; |
83 | | - bool use_imu_; |
| 153 | +public: |
| 154 | + explicit DistortionCorrector3D(rclcpp::Node * node) : DistortionCorrector(node) {} |
| 155 | + void initialize() override; |
| 156 | + void undistortPointImplementation( |
| 157 | + sensor_msgs::PointCloud2Iterator<float> & it_x, sensor_msgs::PointCloud2Iterator<float> & it_y, |
| 158 | + sensor_msgs::PointCloud2Iterator<float> & it_z, |
| 159 | + std::deque<geometry_msgs::msg::TwistStamped>::iterator & it_twist, |
| 160 | + std::deque<geometry_msgs::msg::Vector3Stamped>::iterator & it_imu, const float & time_offset, |
| 161 | + const bool & is_twist_valid, const bool & is_imu_valid); |
| 162 | + void setPointCloudTransform( |
| 163 | + const std::string & base_frame, const std::string & lidar_frame) override; |
84 | 164 | }; |
85 | 165 |
|
86 | 166 | } // namespace pointcloud_preprocessor |
|
0 commit comments