occupancy.hpp

 
#ifndef UFO_MAP_OCCUPANCY_OCCUPANCY_HPP
#define UFO_MAP_OCCUPANCY_OCCUPANCY_HPP
 
// UFO
#include <ufo/map/occupancy/state.hpp>
 
// STL
#include <cstdint>
#include <type_traits>
 
namespace ufo
{
template <class T = float>
struct Occupancy;
 
template <>
struct Occupancy<float> {
    using value_type = float;
 
    value_type logit;
    // First 4 bits indicate contains,
    //      bit #0 = unknown, #1 = free, #2 = occupied, and #4 = void region.
    std::uint8_t data;
};
 
template <>
struct Occupancy<std::uint8_t> {
    using value_type = std::uint8_t;
 
    value_type logit;
    // First 4 bits indicate contains,
    //      bit #0 = unknown, #1 = free, #2 = occupied, and #4 = void region.
    std::uint8_t data;
};
 
template <>
struct Occupancy<OccupancyState> {
    using value_type = OccupancyState;
 
    // First 4 bits indicate contains,
    //      Bit #0 = unknown, #1 = free, #2 = occupied, and #3 = void region.
    // Next 2 bits indicate state
    std::uint8_t data;
};
 
namespace detail
{
template <class T>
[[nodiscard]] constexpr auto occupancy(Occupancy<T> occupancy)
{
    // TODO: Implement
}
 
template <class T>
[[nodiscard]] constexpr auto logit(Occupancy<T> occupancy)
{
    if constexpr (std::is_same_v<bool, T>) {
        return (0b01110000u & occupancy.data) >> 4u;
    } else {
        return occupancy.logit;
    }
}
 
template <class T>
[[nodiscard]] constexpr bool containsUnknown(Occupancy<T> occupancy)
{
    return static_cast<bool>(0b00000001u & occupancy.data);
}
 
template <class T>
[[nodiscard]] constexpr bool containsFree(Occupancy<T> occupancy)
{
    return static_cast<bool>(0b00000010u & occupancy.data);
}
 
template <class T>
[[nodiscard]] constexpr bool containsOccupied(Occupancy<T> occupancy)
{
    return static_cast<bool>(0b00000100u & occupancy.data);
}
 
template <class T>
[[nodiscard]] constexpr bool containsVoidRegion(Occupancy<T> occupancy)
{
    return static_cast<bool>(0b00001000u & occupancy.data);
}
 
template <class T>
[[nodiscard]] constexpr bool unknown(Occupancy<T> occupancy, T min_threshold,
                                     T max_threshold)
{
    // TODO: Implement
}
 
template <class T>
[[nodiscard]] constexpr bool free(Occupancy<T> occupancy, T min_threshold,
                                  T max_threshold)
{
    // TODO: Implement
}
 
template <class T>
[[nodiscard]] bool occupied(Occupancy<T> occupancy, T threshold)
{
    // TODO: Implement
}
};  // namespace detail
}  // namespace ufo
 
#endif  // UFO_MAP_OCCUPANCY_OCCUPANCY_HPP