LocalizationQuery.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::Observers
* @author     Kai Welke (welke at kit dot edu), David Schiebener (david dot schiebener at kit dot edu)
* @copyright  2012 Humanoids Group, HIS, KIT
* @license    http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
// core
#include "LocalizationQuery.h"
#include "LocalizationStrategyOnce.h"
#include "LocalizationStrategyRepeated.h"
#include <MemoryX/libraries/memorytypes/segment/ObjectClassMemorySegment.h>
#include <MemoryX/libraries/helpers/ObjectRecognitionHelpers/ObjectRecognitionWrapper.h>
 
namespace memoryx
{
    LocalizationQuery::LocalizationQuery(const std::string& queryName, const std::string& objectClassName, int cycleTimeMS, int priority)
    {
        this->queryName = queryName;
        this->className = objectClassName;
        this->cycleTimeMS = cycleTimeMS;
        this->priority = priority;
    }
 
    std::vector<LocalizationJobPtr> LocalizationQuery::createJobs(const ObjectClassMemorySegmentBasePtr& objectClassSegment)
    {
        std::scoped_lock lock(jobsMutex);
 
        std::vector<LocalizationJobPtr> jobs;
 
        ObjectClassList objectClasses = objectClassSegment->getClassWithSubclasses(className);
 
        // extract classes which are recognizable and fill in recognition method map
        std::multimap<std::string, std::string> recognitionMethodMap;
        std::set<std::string> recognitionMethods;
 
        for (ObjectClassList::iterator iter = objectClasses.begin() ; iter != objectClasses.end() ; iter++)
        {
            ObjectClassPtr objectClass = ObjectClassPtr::dynamicCast(*iter);
 
            if (getLocalizableObjectClass(objectClass))
            {
                EntityWrappers::ObjectRecognitionWrapperPtr recognitionWrapper = objectClass->addWrapper(new EntityWrappers::ObjectRecognitionWrapper());
                std::string recognitionMethod = recognitionWrapper->getRecognitionMethod();
 
                recognitionMethodMap.insert(std::make_pair(recognitionMethod, objectClass->getName()));
                recognitionMethods.insert(recognitionMethod);
            }
        }
 
        // create one job for each recognition method
        for (std::set<std::string>::iterator iter = recognitionMethods.begin() ; iter != recognitionMethods.end() ; iter++)
        {
            std::string recognitionMethod = *iter;
 
            std::pair<std::multimap<std::string, std::string>::iterator, std::multimap<std::string, std::string>::iterator> classesForMethod;
            classesForMethod = recognitionMethodMap.equal_range(recognitionMethod);
 
            std::vector<std::string> objectClassNames;
 
            for (std::multimap<std::string, std::string>::iterator iter = classesForMethod.first; iter != classesForMethod.second ; iter++)
            {
                objectClassNames.push_back(iter->second);
            }
 
            // create job
            LocalizationJobPtr job;
 
            if (cycleTimeMS <= 0)
            {
                job = new LocalizationStrategyOnce();
            }
            else
            {
                job = new LocalizationStrategyRepeated(cycleTimeMS);
            }
 
            job->init(this, recognitionMethod, objectClassNames);
 
            jobsFinished[recognitionMethod] = false;
            jobs.push_back(job);
        }
 
 
 
        return jobs;
    }
 
    bool LocalizationQuery::getFinished()
    {
        std::scoped_lock lock(jobsMutex);
 
        if (jobsFinished.size() == 0)
        {
            return false;
        }
 
        bool finished = true;
 
        for (std::map<std::string, bool>::iterator iter = jobsFinished.begin() ; iter != jobsFinished.end() ; iter++)
        {
            finished &= iter->second;
        }
 
        return finished;
    }
 
    void LocalizationQuery::setJobFinished(std::string recognitionMethod)
    {
        std::scoped_lock lock(jobsMutex);
        jobsFinished[recognitionMethod] = true;
 
    }
 
    ObjectClassPtr LocalizationQuery::getLocalizableObjectClass(const EntityBasePtr& entity)
    {
        // check if this is an entity of type objectClass
        ObjectClassPtr objectClass = ObjectClassPtr::dynamicCast(entity);
 
        if (objectClass)
        {
            EntityWrappers::ObjectRecognitionWrapperPtr objectRecognitionWrapper = objectClass->addWrapper(new EntityWrappers::ObjectRecognitionWrapper());
            std::string recognitionMethod = objectRecognitionWrapper->getRecognitionMethod();
 
            // check if a recognition method is defined
            if ((recognitionMethod != "") && (recognitionMethod != "<none>"))
            {
                return objectClass;
            }
            else
            {
                if (objectClass->getName() != "all")
                {
                    ARMARX_WARNING_S << "Recognition method for object " << objectClass->getName() << " is undefined";
                }
 
                return NULL;
            }
        }
        else
        {
            ARMARX_WARNING_S << "Entity " << entity->getName() << " could not be casted to an ObjectClassPtr";
            return NULL;
        }
    }
}