ObjectCostmapStorage.cpp

Go to the documentation of this file.

#include "ObjectCostmapStorage.h"
 
#include "util.h"
 
#include <optional>
#include <utility>
#include <vector>
 
#include <Eigen/Geometry>
 
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/time/Duration.h>
 
#include <armarx/navigation/algorithms/Costmap.h>
 
namespace armarx::navigation::components::adaptive_distance_to_obstacle_costmap_provider
{
 
    void
    ObjectCostmapStorage::storeObjectCostmap(CachedObjectCostmap cachedObjectCostmap)
    {
        if (objectCostmaps_.find(cachedObjectCostmap.objectClassID.getClassID().str()) ==
            objectCostmaps_.end())
        {
            objectCostmaps_.emplace(cachedObjectCostmap.objectClassID.getClassID().str(),
                                    std::vector<CachedObjectCostmap>{});
        }
        objectCostmaps_[cachedObjectCostmap.objectClassID.getClassID().str()].emplace_back(
            std::move(cachedObjectCostmap));
    }
 
    std::optional<armarx::navigation::algorithms::Costmap>
    ObjectCostmapStorage::getTransformedCostmap(armarx::ObjectID objectID,
                                                Eigen::Matrix4f objectGlobalPose,
                                                JointValuesT jointValues)
    {
 
        if (objectCostmaps_.find(objectID.getClassID().str()) == objectCostmaps_.end())
        {
            return std::nullopt;
        }
 
        const auto global_T_object_desired_3d = objectGlobalPose;
        Eigen::Vector3f eDesired =
            global_T_object_desired_3d.block<3, 3>(0, 0).eulerAngles(0, 1, 2); // roll-pitch-yaw
 
        for (const auto& cachedObjectCostmap : objectCostmaps_.at(objectID.getClassID().str()))
        {
            // check joint values
            if (not jointValuesMatch(cachedObjectCostmap.jointValues, jointValues))
            {
                ARMARX_VERBOSE << "Skipping cached costmap because of different joint values";
                continue;
            }
 
            const auto global_T_object_stored = cachedObjectCostmap.global_T_object;
            Eigen::Vector3f eStored =
                global_T_object_stored.block<3, 3>(0, 0).eulerAngles(0, 1, 2); // roll-pitch-yaw
 
            // check if the stored costmap matches the desired one in terms of rotation
            float rollDiff = std::abs(eDesired.x() - eStored.x());
            float pitchDiff = std::abs(eDesired.y() - eStored.y());
            float yawDiff = eDesired.z() - eStored.z();
 
            if (rollDiff < 1e-3 && pitchDiff < 1e-3)
            {
 
                const Eigen::Vector2f global_P_object_desired =
                    Eigen::Isometry3f{global_T_object_desired_3d}.translation().head<2>();
                const Eigen::Vector2f global_P_object_stored =
                    Eigen::Isometry3f{global_T_object_stored}.translation().head<2>();
 
                ARMARX_INFO << deactivateSpam(10, "ObjectCostmapStorage::FoundCostmap")
                            << "Found cached costmap for objectID: " << objectID.str()
                            << " with rollDiff: " << rollDiff << ", pitchDiff: " << pitchDiff
                            << ", yawDiff: " << yawDiff;
 
                Eigen::Isometry2f global_T_object_desired =
                    Eigen::Translation2f{global_P_object_desired} *
                    Eigen::Rotation2Df{eDesired.z()};
                Eigen::Isometry2f global_T_object_stored =
                    Eigen::Translation2f{global_P_object_stored} * Eigen::Rotation2Df{eStored.z()};
 
 
                Eigen::Isometry2f global_T_costmap = cachedObjectCostmap.costmap.origin();
                Eigen::Isometry2f costmap_T_object =
                    global_T_costmap.inverse() * global_T_object_stored;
                Eigen::Isometry2f global_T_costmap_new =
                    global_T_object_desired * costmap_T_object.inverse();
 
                // copy costmap and update pose
                algorithms::Costmap adaptedCostmap{cachedObjectCostmap.costmap};
                adaptedCostmap.setOrigin(global_T_costmap_new);
 
                return adaptedCostmap;
            }
 
            ARMARX_DEBUG << "Cached costmap for objectID: " << objectID.str()
                         << " does not match desired rotation. rollDiff: " << rollDiff
                         << ", pitchDiff: " << pitchDiff << ", yawDiff: " << yawDiff;
        }
 
        ARMARX_INFO << deactivateSpam(10)
                    << "No matching cached costmap found for objectID: " << objectID.str();
        return std::nullopt;
    }
 
    void
    ObjectCostmapStorage::printOccupancy(bool noSpam)
    {
        static armarx::core::time::DateTime t1 =
            armarx::core::time::DateTime::Now() - armarx::core::time::Duration::Hours(24);
        if (noSpam)
        {
            const auto duration = armarx::core::time::DateTime::Now() - t1;
            if (duration > armarx::core::time::Duration::Seconds(5))
            {
                t1 = armarx::core::time::DateTime::Now();
            }
            else
            {
                return;
            }
        }
 
        ARMARX_INFO << "Stored object costmaps:";
        for (const auto& [classID, costmaps] : objectCostmaps_)
        {
            ARMARX_INFO << "- ClassID: " << classID
                        << ", number of cached costmaps: " << costmaps.size();
        }
    }
} // namespace armarx::navigation::components::adaptive_distance_to_obstacle_costmap_provider