TravGenNode.hpp

Go to the documentation of this file.

#pragma once
#include <base/Eigen.hpp>
#include <maps/grid/TraversabilityMap3d.hpp>
#include <boost/serialization/serialization.hpp>
#include <base/Angle.hpp>
 
namespace boost::serialization{
    template<class Archive>
    inline void serialize(Archive & ar, base::AngleSegment & segment, const unsigned int version){
        ar & segment.width;
        ar & segment.startRad;
        ar & segment.endRad;
    }
}
 
namespace traversability_generator3d
{
 
enum NodeType
{
    OBSTACLE = 0,
    TRAVERSABLE,
    FRONTIER,
    INFLATED_OBSTACLE,
    INFLATED_FRONTIER,
    UNKNOWN,
    HOLE,
    UNSET
};
 
struct TravGenTrackingData
{
    Eigen::Hyperplane<double, 3> plane;
    
    double slope;
    
    Eigen::Vector3d slopeDirection;
    
    double slopeDirectionAtan2; 
    
    size_t id; 
 
    std::vector<base::AngleSegment> allowedOrientations;
    
    NodeType nodeType;
 
    int cost;
 
    template<class Archive>
    void serialize(Archive & ar, const unsigned int version)
    {
        ar & plane.offset();
        ar & plane.normal().x();
        ar & plane.normal().y();
        ar & plane.normal().z();
        ar & slope;
        ar & slopeDirection.x();
        ar & slopeDirection.y();
        ar & slopeDirection.z();
        ar & slopeDirectionAtan2;
        ar & id;
        ar & allowedOrientations;
        ar & nodeType;
        ar & cost;
    }
};
 
// Inline operator<< to print NodeType as a string
inline std::ostream& operator<<(std::ostream& os, NodeType type)
{
    switch (type)
    {
        case NodeType::OBSTACLE: os << "OBSTACLE"; break;
        case NodeType::TRAVERSABLE: os << "TRAVERSABLE"; break;
        case NodeType::FRONTIER: os << "FRONTIER"; break;
        case NodeType::INFLATED_OBSTACLE: os << "INFLATED_OBSTACLE"; break;
        case NodeType::INFLATED_FRONTIER: os << "INFLATED_FRONTIER"; break;
        case NodeType::UNKNOWN: os << "UNKNOWN"; break;
        case NodeType::HOLE: os << "HOLE"; break;
        case NodeType::UNSET: os << "UNSET"; break;
        default: os << "INVALID_NODE_TYPE"; break;
    }
    return os;
}
 
// Inline operator== for NodeType
inline bool operator==(NodeType lhs, NodeType rhs)
{
    return static_cast<int>(lhs) == static_cast<int>(rhs);
}
 
// Inline operator== for NodeType
inline bool operator==(NodeType lhs, int rhs)
{
    return (static_cast<int>(lhs) == rhs);
}
 
typedef maps::grid::TraversabilityNode<TravGenTrackingData> TravGenNode;
typedef maps::grid::TraversabilityMap3d<TravGenNode *> TravMap3d;
 
}