Branch data     Line data    Source code

       1                 :            : // Copyright 2019 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 NDT__NDT_SCAN_HPP_
      18                 :            : #define NDT__NDT_SCAN_HPP_
      19                 :            : 
      20                 :            : #include <common/types.hpp>
      21                 :            : #include <helper_functions/crtp.hpp>
      22                 :            : #include <ndt/visibility_control.hpp>
      23                 :            : #include <point_cloud_msg_wrapper/point_cloud_msg_wrapper.hpp>
      24                 :            : #include <sensor_msgs/msg/point_cloud2.hpp>
      25                 :            : #include <sensor_msgs/point_cloud2_iterator.hpp>
      26                 :            : #include <time_utils/time_utils.hpp>
      27                 :            : 
      28                 :            : #include <Eigen/Core>
      29                 :            : #include <vector>
      30                 :            : 
      31                 :            : using autoware::common::types::bool8_t;
      32                 :            : using autoware::common::types::float32_t;
      33                 :            : 
      34                 :            : namespace autoware
      35                 :            : {
      36                 :            : namespace localization
      37                 :            : {
      38                 :            : namespace ndt
      39                 :            : {
      40                 :            : /// CRTP Base class defining the required minimal API of an NDTScan.
      41                 :            : /// \tparam Derived Derived class
      42                 :            : /// \tparam NDTUnit The unit representing a single element within the scan.
      43                 :            : /// \tparam IteratorT The type of iterator to iterate the scan.
      44                 :            : template<typename Derived, typename NDTUnit, typename IteratorT>
      45                 :            : class NDTScanBase : public common::helper_functions::crtp<Derived>
      46                 :            : {
      47                 :            : public:
      48                 :            :   using Point = NDTUnit;
      49                 :            :   using TimePoint = std::chrono::system_clock::time_point;
      50                 :            : 
      51                 :            :   /// Get iterator pointing to the beginning of the internal container.
      52                 :            :   /// \return Begin iterator.
      53                 :        685 :   IteratorT begin() const
      54                 :            :   {
      55                 :        685 :     return this->impl().begin_();
      56                 :            :   }
      57                 :            : 
      58                 :            :   /// Get iterator pointing to the end of the internal container.
      59                 :            :   /// \return End iterator.
      60                 :        685 :   IteratorT end() const
      61                 :            :   {
      62                 :        685 :     return this->impl().end_();
      63                 :            :   }
      64                 :            : 
      65                 :            :   /// Clear the states and the internal cache of the scan.
      66                 :         10 :   void clear()
      67                 :            :   {
      68                 :         10 :     return this->impl().clear_();
      69                 :            :   }
      70                 :            : 
      71                 :            :   /// Check if there is any data in the scan.
      72                 :            :   /// \return True if the internal container is empty.
      73                 :          3 :   bool8_t empty()
      74                 :            :   {
      75                 :          3 :     return this->impl().empty_();
      76                 :            :   }
      77                 :            : 
      78                 :            :   /// Insert a point cloud into the NDTScan. This is the step where the pointcloud is
      79                 :            :   /// converted into the ndt scan representation.
      80                 :            :   /// \param msg Point cloud to insert.
      81                 :         12 :   void insert(const sensor_msgs::msg::PointCloud2 & msg)
      82                 :            :   {
      83                 :         12 :     this->impl().insert_(msg);
      84                 :         11 :   }
      85                 :            : 
      86                 :            :   /// Number of points inside the scan.
      87                 :            :   /// \return Number of points
      88                 :          3 :   std::size_t size() const
      89                 :            :   {
      90                 :          3 :     return this->impl().size_();
      91                 :            :   }
      92                 :            : 
      93                 :            :   TimePoint stamp()
      94                 :            :   {
      95                 :            :     return this->impl().stamp_();
      96                 :            :   }
      97                 :            : };
      98                 :            : 
      99                 :            : /// Represents a lidar scan in a P2D optimization problem. It is a wrapper around an
     100                 :            : /// std::vector<Eigen::Vector3d>
     101                 :            : class NDT_PUBLIC P2DNDTScan : public NDTScanBase<P2DNDTScan,
     102                 :            :     Eigen::Vector3d, std::vector<Eigen::Vector3d>::const_iterator>
     103                 :            : {
     104                 :            : public:
     105                 :            :   using Container = std::vector<Eigen::Vector3d>;
     106                 :            :   using iterator = Container::const_iterator;
     107                 :            : 
     108                 :            :   // Make sure the given iterator type in the template is compatible with the used container.
     109                 :            :   // container should have `iterator` type/alias defined.
     110                 :            :   static_assert(
     111                 :            :     std::is_same<decltype(std::declval<NDTScanBase>().begin()), iterator>::value,
     112                 :            :     "P2DNDTScan: The iterator type parameter should match the "
     113                 :            :     "iterator of the container.");
     114                 :            : 
     115                 :            :   /// Constructor
     116                 :            :   /// \param msg Point cloud message to initialize this scan with.
     117                 :            :   /// \param capacity Capacity of the scan. It should be configured according to the max. expected
     118                 :            :   /// point cloud message size from the lidar.
     119                 :          7 :   P2DNDTScan(
     120                 :            :     const sensor_msgs::msg::PointCloud2 & msg,
     121                 :            :     std::size_t capacity)
     122                 :          7 :   {
     123         [ +  - ]:          7 :     m_points.reserve(capacity);
     124         [ +  - ]:          7 :     insert_(msg);
     125                 :          7 :   }
     126                 :            : 
     127                 :            :   // Scans should be moved rather than being copied.
     128                 :            :   P2DNDTScan(const P2DNDTScan &) = delete;
     129                 :            :   P2DNDTScan & operator=(const P2DNDTScan &) = delete;
     130                 :            : 
     131                 :            :   // Explicitly declaring to default is needed since we explicitly deleted the copy methods.
     132                 :          6 :   P2DNDTScan(P2DNDTScan &&) = default;
     133                 :            :   P2DNDTScan & operator=(P2DNDTScan &&) = default;
     134                 :            : 
     135                 :            :   /// Constructor
     136                 :            :   /// \param capacity Capacity of the scan. It should be configured according to the max. expected
     137                 :            :   /// point cloud message size from the lidar.
     138                 :          8 :   explicit P2DNDTScan(std::size_t capacity)
     139                 :          8 :   {
     140         [ +  - ]:          8 :     m_points.reserve(capacity);
     141                 :          8 :   }
     142                 :            : 
     143                 :            :   /// Insert a point cloud into the NDTScan. This is the step where the pointcloud is
     144                 :            :   /// converted into the ndt scan representation.
     145                 :            :   /// \param msg Point cloud to insert.
     146                 :         19 :   void insert_(const sensor_msgs::msg::PointCloud2 & msg)
     147                 :            :   {
     148         [ +  + ]:         19 :     if (!m_points.empty()) {
     149                 :          1 :       m_points.clear();
     150                 :            :     }
     151                 :            : 
     152                 :         19 :     m_stamp = ::time_utils::from_message(msg.header.stamp);
     153                 :            : 
     154                 :         19 :     constexpr auto container_full_error = "received a lidar scan with more points than the "
     155                 :            :       "ndt scan representation can contain. Please re-configure the scan"
     156                 :            :       "representation accordingly.";
     157                 :            : 
     158         [ +  + ]:         19 :     if (msg.width > m_points.capacity()) {
     159         [ +  - ]:          1 :       throw std::length_error(container_full_error);
     160                 :            :     }
     161                 :            :     using autoware::common::types::PointXYZI;
     162         [ +  - ]:         18 :     point_cloud_msg_wrapper::PointCloud2View<PointXYZI> msg_view{msg};
     163         [ +  + ]:       1862 :     for (const auto & point : msg_view) {
     164         [ +  - ]:       1844 :       m_points.emplace_back(point.x, point.y, point.z);
     165                 :            :     }
     166                 :         18 :   }
     167                 :            : 
     168                 :            :   /// Get iterator pointing to the beginning of the internal container.
     169                 :            :   /// \return Begin iterator.
     170                 :        685 :   iterator begin_() const
     171                 :            :   {
     172                 :        685 :     return m_points.cbegin();
     173                 :            :   }
     174                 :            : 
     175                 :            :   /// Get iterator pointing to the end of the internal container.
     176                 :            :   /// \return End iterator.
     177                 :        685 :   iterator end_() const
     178                 :            :   {
     179                 :        685 :     return m_points.cend();
     180                 :            :   }
     181                 :            : 
     182                 :            :   /// Check if there is any data in the scan.
     183                 :            :   /// \return True if the internal container is empty.
     184                 :          3 :   bool8_t empty_()
     185                 :            :   {
     186                 :          3 :     return m_points.empty();
     187                 :            :   }
     188                 :            : 
     189                 :            :   /// Clear the states and the internal cache of the scan.
     190                 :         10 :   void clear_()
     191                 :            :   {
     192                 :         10 :     m_points.clear();
     193                 :         10 :   }
     194                 :            : 
     195                 :            :   /// Number of points inside the scan.
     196                 :            :   /// \return Number of points
     197                 :          3 :   std::size_t size_() const
     198                 :            :   {
     199                 :          3 :     return m_points.size();
     200                 :            :   }
     201                 :            : 
     202                 :            :   TimePoint stamp_()
     203                 :            :   {
     204                 :            :     return m_stamp;
     205                 :            :   }
     206                 :            : 
     207                 :            : private:
     208                 :            :   Container m_points;
     209                 :            :   NDTScanBase::TimePoint m_stamp{};
     210                 :            : };
     211                 :            : 
     212                 :            : }  // namespace ndt
     213                 :            : }  // namespace localization
     214                 :            : }  // namespace autoware
     215                 :            : 
     216                 :            : #endif  // NDT__NDT_SCAN_HPP_