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 <boost/serialization/version.hpp>
#include <base/Angle.hpp>
#include <cstdint>
 
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,
    PARTIALLY_TRAVERSABLE
};
 
enum ObstacleCause
{
    CAUSE_NONE = 0,  
    UNMEASURED,      
    STEEP_SLOPE,     
    STEP_HEIGHT,     
    INCLINE_LIMIT,   
    NO_SAFE_YAW,     
    MAP_BOUNDARY     
};
 
struct TravGenTrackingData
{
    Eigen::Hyperplane<double, 3> plane;
    
    double slope = 0.0;
    
    Eigen::Vector3d slopeDirection = Eigen::Vector3d::Zero();
    
    double slopeDirectionAtan2 = 0.0; 
    
    size_t id = 0; 
 
    std::vector<base::AngleSegment> allowedOrientations;
    
    NodeType nodeType = NodeType::UNSET;
 
    ObstacleCause obstacleCause = ObstacleCause::CAUSE_NONE;
 
    int cost = 0;
 
    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;
        // obstacleCause was appended in class version 1; archives written
        // before that must not try to read it.
        if (version >= 1)
            ar & obstacleCause;
        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;
        case NodeType::PARTIALLY_TRAVERSABLE: os << "PARTIALLY_TRAVERSABLE"; 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;
 
}
 
BOOST_CLASS_VERSION(traversability_generator3d::TravGenTrackingData, 1)