ObjectCostmapStorage.h

Go to the documentation of this file.

#pragma once
 
#include <map>
#include <optional>
#include <string>
#include <unordered_map>
#include <vector>
 
#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
 
#include <armarx/navigation/algorithms/Costmap.h>
 
#include "util.h"
 
namespace armarx::navigation::components::adaptive_distance_to_obstacle_costmap_provider
{
    struct CachedObjectCostmap
    {
        armarx::ObjectID objectClassID;
 
        Eigen::Matrix4f global_T_object;
 
        JointValuesT jointValues;
 
        armarx::navigation::algorithms::Costmap costmap;
    };
 
    class ObjectCostmapStorage
    {
    public:
        ObjectCostmapStorage() = default;
 
        void storeObjectCostmap(CachedObjectCostmap cachedObjectCostmap);
        std::optional<armarx::navigation::algorithms::Costmap>
        getTransformedCostmap(armarx::ObjectID objectID,
                              Eigen::Matrix4f objectGlobalPose,
                              JointValuesT jointValues);
 
        void printOccupancy(bool noSpam = true);
 
    private:
        // class ID -> list of cached costmaps for base poses
        std::unordered_map<std::string, std::vector<CachedObjectCostmap>> objectCostmaps_;
    };
} // namespace armarx::navigation::components::adaptive_distance_to_obstacle_costmap_provider