PoseSegment.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    VisionX::ArmarXObjects::armem_images_server
 * @author     Rainer Kartmann ( rainer dot kartmann at kit dot edu )
 * @date       2021
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "PoseSegment.h"
 
#include "ArmarXCore/core/time/Clock.h"
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
 
#include <VisionX/libraries/armem_human/types.h>
#include <VisionX/libraries/armem_human/aron/HumanPose.aron.generated.h>
#include <VisionX/libraries/armem_human/aron_conversions.h>
#include <VisionX/libraries/armem_human/memory_ids.h>
 
 
namespace armarx::armem::server::human
{
    const std::string PoseSegment::CORE_SEGMENT_NAME =
        armarx::human::PoseCoreSegmentID.coreSegmentName;
 
    PoseSegment::PoseSegment(armem::server::MemoryToIceAdapter& iceMemory) :
        Base(iceMemory, CORE_SEGMENT_NAME, armarx::human::arondto::HumanPose::ToAronType(), 256)
    {
    }
 
    void
    PoseSegment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
    {
        Base::defineProperties(defs, prefix);
    }
 
    std::map<std::string, std::vector<armarx::armem::human::HumanPose>>
    PoseSegment::getHumanPoseEntities(const Duration& maxAge) const
    {
        std::map<std::string, std::vector<armarx::armem::human::HumanPose>> humanPosesMap;
 
        const auto now = armarx::Clock::Now();
 
        return segmentPtr->doLocked(
            [&]
            {
                segmentPtr->forEachProviderSegment(
                    [&](const wm::ProviderSegment& provSegment)
                    {
                        humanPosesMap[provSegment.name()] = {};
 
                        provSegment.forEachEntity(
                            [&](const wm::Entity& entity)
                            {
                                if (entity.empty())
                                {
                                    return;
                                }
                                entity.getLatestSnapshot().forEachInstance(
                                    [&](const wm::EntityInstance& instance)
                                    {
                                        armarx::human::arondto::HumanPose dto;
                                        dto.fromAron(instance.data());
                                        
 
                                        armarx::armem::human::HumanPose bo;
                                        fromAron(dto, bo);
 
                                        if ((now - instance.metadata().referencedTime) < maxAge)
                                        {
                                            humanPosesMap[provSegment.name()].push_back(bo);
                                        }
                                    });
                            });
                    });
 
                return humanPosesMap;
            });
    }
 
} // namespace armarx::armem::server::human