EnvironmentalPrimitiveSegment.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    MemoryX::WorkingMemory
* @author     Peter Kaiser (peter dot kaiser at kit dot edu)
* @date       2014
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#include "EnvironmentalPrimitiveSegment.h"
 
namespace memoryx
{
    EnvironmentalPrimitiveSegment::EnvironmentalPrimitiveSegment() :
        WorkingMemoryEntitySegment<EnvironmentalPrimitive>::WorkingMemoryEntitySegment(),
        EnvironmentalPrimitiveSegmentBase::EnvironmentalPrimitiveSegmentBase()
    {
    }
 
    EnvironmentalPrimitiveBaseList
    EnvironmentalPrimitiveSegment::getEnvironmentalPrimitives(const ::Ice::Current& c) const
    {
        EnvironmentalPrimitiveBaseList primitives;
 
        for (auto& id : getAllEntityIds(c))
        {
            EnvironmentalPrimitiveBasePtr primitive =
                EnvironmentalPrimitiveBasePtr::dynamicCast(getEntityById(id, c));
 
            if (primitive)
            {
                primitives.push_back(primitive);
            }
        }
 
        return primitives;
    }
 
    PlanePrimitiveBaseList
    EnvironmentalPrimitiveSegment::getPlanes(const Ice::Current& c) const
    {
        PlanePrimitiveBaseList primitives;
 
        for (auto& id : getAllEntityIds(c))
        {
            EntityBasePtr primitive = getEntityById(id, c);
 
            if (primitive->ice_isA(PlanePrimitiveBase::ice_staticId()))
            {
                primitives.push_back(PlanePrimitiveBasePtr::dynamicCast(primitive));
            }
        }
 
        return primitives;
    }
 
    SpherePrimitiveBaseList
    EnvironmentalPrimitiveSegment::getSpheres(const Ice::Current& c) const
    {
        SpherePrimitiveBaseList primitives;
 
        for (auto& id : getAllEntityIds(c))
        {
            EntityBasePtr primitive = getEntityById(id, c);
 
            if (primitive->ice_isA(SpherePrimitiveBase::ice_staticId()))
            {
                primitives.push_back(SpherePrimitiveBasePtr::dynamicCast(primitive));
            }
        }
 
        return primitives;
    }
 
    CylinderPrimitiveBaseList
    EnvironmentalPrimitiveSegment::getCylinders(const Ice::Current& c) const
    {
        CylinderPrimitiveBaseList primitives;
 
        for (auto& id : getAllEntityIds(c))
        {
            EntityBasePtr primitive = getEntityById(id, c);
 
            if (primitive->ice_isA(CylinderPrimitiveBase::ice_staticId()))
            {
                primitives.push_back(CylinderPrimitiveBasePtr::dynamicCast(primitive));
            }
        }
 
        return primitives;
    }
 
    BoxPrimitiveBaseList
    EnvironmentalPrimitiveSegment::getBoxes(const Ice::Current& c) const
    {
        BoxPrimitiveBaseList primitives;
 
        for (auto& id : getAllEntityIds(c))
        {
            EntityBasePtr primitive = getEntityById(id, c);
 
            if (primitive->ice_isA(BoxPrimitiveBase::ice_staticId()))
            {
                primitives.push_back(BoxPrimitiveBasePtr::dynamicCast(primitive));
            }
        }
 
        return primitives;
    }
 
    EnvironmentalPrimitiveBasePtr
    EnvironmentalPrimitiveSegment::getEnvironmentalPrimitiveById(const std::string& id,
                                                                 const Ice::Current& c) const
    {
        return EnvironmentalPrimitiveBasePtr::dynamicCast(getEntityById(id, c));
    }
 
    PlanePrimitiveBasePtr
    EnvironmentalPrimitiveSegment::getPlaneById(const std::string& id, const Ice::Current& c) const
    {
        EntityBasePtr primitive = getEntityById(id, c);
 
        if (primitive->ice_isA(PlanePrimitiveBase::ice_staticId()))
        {
            return PlanePrimitiveBasePtr::dynamicCast(primitive);
        }
        else
        {
            return PlanePrimitiveBasePtr();
        }
    }
 
