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/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/system/ImportExport.h>
#include <ArmarXCore/observers/variant/DataFieldIdentifier.h>
 
#include <RobotAPI/interface/core/RobotState.h>
#include <RobotAPI/interface/units/TCPControlUnit.h>
 
#include <MemoryX/components/CommonPlacesLearner/CommonPlacesLearner.h>
#include <MemoryX/libraries/helpers/ObjectRecognitionHelpers/LocalizationJob.h>
#include <MemoryX/libraries/helpers/ObjectRecognitionHelpers/LocalizationJobContainer.h>
#include <MemoryX/libraries/helpers/ObjectRecognitionHelpers/LocalizationQuery.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectClass.h>
#include <MemoryX/libraries/memorytypes/segment/AgentInstancesSegment.h>
#include <MemoryX/libraries/memorytypes/segment/ObjectInstanceMemorySegment/ObjectInstanceMemorySegment.h>
#include <MemoryX/libraries/workingmemory/updater/WorkingMemoryUpdater.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 memoryx
 
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;
    }
} // namespace std