LocalizationJobContainer.h

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::Core
* @author     Kai Welke <welke@kit.edu>
* @copyright  2012 Kai Welke
* @license    http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#pragma once
 
#include <string>
 
#include <Eigen/Eigen>
 
#include <Ice/Ice.h>
 
#include <RobotAPI/interface/core/RobotState.h>
#include <RobotAPI/libraries/core/Pose.h>
 
#include <MemoryX/libraries/helpers/ObjectRecognitionHelpers/LocalizationJob.h>
 
namespace memoryx
{
    class LocalizationQuery;
    using LocalizationQueryPtr = IceInternal::Handle<LocalizationQuery>;
 
    /**
     * @brief The LocalizationJobContainer class is used by ObjectLocalizationMemoryUpdater to create and execute segmentation specific groups of localization jobs.
     */
    class LocalizationJobContainer : public Ice::LocalObject
    {
    public:
        LocalizationJobContainer(const std::string& recognitionMethod) :
            recognitionMethod(recognitionMethod)
        {
            robotPose = new armarx::Pose(Eigen::Matrix4f::Identity());
        }
 
        ~LocalizationJobContainer() override
        {
            try
            {
                if (this->robotStatePrx)
                {
                    this->robotStatePrx->unref();
                }
            }
            catch (...)
            {
            }
        }
 
        void
        addJob(const LocalizationJobPtr& job)
        {
            jobs.push_back(job);
 
            std::vector<std::string> jobClassNames = job->getClassNamesToBeLocalized();
 
            for (std::vector<std::string>::iterator iterNames = jobClassNames.begin();
                 iterNames != jobClassNames.end();
                 iterNames++)
            {
                classNamesUnique.insert(*iterNames);
            }
        }
 
        /**
         * @brief startJobs calls the LocalizationJob::start() method on all jobs added via LocalizationJobContainer::addJob().
         */
        void
        startJobs()
        {
            for (LocalizationJobList::iterator iter = jobs.begin(); iter != jobs.end(); iter++)
            {
                (*iter)->start();
            }
        }
 
        /**
         * @brief finishJobs sets the finished property of all localization jobs to true
         *
         * This results in LocalizationQuery::getFinished() to return true, indicating that all localization jobs have finished running.
         * Calls LocalizationJob::setFinished() on each job added via LocalizationJobContainer::addJob().
         */
        void
        finishJobs()
        {
            for (LocalizationJobList::iterator iter = jobs.begin(); iter != jobs.end(); iter++)
            {
                (*iter)->setFinished();
            }
        }
 
        std::vector<std::string>
        getClassNamesUnique()
        {
            std::vector<std::string> classNames;
            std::copy(
                classNamesUnique.begin(), classNamesUnique.end(), std::back_inserter(classNames));
 
            return classNames;
        }
 
        std::vector<LocalizationQueryPtr>
        getQueries()
        {
            std::vector<LocalizationQueryPtr> queries;
 
            for (LocalizationJobList::iterator iter = jobs.begin(); iter != jobs.end(); iter++)
            {
                queries.push_back((*iter)->getQuery());
            }
 
            return queries;
        }
 
        std::string
        getRecognitionMethod()
        {
            return recognitionMethod;
        }
 
        void
        setRobotState(const armarx::SharedRobotInterfacePrx& robotStatePrx)
        {
            if (this->robotStatePrx)
            {
                this->robotStatePrx->unref();
            }
 
            this->robotStatePrx = robotStatePrx;
 
            if (this->robotStatePrx)
            {
                this->robotStatePrx->ref();
            }
        }
 
        armarx::SharedRobotInterfacePrx
        getRobotState()
        {
            return robotStatePrx;
        }
 
        void
        setRobotPose(const armarx::PosePtr& robotPose)
        {
            this->robotPose = robotPose;
        }
 
        armarx::PosePtr
        getRobotPose()
        {
            return robotPose;
        }
 
 
    private:
        LocalizationJobList jobs;
        std::set<std::string> classNamesUnique;
        std::string recognitionMethod;
        armarx::SharedRobotInterfacePrx robotStatePrx;
        armarx::PosePtr robotPose;
    };
 
    using LocalizationJobContainerPtr = IceUtil::Handle<LocalizationJobContainer>;
} // namespace memoryx