    SpherePrimitiveBasePtr
    EnvironmentalPrimitiveSegment::getSphereById(const std::string& id, const Ice::Current& c) const
    {
        EntityBasePtr primitive = getEntityById(id, c);
 
        if (primitive->ice_isA(SpherePrimitiveBase::ice_staticId()))
        {
            return SpherePrimitiveBasePtr::dynamicCast(primitive);
        }
        else
        {
            return SpherePrimitiveBasePtr();
        }
    }
 
    CylinderPrimitiveBasePtr
    EnvironmentalPrimitiveSegment::getCylinderById(const std::string& id,
                                                   const Ice::Current& c) const
    {
        EntityBasePtr primitive = getEntityById(id, c);
 
        if (primitive->ice_isA(CylinderPrimitiveBase::ice_staticId()))
        {
            return CylinderPrimitiveBasePtr::dynamicCast(primitive);
        }
        else
        {
            return CylinderPrimitiveBasePtr();
        }
    }
 
    BoxPrimitiveBasePtr
    EnvironmentalPrimitiveSegment::getBoxById(const std::string& id, const Ice::Current&) const
    {
        EntityBasePtr primitive = getEntityById(id);
 
        if (primitive->ice_isA(BoxPrimitiveBase::ice_staticId()))
        {
            return BoxPrimitiveBasePtr::dynamicCast(primitive);
        }
        else
        {
            return BoxPrimitiveBasePtr();
        }
    }
 
    EnvironmentalPrimitiveBaseList
    EnvironmentalPrimitiveSegment::getEnvironmentalPrimitivesByTimestamp(
        const armarx::TimestampBasePtr& timestamp,
        const Ice::Current& c) const
    {
        EnvironmentalPrimitiveBaseList result;
 
        for (auto& id : getAllEntityIds(c))
        {
            EnvironmentalPrimitiveBasePtr primitive = getEnvironmentalPrimitiveById(id, c);
 
            if (primitive->getTime(c)->timestamp == timestamp->timestamp)
            {
                result.push_back(primitive);
            }
        }
 
        return result;
    }
 
    PlanePrimitiveBaseList
    EnvironmentalPrimitiveSegment::getPlanesByTimestamp(const armarx::TimestampBasePtr& timestamp,
                                                        const Ice::Current& c) const
    {
        PlanePrimitiveBaseList result;
 
        for (auto& id : getAllEntityIds(c))
        {
            EnvironmentalPrimitiveBasePtr primitive = getEnvironmentalPrimitiveById(id, c);
 
            if (primitive->ice_isA(PlanePrimitiveBase::ice_staticId()) &&
                primitive->getTime(c)->timestamp == timestamp->timestamp)
            {
                result.push_back(PlanePrimitiveBasePtr::dynamicCast(primitive));
            }
        }
 
        return result;
    }
 
    SpherePrimitiveBaseList
    EnvironmentalPrimitiveSegment::getSpheresByTimestamp(const armarx::TimestampBasePtr& timestamp,
                                                         const Ice::Current& c) const
    {
        SpherePrimitiveBaseList result;
 
        for (auto& id : getAllEntityIds(c))
        {
            EnvironmentalPrimitiveBasePtr primitive = getEnvironmentalPrimitiveById(id, c);
 
            if (primitive->ice_isA(SpherePrimitiveBase::ice_staticId()) &&
                primitive->getTime(c)->timestamp == timestamp->timestamp)
            {
                result.push_back(SpherePrimitiveBasePtr::dynamicCast(primitive));
            }
        }
 
        return result;
    }
 
    CylinderPrimitiveBaseList
    EnvironmentalPrimitiveSegment::getCylindersByTimestamp(
        const armarx::TimestampBasePtr& timestamp,
        const Ice::Current& c) const
    {
        CylinderPrimitiveBaseList result;
 
        for (auto& id : getAllEntityIds(c))
        {
            EnvironmentalPrimitiveBasePtr primitive = getEnvironmentalPrimitiveById(id, c);
 
            if (primitive->ice_isA(CylinderPrimitiveBase::ice_staticId()) &&
                primitive->getTime(c)->timestamp == timestamp->timestamp)
            {
                result.push_back(CylinderPrimitiveBasePtr::dynamicCast(primitive));
            }
        }
 
        return result;
    }
 
