occupancy_grid_builder.hpp

Go to the documentation of this file.

// Copyright 2015 TIER IV, Inc. All rights reserved.
//
// 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.
 
#ifndef SIMPLE_SENSOR_SIMULATOR__SENSOR_SIMULATION__OCCUPANCY_GRID__OCCUPANCY_GRID_BUILDER_HPP_
#define SIMPLE_SENSOR_SIMULATOR__SENSOR_SIMULATION__OCCUPANCY_GRID__OCCUPANCY_GRID_BUILDER_HPP_
 
#include <Eigen/Core>
#include <geometry_msgs/msg/point.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <simple_sensor_simulator/sensor_simulation/primitives/box.hpp>
#include <vector>
 
namespace simple_sensor_simulator
{
class OccupancyGridBuilder
{
  using MarkerCounterType = int16_t;
  using MarkerGridType = std::vector<MarkerCounterType>;
  using OccupancyGridType = std::vector<int8_t>;
  using PointType = geometry_msgs::msg::Point;
  using PoseType = geometry_msgs::msg::Pose;
  using PrimitiveType = primitives::Primitive;
  using PolygonType = std::vector<PointType>;
 
public:
  OccupancyGridBuilder(
    double resolution, size_t height, size_t width, int8_t occupied_cost = 100,
    int8_t invisible_cost = 50);
 
  const double resolution;
  const size_t height;
  const size_t width;
  const int8_t occupied_cost;
  const int8_t invisible_cost;
 
  auto add(const PrimitiveType & primitive) -> void;
 
  auto reset(const PoseType & origin) -> void;
 
  auto build() -> void;
 
  auto get() const -> const OccupancyGridType &;
 
private:
  PoseType origin_;
 
  MarkerCounterType primitive_count_ = 0;
 
  MarkerGridType occupied_grid_;
 
  MarkerGridType invisible_grid_;
 
  OccupancyGridType values_;
 
  std::vector<int32_t> min_cols_, max_cols_;
 
  inline auto addPolygon(MarkerGridType & grid, const PolygonType & convex_hull) -> void;
 
  inline auto transformToGrid(const PointType & world_point) const -> PointType;
 
  inline auto transformToPixel(const PointType & grid_point) const -> PointType;
 
  inline auto makePoint(double x, double y, double z = 0) const -> PointType;
 
  inline auto makeOccupiedArea(const PrimitiveType & primitive) const -> PolygonType;
 
  inline auto makeInvisibleArea(const PolygonType & occupied_polygon) const -> PolygonType;
};
}  // namespace simple_sensor_simulator
 
#endif  // SIMPLE_SENSOR_SIMULATOR__SENSOR_SIMULATION__OCCUPANCY_GRID__OCCUPANCY_GRID_BUILDER_HPP_