Autoware.Auto
point_cloud_filter_transform_node.hpp
Go to the documentation of this file.
1 // Copyright 2017-2020 the Autoware Foundation
2 //
3 // Licensed under the Apache License, Version 2.0 (the "License");
4 // you may not use this file except in compliance with the License.
5 // You may obtain a copy of the License at
6 //
7 // http://www.apache.org/licenses/LICENSE-2.0
8 //
9 // Unless required by applicable law or agreed to in writing, software
10 // distributed under the License is distributed on an "AS IS" BASIS,
11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 // See the License for the specific language governing permissions and
13 // limitations under the License.
14 //
15 // Co-developed by Tier IV, Inc. and Apex.AI, Inc.
16 
17 #ifndef POINT_CLOUD_FILTER_TRANSFORM_NODES__POINT_CLOUD_FILTER_TRANSFORM_NODE_HPP_
18 #define POINT_CLOUD_FILTER_TRANSFORM_NODES__POINT_CLOUD_FILTER_TRANSFORM_NODE_HPP_
19 
21 #include <rclcpp/rclcpp.hpp>
23 #include <sensor_msgs/msg/point_cloud2.hpp>
24 
25 #include <memory>
26 #include <string>
27 
28 #include "tf2_ros/buffer.h"
29 #include "tf2_ros/transform_listener.h"
30 
31 namespace autoware
32 {
33 namespace perception
34 {
35 namespace filters
36 {
38 namespace point_cloud_filter_transform_nodes
39 {
40 
47 using std::placeholders::_1;
48 
50  const std::string & input_frame_id,
51  const std::string & output_frame_id,
52  float64_t r_x, float64_t r_y, float64_t r_z, float64_t r_w, float64_t t_x,
53  float64_t t_y, float64_t t_z);
54 
57 class POINT_CLOUD_FILTER_TRANSFORM_NODES_PUBLIC PointCloud2FilterTransformNode
58  : public rclcpp::Node
59 {
60 public:
63  explicit PointCloud2FilterTransformNode(const rclcpp::NodeOptions & node_options);
64 
65 protected:
70  const PointCloud2 & filter_and_transform(const PointCloud2 & msg);
71 
73  void process_filtered_transformed_message(
74  const PointCloud2::SharedPtr msg);
75 
76  template<typename PointType>
81  bool8_t point_not_filtered(const PointType & pt) const
82  {
83  return m_angle_filter(pt) && m_distance_filter(pt);
84  }
85 
86  template<typename PointType>
91  PointType transform_point(const PointType & pt) const
92  {
93  PointType pt_final;
94  m_static_transformer->transform(pt, pt_final);
95  return pt_final;
96  }
97 
98 private:
100  using DistanceFilter = autoware::common::lidar_utils::DistanceFilter;
101  using StaticTransformer = autoware::common::lidar_utils::StaticTransformer;
102  AngleFilter m_angle_filter;
103  DistanceFilter m_distance_filter;
104  const std::string m_input_frame_id;
105  const std::string m_output_frame_id;
106  std::unique_ptr<StaticTransformer> m_static_transformer;
107  const std::chrono::nanoseconds m_init_timeout;
108  const std::chrono::nanoseconds m_timeout;
109  const typename rclcpp::Subscription<PointCloud2>::SharedPtr m_sub_ptr;
110  const typename std::shared_ptr<rclcpp::Publisher<PointCloud2>> m_pub_ptr;
111  const size_t m_expected_num_publishers;
112  const size_t m_expected_num_subscribers;
113  const std::uint32_t m_pcl_size;
114  PointCloud2 m_filtered_transformed_msg;
115 };
116 
117 } // namespace point_cloud_filter_transform_nodes
118 } // namespace filters
119 } // namespace perception
120 } // namespace autoware
121 
122 #endif // POINT_CLOUD_FILTER_TRANSFORM_NODES__POINT_CLOUD_FILTER_TRANSFORM_NODE_HPP_
autoware::common::lidar_utils::StaticTransformer
Transform to apply a constant transform to given points.
Definition: point_cloud_utils.hpp:237
point_cloud_utils.hpp
This class defines common functions and classes to work with pointclouds.
autoware::perception::filters::point_cloud_filter_transform_nodes::get_transform
TransformStamped get_transform(const std::string &input_frame_id, const std::string &output_frame_id, float64_t r_x, float64_t r_y, float64_t r_z, float64_t r_w, float64_t t_x, float64_t t_y, float64_t t_z)
Definition: point_cloud_filter_transform_node.cpp:44
autoware::common::lidar_utils::AngleFilter
Filter class to check if a point's azimuth lies within a range defined by a start and end angles....
Definition: point_cloud_utils.hpp:270
autoware::common::lidar_utils::DistanceFilter
Filter class to check if a point lies within a range defined by a min and max radius.
Definition: point_cloud_utils.hpp:208
autoware::common::types::bool8_t
bool bool8_t
Definition: types.hpp:39
autoware
This file defines the lanelet2_map_provider_node class.
Definition: quick_sort.hpp:24
autoware::perception::filters::point_cloud_filter_transform_nodes::PointCloud2FilterTransformNode::transform_point
PointType transform_point(const PointType &pt) const
Apply static transform to the given point.
Definition: point_cloud_filter_transform_node.hpp:91
autoware::perception::filters::point_cloud_filter_transform_nodes::PointCloud2FilterTransformNode
Base class to subscribe to raw point cloud and transform and filter it to publish filtered point clou...
Definition: point_cloud_filter_transform_node.hpp:57
motion::planning::recordreplay_planner_nodes::Transform
geometry_msgs::msg::TransformStamped Transform
Definition: recordreplay_planner_node.hpp:56
motion::motion_testing_nodes::TransformStamped
geometry_msgs::msg::TransformStamped TransformStamped
Definition: motion_testing_publisher.hpp:37
autoware::common::types::float32_t
float float32_t
Definition: types.hpp:45
autoware::common::types::float64_t
double float64_t
Definition: types.hpp:47
visibility_control.hpp
autoware::perception::filters::filter_node_base::PointCloud2
sensor_msgs::msg::PointCloud2 PointCloud2
Definition: filter_node_base.cpp:32
autoware::perception::filters::point_cloud_filter_transform_nodes::PointCloud2FilterTransformNode::point_not_filtered
bool8_t point_not_filtered(const PointType &pt) const
Check if the point is within the specified angle and radius limits.
Definition: point_cloud_filter_transform_node.hpp:81