map.hpp

Go to the documentation of this file.

// Copyright 2020 the Autoware Foundation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
 
#ifndef POINT_CLOUD_MAPPING__MAP_HPP_
#define POINT_CLOUD_MAPPING__MAP_HPP_
 
#include <point_cloud_mapping/visibility_control.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <sensor_msgs/point_cloud2_iterator.hpp>
#include <voxel_grid/voxel_grid.hpp>
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <common/types.hpp>
#include <pcl/io/pcd_io.h>
#include <vector>
#include <string>
#include <unordered_map>
 
namespace autoware
{
namespace mapping
{
namespace point_cloud_mapping
{
 
enum class MapUpdateType
{
  NEW,
  UPDATE,
  PARTIAL_UPDATE,
  NO_CHANGE
};
 
enum class MapStorageMode
{
  Independent,
  Shared
};
 
struct POINT_CLOUD_MAPPING_PUBLIC MapUpdateSummary
{
  MapUpdateType update_type;
  std::size_t num_added_pts;
};
using common::types::float32_t;
 
template<typename IncrementT, MapStorageMode StorageModeT>
class POINT_CLOUD_MAPPING_PUBLIC MapRepresentationBase
{
public:
  using Increment = IncrementT;
  virtual MapUpdateSummary try_add_observation(
    const IncrementT & observation,
    const geometry_msgs::msg::PoseWithCovarianceStamped & pose) = 0;
 
  virtual void write(const std::string & file_name_prefix) const = 0;
 
  virtual std::size_t size() const noexcept = 0;
 
  virtual std::size_t capacity() const noexcept = 0;
 
  static constexpr MapStorageMode storage_mode() noexcept
  {
    return StorageModeT;
  }
 
  virtual ~MapRepresentationBase() = default;
 
  virtual void clear() = 0;
};
 
class POINT_CLOUD_MAPPING_PUBLIC PlainPointCloudMap
  : public MapRepresentationBase<sensor_msgs::msg::PointCloud2, MapStorageMode::Independent>
{
public:
  using Base = MapRepresentationBase<sensor_msgs::msg::PointCloud2,
      MapStorageMode::Independent>;
  static constexpr auto NUM_FIELDS{4U};
  // pcl cloud is used to seamlessly write the file without needing to copy/allocate.
  using PCLCloud = pcl::PointCloud<pcl::PointXYZI>;
  using Cloud = sensor_msgs::msg::PointCloud2;
  using CloudIt = sensor_msgs::PointCloud2Iterator<float32_t>;
  using CloudConstIt = sensor_msgs::PointCloud2ConstIterator<float32_t>;
 
  explicit PlainPointCloudMap(std::size_t capacity, const std::string & frame = "map");
 
  MapUpdateSummary try_add_observation(
    const Cloud & observation,
    const geometry_msgs::msg::PoseWithCovarianceStamped &) override;
 
  void write(const std::string & file_name_prefix) const override;
  std::size_t size() const noexcept override;
  std::size_t capacity() const noexcept override;
  void clear() override;
 
private:
  PCLCloud m_cloud;
  std::size_t m_capacity;
  std::string m_frame_id;
};
 
class POINT_CLOUD_MAPPING_PUBLIC VoxelMap
  : public MapRepresentationBase<sensor_msgs::msg::PointCloud2, MapStorageMode::Independent>
{
public:
  using Base = MapRepresentationBase<sensor_msgs::msg::PointCloud2,
      MapStorageMode::Independent>;
  static constexpr auto NUM_FIELDS{4U};
  using PCLCloud = pcl::PointCloud<pcl::PointXYZI>;
  using Cloud = sensor_msgs::msg::PointCloud2;
  using CloudIt = sensor_msgs::PointCloud2Iterator<float32_t>;
  using CloudConstIt = sensor_msgs::PointCloud2ConstIterator<float32_t>;
 
  explicit VoxelMap(
    const perception::filters::voxel_grid::Config & grid_config,
    const std::string & frame = "map");
 
  MapUpdateSummary try_add_observation(
    const Cloud & observation,
    const geometry_msgs::msg::PoseWithCovarianceStamped &) override;
  void write(const std::string & file_name_prefix) const override;
  std::size_t size() const noexcept override;
  std::size_t capacity() const noexcept override;
  void clear() override;
 
private:
  perception::filters::voxel_grid::Config m_grid_config;
  std::unordered_map<uint64_t,
    perception::filters::voxel_grid::CentroidVoxel<common::types::PointXYZIF>> m_grid;
  std::string m_frame_id;
};
 
 
}  // namespace point_cloud_mapping
}  // namespace mapping
}  // namespace autoware
 
#endif  // POINT_CLOUD_MAPPING__MAP_HPP_