ObjectLocalizationMemoryUpdater.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 <ArmarXCore/core/system/ImportExport.h>
 
#include <MemoryX/libraries/memorytypes/segment/ObjectInstanceMemorySegment/ObjectInstanceMemorySegment.h>
#include <MemoryX/libraries/memorytypes/segment/AgentInstancesSegment.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectClass.h>
#include <MemoryX/libraries/workingmemory/updater/WorkingMemoryUpdater.h>
#include <MemoryX/libraries/helpers/ObjectRecognitionHelpers/LocalizationJob.h>
 
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <RobotAPI/interface/core/RobotState.h>
#include <ArmarXCore/observers/variant/DataFieldIdentifier.h>
 
#include <MemoryX/libraries/helpers/ObjectRecognitionHelpers/LocalizationQuery.h>
#include <MemoryX/libraries/helpers/ObjectRecognitionHelpers/LocalizationJobContainer.h>
 
#include <RobotAPI/interface/units/TCPControlUnit.h>
 
#include <MemoryX/components/CommonPlacesLearner/CommonPlacesLearner.h>
 
 
namespace memoryx
{
    class ObjectLocalizationMemoryUpdater;
    using ObjectLocalizationMemoryUpdaterPtr = IceInternal::Handle<ObjectLocalizationMemoryUpdater>;
 
