Branch data     Line data    Source code

       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                 :            : 
      20                 :            : #include <point_cloud_filter_transform_nodes/visibility_control.hpp>
      21                 :            : #include <rclcpp/rclcpp.hpp>
      22                 :            : #include <lidar_utils/point_cloud_utils.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                 :            : {
      37                 :            : /// \brief Boilerplate Apex.OS nodes around point_cloud_filter_transform_nodes
      38                 :            : namespace point_cloud_filter_transform_nodes
      39                 :            : {
      40                 :            : 
      41                 :            : using autoware::common::types::bool8_t;
      42                 :            : using autoware::common::types::float32_t;
      43                 :            : using autoware::common::types::float64_t;
      44                 :            : using geometry_msgs::msg::Transform;
      45                 :            : using geometry_msgs::msg::TransformStamped;
      46                 :            : using sensor_msgs::msg::PointCloud2;
      47                 :            : using std::placeholders::_1;
      48                 :            : 
      49                 :            : TransformStamped get_transform(
      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                 :            : 
      55                 :            : /// \brief Base class to subscribe to raw point cloud and transform and filter it to publish
      56                 :            : ///        filtered point cloud. Calls angle filter, distance filter and static transformer.
      57                 :            : class POINT_CLOUD_FILTER_TRANSFORM_NODES_PUBLIC PointCloud2FilterTransformNode
      58                 :            :   : public rclcpp::Node
      59                 :            : {
      60                 :            : public:
      61                 :            :   /// \brief Parameter constructor
      62                 :            :   /// \param node_options Additional options to control creation of the node.
      63                 :            :   explicit PointCloud2FilterTransformNode(const rclcpp::NodeOptions & node_options);
      64                 :            : 
      65                 :            : protected:
      66                 :            :   /// \brief Call distance & angle filter and then static transformer for all the points
      67                 :            :   /// \param msg Raw point cloud
      68                 :            :   /// \return Filtered and Transformed point cloud.
      69                 :            :   /// \throws std::runtime_error on unexpected input contents or not enough output capacity
      70                 :            :   const PointCloud2 & filter_and_transform(const PointCloud2 & msg);
      71                 :            : 
      72                 :            :   /// \brief Run main subscribe -> filter & transform -> publish loop
      73                 :            :   void process_filtered_transformed_message(
      74                 :            :     const PointCloud2::SharedPtr msg);
      75                 :            : 
      76                 :            :   template<typename PointType>
      77                 :            :   /// \brief Check if the point is within the specified angle and radius limits
      78                 :            :   /// \tparam PointType type with x, y, z.
      79                 :            :   /// \param pt point with x, y, z
      80                 :            :   /// \return True if the point is within the desired radius and angle limits. False otherwise.
      81                 :    9011720 :   bool8_t point_not_filtered(const PointType & pt) const
      82                 :            :   {
      83   [ +  +  +  + ]:    9011720 :     return m_angle_filter(pt) && m_distance_filter(pt);
      84                 :            :   }
      85                 :            : 
      86                 :            :   template<typename PointType>
      87                 :            :   /// \brief Apply static transform to the given point
      88                 :            :   /// \tparam PointType type with x, y, z
      89                 :            :   /// \param pt point with x, y, z
      90                 :            :   /// \return Transformed point
      91                 :            :   PointType transform_point(const PointType & pt) const
      92                 :            :   {
      93                 :    7175690 :     PointType pt_final;
      94                 :    7175690 :     m_static_transformer->transform(pt, pt_final);
      95                 :    7175690 :     return pt_final;
      96                 :            :   }
      97                 :            : 
      98                 :            : private:
      99                 :            :   using AngleFilter = autoware::common::lidar_utils::AngleFilter;
     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_