|
| 1 | +// Copyright(c) 2025 AutoCore Technology (Nanjing) Co., Ltd. All rights reserved. |
| 2 | +// |
| 3 | +// Copyright 2025 TIER IV, Inc. |
| 4 | +// |
| 5 | +// Licensed under the Apache License, Version 2.0 (the "License"); |
| 6 | +// you may not use this file except in compliance with the License. |
| 7 | +// You may obtain a copy of the License at |
| 8 | +// |
| 9 | +// http://www.apache.org/licenses/LICENSE-2.0 |
| 10 | +// |
| 11 | +// Unless required by applicable law or agreed to in writing, software |
| 12 | +// distributed under the License is distributed on an "AS IS" BASIS, |
| 13 | +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| 14 | +// See the License for the specific language governing permissions and |
| 15 | +// limitations under the License. |
| 16 | + |
| 17 | +#ifndef AUTOWARE__CROP_BOX_FILTER__CROP_BOX_FILTER_NODE_HPP_ |
| 18 | +#define AUTOWARE__CROP_BOX_FILTER__CROP_BOX_FILTER_NODE_HPP_ |
| 19 | + |
| 20 | +#include <autoware/point_types/types.hpp> |
| 21 | +#include <autoware_utils/ros/debug_publisher.hpp> |
| 22 | +#include <autoware_utils/ros/managed_transform_buffer.hpp> |
| 23 | +#include <autoware_utils/ros/published_time_publisher.hpp> |
| 24 | +#include <autoware_utils/system/stop_watch.hpp> |
| 25 | + |
| 26 | +#include <geometry_msgs/msg/polygon_stamped.hpp> |
| 27 | +#include <sensor_msgs/msg/point_cloud2.hpp> |
| 28 | +#include <sensor_msgs/point_cloud2_iterator.hpp> |
| 29 | + |
| 30 | +#include <memory> |
| 31 | +#include <string> |
| 32 | +#include <utility> |
| 33 | +#include <vector> |
| 34 | + |
| 35 | +using PointCloud2 = sensor_msgs::msg::PointCloud2; |
| 36 | +using PointCloud2ConstPtr = sensor_msgs::msg::PointCloud2::ConstSharedPtr; |
| 37 | + |
| 38 | +using PointCloud = pcl::PointCloud<pcl::PointXYZ>; |
| 39 | +using PointCloudPtr = PointCloud::Ptr; |
| 40 | +using PointCloudConstPtr = PointCloud::ConstPtr; |
| 41 | + |
| 42 | +namespace autoware::crop_box_filter |
| 43 | +{ |
| 44 | + |
| 45 | +class CropBoxFilter : public rclcpp::Node |
| 46 | +{ |
| 47 | +private: |
| 48 | + // member variable declaration & definitions ************************************* |
| 49 | + |
| 50 | + /** \brief The managed transform buffer. */ |
| 51 | + std::unique_ptr<autoware_utils::ManagedTransformBuffer> managed_tf_buffer_{nullptr}; |
| 52 | + |
| 53 | + /** \brief The input TF frame the data should be transformed into, |
| 54 | + * if input.header.frame_id is different. */ |
| 55 | + std::string tf_input_frame_; |
| 56 | + |
| 57 | + /** \brief The original data input TF frame. */ |
| 58 | + std::string tf_input_orig_frame_; |
| 59 | + |
| 60 | + /** \brief The output TF frame the data should be transformed into, |
| 61 | + * if input.header.frame_id is different. */ |
| 62 | + std::string tf_output_frame_; |
| 63 | + |
| 64 | + /** \brief The maximum queue size (default: 3). */ |
| 65 | + size_t max_queue_size_ = 3; |
| 66 | + |
| 67 | + /** \brief Internal mutex. */ |
| 68 | + std::mutex mutex_; |
| 69 | + |
| 70 | + bool need_preprocess_transform_ = false; |
| 71 | + bool need_postprocess_transform_ = false; |
| 72 | + |
| 73 | + Eigen::Matrix4f eigen_transform_preprocess_ = Eigen::Matrix4f::Identity(4, 4); |
| 74 | + Eigen::Matrix4f eigen_transform_postprocess_ = Eigen::Matrix4f::Identity(4, 4); |
| 75 | + |
| 76 | + struct CropBoxParam |
| 77 | + { |
| 78 | + float min_x; |
| 79 | + float max_x; |
| 80 | + float min_y; |
| 81 | + float max_y; |
| 82 | + float min_z; |
| 83 | + float max_z; |
| 84 | + bool negative{false}; |
| 85 | + } param_; |
| 86 | + |
| 87 | + /** \brief Parameter service callback result : needed to be hold */ |
| 88 | + OnSetParametersCallbackHandle::SharedPtr set_param_res_; |
| 89 | + |
| 90 | + // publisher and subscriber declaration ********************* |
| 91 | + |
| 92 | + /** \brief The input PointCloud2 subscriber. */ |
| 93 | + rclcpp::Subscription<PointCloud2>::SharedPtr sub_input_; |
| 94 | + |
| 95 | + rclcpp::Publisher<PointCloud2>::SharedPtr pub_output_; |
| 96 | + rclcpp::Publisher<geometry_msgs::msg::PolygonStamped>::SharedPtr crop_box_polygon_pub_; |
| 97 | + |
| 98 | + /** \brief processing time publisher. **/ |
| 99 | + std::unique_ptr<autoware_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_; |
| 100 | + std::unique_ptr<autoware_utils::DebugPublisher> debug_publisher_; |
| 101 | + std::unique_ptr<autoware_utils::PublishedTimePublisher> published_time_publisher_; |
| 102 | + |
| 103 | + // function declaration ************************************* |
| 104 | + |
| 105 | + void publish_crop_box_polygon(); |
| 106 | + |
| 107 | + void pointcloud_callback(const PointCloud2ConstPtr cloud); |
| 108 | + |
| 109 | + /** \brief Parameter service callback */ |
| 110 | + rcl_interfaces::msg::SetParametersResult param_callback(const std::vector<rclcpp::Parameter> & p); |
| 111 | + |
| 112 | + /** \brief Return whether the input PointCloud2 data has the same layout than PointXYZI. That is |
| 113 | + * to say whether you can memcpy from the PointCloud2 data buffer to a PointXYZI */ |
| 114 | + bool is_data_layout_compatible_with_point_xyzi(const PointCloud2 & input); |
| 115 | + |
| 116 | + /** \brief Return whether the input PointCloud2 data has the same layout than PointXYZIRC. That is |
| 117 | + * to say whether you can memcpy from the PointCloud2 data buffer to a PointXYZIRC */ |
| 118 | + bool is_data_layout_compatible_with_point_xyzirc(const PointCloud2 & input); |
| 119 | + |
| 120 | + /** \brief Return whether the input PointCloud2 data has the same layout than PointXYZIRADRT. That |
| 121 | + * is to say whether you can memcpy from the PointCloud2 data buffer to a PointXYZIRADRT */ |
| 122 | + bool is_data_layout_compatible_with_point_xyziradrt(const PointCloud2 & input); |
| 123 | + |
| 124 | + /** \brief Return whether the input PointCloud2 data has the same layout than PointXYZIRCAEDT. |
| 125 | + * That is to say whether you can memcpy from the PointCloud2 data buffer to a PointXYZIRCAEDT */ |
| 126 | + bool is_data_layout_compatible_with_point_xyzircaedt(const PointCloud2 & input); |
| 127 | + |
| 128 | + bool is_valid(const PointCloud2ConstPtr & cloud); |
| 129 | + |
| 130 | + /** \brief For parameter service callback */ |
| 131 | + template <typename T> |
| 132 | + bool get_param(const std::vector<rclcpp::Parameter> & p, const std::string & name, T & value) |
| 133 | + { |
| 134 | + auto it = std::find_if(p.cbegin(), p.cend(), [&name](const rclcpp::Parameter & parameter) { |
| 135 | + return parameter.get_name() == name; |
| 136 | + }); |
| 137 | + if (it != p.cend()) { |
| 138 | + value = it->template get_value<T>(); |
| 139 | + return true; |
| 140 | + } |
| 141 | + return false; |
| 142 | + } |
| 143 | + |
| 144 | +public: |
| 145 | + PCL_MAKE_ALIGNED_OPERATOR_NEW |
| 146 | + explicit CropBoxFilter(const rclcpp::NodeOptions & options); |
| 147 | + void filter_pointcloud(const PointCloud2ConstPtr & cloud, PointCloud2 & output); |
| 148 | +}; |
| 149 | +} // namespace autoware::crop_box_filter |
| 150 | + |
| 151 | +#endif // AUTOWARE__CROP_BOX_FILTER__CROP_BOX_FILTER_NODE_HPP_ |
0 commit comments