    struct ObjectLocalizationMemoryUpdaterProperties : WorkingMemoryUpdaterProperties
    {
        ObjectLocalizationMemoryUpdaterProperties(std::string prefix):
            WorkingMemoryUpdaterProperties(prefix)
        {
            defineOptionalProperty<std::string>("RobotStateComponentName", "RobotStateComponent", "Name of the RobotStateComponent");
            defineOptionalProperty<std::string>("WorkingMemoryName", "WorkingMemory", "Name of the WorkingMemory component that should be used");
            defineOptionalProperty<std::string>("LongtermMemoryName", "LongtermMemory", "Name of the LongtermMemory component that should be used");
            defineOptionalProperty<std::string>("RobotStateObserverName", "RobotStateObserver", "Name of the RobotStateObserver that can be used for observing the head motion");
            defineOptionalProperty<std::string>("GazeTCPName", "VirtualCentralGaze", "Name of the gaze tcp that can be used for observing the head motion");
            defineOptionalProperty<std::string>("CommonPlacesLearnerName", "", "Name of the CommonPlacesLearner that will be trained when object instances are updated");
            defineOptionalProperty<bool>("CheckFieldOfView", false, "Check if detected object is inside field of view.");
            defineOptionalProperty<float>("ExistenceCertaintyReductionFactorWhenLocalizationFailed", 0.95f, "Existence certainty will be reduced by this factor when a localization returns without result");
            defineOptionalProperty<float>("CameraOpeningAngle", 30.0f * M_PI / 180.0f, "Very conservative estimation of the camera opening angle, in radians");
            defineOptionalProperty<float>("MaximalObjectDistance", 1500.0f, "Maximal distance an object may have from the camera to be localized");
            defineOptionalProperty<std::string>("RobotNodeNameLeftCamera", "EyeLeftCamera", "Name of the robot node of the left camera");
            defineOptionalProperty<std::string>("RobotNodeNameRightCamera", "EyeRightCamera", "Name of the robot node of the right camera");
            defineOptionalProperty<float>("HeadMotionVelocityLimit", 100, "Maximal velocity of the head up to which localizations are executed. Speed is in mm/s at a the virtual gaze tcp");
            defineOptionalProperty<std::string>("HandNodeNameLeft", "TCP L", "Name of the robot node for the left hand/TCP");
            defineOptionalProperty<std::string>("HandNodeNameRight", "TCP R", "Name of the robot node for the right hand/TCP");
 
            defineOptionalProperty<std::string>("KBMReferenceNodeName", "Cameras", "Name of the robot node on which the KBM resides. Data should be provided wrt. to its coordinate frame (i.e., not be transformed by the robot model which introduces a new source of error).");
            defineOptionalProperty<std::string>("KBMLeftArmNodeSetName", "LeftArm", "Name of the robot node set for the kinematic chain for the left arm.");
            defineOptionalProperty<std::string>("KBMRightArmNodeSetName", "RightArm", "Name of the robot node set for the kinematic chain for the right arm.");
        }
    };
 
 
    /**
     * @class ObjectLocalizationMemoryUpdater
     * @ingroup WorkingMemory
     *
     * The ObjectLocalizationMemoryUpdater triggers the localization of object classes based on the content of
     * the object classes working memory segment and generates object instances in the object instances
     * if objects have been localized.
     *
     * The localization is triggered by adding a LocalizationQuery to the corresponding ObjectClass entity
     * via the ObjectRecognitionWrapper:
     */
    class ARMARXCORE_IMPORT_EXPORT ObjectLocalizationMemoryUpdater :
        virtual public WorkingMemoryUpdater,
        virtual public ObjectLocalizationMemoryUpdaterBase
    {
    public:
        ObjectLocalizationMemoryUpdater();
        void setSegmentNames(std::string classSegmentName, std::string instanceSegmentName);
        void setReferenceFrame(const std::string& referenceFrameName);
 
        armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
        {
            return armarx::PropertyDefinitionsPtr(new ObjectLocalizationMemoryUpdaterProperties(getConfigIdentifier()));
        }
 
        std::string getDefaultName() const override
        {
            return "ObjectLocalizationMemoryUpdater";
        }
 
 
    protected:
        // inherited from managedIceObject
        void onInitComponent() override;
        void onConnectComponent() override;
        void onDisconnectComponent() override;
 
        // inherited from WorkingMemoryListenerInterface
        void reportEntityCreated(const std::string& segmentName, const EntityBasePtr& entity, const Ice::Current& c = Ice::emptyCurrent) override {}
        void reportEntityUpdated(const std::string& segmentName, const EntityBasePtr& entityOld, const EntityBasePtr& entityNew, const Ice::Current& c = Ice::emptyCurrent) override;
        void reportEntityRemoved(const std::string& segmentName, const EntityBasePtr& entity, const Ice::Current& c = Ice::emptyCurrent) override {}
        void reportSnapshotLoaded(const std::string& segmentName, const Ice::Current& c = Ice::emptyCurrent) override { }
        void reportSnapshotCompletelyLoaded(const Ice::Current& c = Ice::emptyCurrent) override { }
        void reportMemoryCleared(const std::string& segmentName, const Ice::Current& c = Ice::emptyCurrent) override { }
 
    private:
        // main scheduler
        void runJobs();
        void scheduleJobs();
 
 
        // asynchronous localization
        void startLocalization(const LocalizationJobContainerPtr& jobContainer);
        void localizationFinished(const Ice::AsyncResultPtr& r);
        /**
         * @brief finishJobContainer stops all LocalizationJob instances associated with \p jobContainer and updates all pending queries.
         * @param jobContainer
         */
        void finishJobContainer(LocalizationJobContainerPtr jobContainer);
 
        // running state handling of recognition methods
        bool isRecognitionMethodRunning(const std::string& recognitionMethod);
        void setRecognitionMethodRunningState(const std::string& recognitionMethod, bool running);
 
        // utility methods
        ObjectLocalizerInterfacePrx getProxyCached(const std::string& recognitionMethod);
        LocalizationQueryList getMissingQueries(const LocalizationQueryList& queriesBase, const LocalizationQueryList& queriesCompare);
        void updateQueries(const LocalizationJobContainerPtr& jobContainer);
        bool checkObjectVisibility(armarx::FramedPositionPtr pos);
        void setMotionModel(ObjectInstancePtr& objectInstance);
        void predictPoses();
 
        // working memory segments
        std::string workingMemoryName;
        std::string classSegmentName;
        std::string instanceSegmentName;
        std::string agentInstancesSegmentName;
        ObjectInstanceMemorySegmentPtr objectInstanceSegment;
        ObjectClassMemorySegmentBasePtr objectClassSegment;
        AgentInstancesSegmentPtr agentInstancesSegment;
        std::string referenceFrameName;
        std::string agentName;
 
        // job scheduling
        armarx::PeriodicTask<ObjectLocalizationMemoryUpdater>::pointer_type localizationTask;
        std::mutex jobsMutex;
        std::mutex predictionMutex;
        std::vector<LocalizationJobPtr> jobs;
 
        // recognition method running state
        std::mutex recognitionMethodStateMutex;
        std::map<std::string, bool> recognitionMethodRunning;
 
        // proxy cache
        std::map<std::string, ObjectLocalizerInterfacePrx> knownObjectProxies;
        armarx::RobotStateComponentInterfacePrx robotStateInterfacePrx;
 
        // observes the head motion speed
        armarx::ObserverInterfacePrx robotStateObserverProxy;
        armarx::DataFieldIdentifierPtr headTCPVelocityDatafieldID;
        float headMotionVelocityLimit;
 
        // CommonPlacesLearner to train
        memoryx::CommonPlacesLearnerInterfacePrx commonPlacesLearnerProxy;
 
        // opening angle of the used cameras
        bool checkFieldOfView;
        float cameraOpeningAngle;
        std::string robotNodeNameLeftCamera, robotNodeNameRightCamera, handNodeNameLeft, handNodeNameRight;
 
        LongtermMemoryInterfacePrx longtermMemoryPrx;
 
    };
}
 
namespace std
{
    template<typename T>
    ostream&
    operator<<(ostream& str, const std::set<T>& set)
    {
        str << "Set(" << set.size() << "):\n";
 
        for (const auto& elem : set)
        {
            str << "\t" << elem << "\n";
        }
 
        return str;
    }
}