Program Listing for File occupancy_grid.hpp
↰ Return to documentation for file (include/beluga/sensor/data/occupancy_grid.hpp
)
// Copyright 2023 Ekumen, Inc.
//
// 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 BELUGA_SENSOR_DATA_OCCUPANCY_GRID_HPP
#define BELUGA_SENSOR_DATA_OCCUPANCY_GRID_HPP
#include <cstdint>
#include <tuple>
#include <vector>
#include <beluga/sensor/data/linear_grid.hpp>
#include <range/v3/view/enumerate.hpp>
#include <range/v3/view/filter.hpp>
#include <range/v3/view/transform.hpp>
#include <Eigen/Core>
namespace beluga {
template <typename Derived>
class BaseOccupancyGrid2 : public BaseLinearGrid2<Derived> {
public:
enum class Frame : std::uint8_t { kLocal, kGlobal };
[[nodiscard]] bool free_at(std::size_t index) const {
const auto data = this->self().data_at(index);
if (!data.has_value()) {
return false;
}
return this->self().value_traits().is_free(data.value());
}
[[nodiscard]] bool free_at(int xi, int yi) const { return this->self().free_at(this->self().index_at(xi, yi)); }
[[nodiscard]] bool free_at(const Eigen::Vector2i& pi) const { return this->self().free_at(pi.x(), pi.y()); }
[[nodiscard]] bool free_near(double x, double y) const {
return this->self().free_at(this->self().cell_near(Eigen::Vector2d{x, y}));
}
[[nodiscard]] bool free_near(const Eigen::Vector2d& p) const { return this->self().free_near(p.x(), p.y()); }
using BaseLinearGrid2<Derived>::coordinates_at;
[[nodiscard]] auto coordinates_at(std::size_t index, Frame frame) const {
auto position = this->self().coordinates_at(index);
if (frame == Frame::kGlobal) {
position = this->self().origin() * position;
}
return position;
}
template <class Range>
[[nodiscard]] auto coordinates_for(Range&& cells, Frame frame) const {
return cells | ranges::views::transform(
[this, frame](const auto& cell) { return this->self().coordinates_at(cell, frame); });
}
[[nodiscard]] auto free_cells() const {
return ranges::views::enumerate(this->self().data()) |
ranges::views::filter([value_traits = this->self().value_traits()](const auto& tuple) {
return value_traits.is_free(std::get<1>(tuple));
}) |
ranges::views::transform([](const auto& tuple) { return std::get<0>(tuple); });
}
[[nodiscard]] auto obstacle_data() const {
return this->self().data() |
ranges::views::transform([value_traits = this->self().value_traits()](const auto& value) {
return value_traits.is_occupied(value);
});
}
};
} // namespace beluga
#endif