    BoxPrimitiveBaseList
    EnvironmentalPrimitiveSegment::getBoxesByTimestamp(const armarx::TimestampBasePtr& timestamp,
                                                       const Ice::Current& c) const
    {
        BoxPrimitiveBaseList result;
 
        for (auto& id : getAllEntityIds(c))
        {
            EnvironmentalPrimitiveBasePtr primitive = getEnvironmentalPrimitiveById(id, c);
 
            if (primitive->ice_isA(BoxPrimitiveBase::ice_staticId()) &&
                primitive->getTime(c)->timestamp == timestamp->timestamp)
            {
                result.push_back(BoxPrimitiveBasePtr::dynamicCast(primitive));
            }
        }
 
        return result;
    }
 
    EnvironmentalPrimitiveBaseList
    EnvironmentalPrimitiveSegment::getMostRecentEnvironmentalPrimitives(const Ice::Current& c) const
    {
        EnvironmentalPrimitiveBaseList primitives = getEnvironmentalPrimitives(c);
        Ice::Long timestamp = getMostRecentTimestamp<EnvironmentalPrimitiveBasePtr>(primitives, c);
 
        EnvironmentalPrimitiveBaseList result;
        for (auto& p : primitives)
        {
            if (p->getTime()->timestamp == timestamp)
            {
                result.push_back(p);
            }
        }
 
        return result;
    }
 
    PlanePrimitiveBaseList
    EnvironmentalPrimitiveSegment::getMostRecentPlanes(const Ice::Current& c) const
    {
        PlanePrimitiveBaseList primitives = getPlanes(c);
        Ice::Long timestamp = getMostRecentTimestamp<PlanePrimitiveBasePtr>(primitives, c);
 
        PlanePrimitiveBaseList result;
        for (auto& p : primitives)
        {
            if (p->getTime()->timestamp == timestamp)
            {
                result.push_back(p);
            }
        }
 
        return result;
    }
 
    SpherePrimitiveBaseList
    EnvironmentalPrimitiveSegment::getMostRecentSpheres(const Ice::Current& c) const
    {
        SpherePrimitiveBaseList primitives = getSpheres(c);
        Ice::Long timestamp = getMostRecentTimestamp<SpherePrimitiveBasePtr>(primitives, c);
 
        SpherePrimitiveBaseList result;
        for (auto& p : primitives)
        {
            if (p->getTime()->timestamp == timestamp)
            {
                result.push_back(p);
            }
        }
 
        return result;
    }
 
    CylinderPrimitiveBaseList
    EnvironmentalPrimitiveSegment::getMostRecentCylinders(const Ice::Current& c) const
    {
        CylinderPrimitiveBaseList primitives = getCylinders(c);
        Ice::Long timestamp = getMostRecentTimestamp<CylinderPrimitiveBasePtr>(primitives, c);
 
        CylinderPrimitiveBaseList result;
        for (auto& p : primitives)
        {
            if (p->getTime()->timestamp == timestamp)
            {
                result.push_back(p);
            }
        }
 
        return result;
    }
 
    BoxPrimitiveBaseList
    EnvironmentalPrimitiveSegment::getMostRecentBoxes(const Ice::Current& c) const
    {
        BoxPrimitiveBaseList primitives = getBoxes(c);
        Ice::Long timestamp = getMostRecentTimestamp<BoxPrimitiveBasePtr>(primitives, c);
 
        BoxPrimitiveBaseList result;
        for (auto& p : primitives)
        {
            if (p->getTime()->timestamp == timestamp)
            {
                result.push_back(p);
            }
        }
 
        return result;
    }
 
    void
    EnvironmentalPrimitiveSegment::removePrimitivesByTimestamp(
        const armarx::TimestampBasePtr& timestamp,
        const Ice::Current& c)
    {
        EnvironmentalPrimitiveBaseList primitives = getEnvironmentalPrimitives(c);
 
        for (auto& p : primitives)
        {
            if (p->getTime()->timestamp == timestamp)
            {
                removeEntity(p->getId(), c);
            }
        }
    }
 
    void
    EnvironmentalPrimitiveSegment::removeOlderPrimitives(const armarx::TimestampBasePtr& olderThan,
                                                         const Ice::Current& c)
    {
        EnvironmentalPrimitiveBaseList primitives = getEnvironmentalPrimitives(c);
 
        for (auto& p : primitives)
        {
            if (p->getTime()->timestamp < olderThan)
            {
                removeEntity(p->getId(), c);
            }
        }
    }
} // namespace memoryx