WorkingMemoryGuiPlugin.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    ArmarX::MemoryX::WorkingMemoryGuiPlugin
* @author    Nikolaus Vahrenkamp (vahrenkamp at kit dot edu)
* @copyright  2012
* @license    http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
 
*/
 
#pragma once
 
/* ArmarX headers */
#include <ArmarXCore/core/Component.h>
 
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
 
#include <RobotAPI/libraries/DebugDrawerConfigWidget/DebugDrawerConfigWidget.h>
#include <RobotAPI/libraries/core/Pose.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
#include <RobotAPI/libraries/widgets/DebugLayerControlWidget.h>
 
#include <MemoryX/components/EntityDrawer/EntityDrawerComponent.h>
#include <MemoryX/gui-plugins/WorkingMemoryGui/ui_WorkingMemoryGuiPlugin.h>
 
/* MemoryX headers */
#include <MemoryX/core/GridFileManager.h>
#include <MemoryX/interface/memorytypes/MemorySegments.h>
#include <MemoryX/interface/workingmemory/AbstractWorkingMemoryInterface.h>
#include <MemoryX/libraries/helpers/VirtualRobotHelpers/SimoxObjectWrapper.h>
#include <MemoryX/libraries/memorytypes/entity/AgentInstance.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectClass.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
 
/** VirtualRobot headers **/
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
#include <VirtualRobot/Visualization/VisualizationFactory.h>
 
/* Qt headers */
#include <QMainWindow>
#include <QMatrix4x4>
#include <QTreeWidget>
#include <QTreeWidgetItem>
 
/* Coin3D/SoQt headers */
#include <atomic>
#include <memory>
 
#include <Eigen/Dense>
 
#include <Inventor/Qt/SoQt.h>
#include <Inventor/Qt/viewers/SoQtExaminerViewer.h>
#include <Inventor/nodes/SoEventCallback.h>
#include <Inventor/nodes/SoMatrixTransform.h>
#include <Inventor/nodes/SoNode.h>
#include <Inventor/nodes/SoSeparator.h>
#include <Inventor/sensors/SoTimerSensor.h>
 
namespace memoryx
{
    class WorkingMemoryConfigDialog;
 
    /** \class WorkingMemoryGuiPlugin
     * \brief The WorkingMemoryGuiPlugin provides a widget that allows you to inspect the current state of the
     * working memory.
     * @see WorkingMemoryController
     */
    class WorkingMemoryGuiPlugin : public armarx::ArmarXGuiPlugin
    {
        Q_OBJECT
        Q_INTERFACES(ArmarXGuiInterface)
        Q_PLUGIN_METADATA(IID "ArmarXGuiInterface/1.00")
    public:
        WorkingMemoryGuiPlugin();
    };
 
    /**
     \page MemoryX-GuiPlugins-WorkingMemoryEditor WorkingMemoryEditor
     \brief This widget allows you to inspect the current state of the working memory.
     \image html WorkingMemoryGui.png
     You can inspect the current working memory.
     The 3D viewer shows the currently known 3D workspace.
     In the table in the center of the widget you find a list of all object and agent instances and their attributes that are currently
     in the working memory.
 
     When you add the widget to the Gui, you need to specify the following parameters:
 
     Parameter Name   | Example Value     | Required?     | Description
      :----------------  | :-------------:   | :-------------- |:--------------------
     Gui Plugin Name | WorkingMemoryGuiPlugin | ? | ?
     Working Memory Name | Working Memory | ? | ?
     Working Memory Updates Topic | WorkingMemoryUpdates | ? | ?
     Object Instances Segment Name | objectInstances | ? | The name of the segment in memory containing the object instances.
     Agent Instances Segment Name | agentInstances | ? | The name of the segment in memory containing the agent instances.
     World State Segment Name | worldState | ? | The name of the world state.
     Extend datapaths with additional ArmarX packages (comma-separated) | Armar 3, Armar 4 | ? | ?
 
     \note In order to use this widget there must be working memory available.
 
    An example scenario for viewing and editing the WorkingMemory
 
    \verbatim
    ${MemoryX_DIR}/scenarios/WorkingMemoryGui
    \endverbatim
 
    The collection db.snapshots of memdb contains all snapshots such as
    \li MobileKitchen_mm (millimeter)
    \li KitchenKK
 
    Those snapshots contain a list with segment names.
    Each entry in this list refers to a collection name
    which contains all objects in the scene for this segment.
    An object entry contains:
 
    \li object name
    \li object class
    \li object pose
    \li object certainty
 
    The WorkingMemoryExample contains several tests which load one Snapshot into WorkingMemory
    and operates on it.
    It is located in
    \verbatim
    ${MemoryX_DIR}/scenarios/WorkingMemoryExample
    \endverbatim
 
    The configuration file WorkingMemory.cfg contains important parameters such as
 
    \li "DatabaseName": name of the main database (should be memdb in our case)
    \li "KalmanFilter": object positions can be fused with a filter
    \li "WMExample.SnapshotName": name of the snapshot to load
    \li "WMExample.TestToRun": name of the test to execute
    \li "WMExample.CommonPlacesSnapshot": name of the CommonPlaces snapshot which should be used
    \li "MemoryX.PriorKnowledge.ClassCollections": which collections should be used for PriorKnowledge
 
    After the configuration files are in the wanted state the Gui can be started with
    \verbatim
    ${MemoryX_DIR}/scenarios/WorkingMemoryExample/startGui.sh
    \endverbatim
 
    After the Gui has loaded click on the  "addWidget()" menu and then select "WorkingMemoryGui".
 
    \li ->setClass("cup", ..)
 
     WorkingMemoryEditor API Documentation \ref WorkingMemoryController
     \see WorkingMemoryGuiPlugin
     */
 
    /**
     * @class WorkingMemoryController
     * \brief This widget allows you to inspect the current state of the
     * working memory.
     * \see WorkingMemoryGuiPlugin
     */
    class WorkingMemoryController :
        public armarx::ArmarXComponentWidgetControllerTemplate<WorkingMemoryController>,
        public WorkingMemoryListenerInterface
    {
        Q_OBJECT
    public:
        WorkingMemoryController();
        ~WorkingMemoryController() override;
 
        /* Inherited from Component */
        void onInitComponent() override;
        void onConnectComponent() override;
        void onDisconnectComponent() override;
        void onExitComponent() override;
 
        /* Inherited from ArmarXWidget. */
        static QString
        GetWidgetName()
        {
            return "MemoryX.WorkingMemoryGui";
        }
 
        static QIcon
        GetWidgetIcon()
        {
            return QIcon("://icons/brain.svg");
        }
 
        void loadSettings(QSettings* settings) override;
        void saveSettings(QSettings* settings) override;
        QPointer<QDialog> getConfigDialog(QWidget* parent = nullptr) override;
 
        void configured() override;
        SoNode* getScene() override;
        QPointer<QDialog> getSceneConfigDialog(QWidget* parent = nullptr) override;
 
        /* Inherited from WorkingMemoryListener. */
        void reportEntityCreated(const std::string& segmentName,
                                 const ::memoryx::EntityBasePtr& entity,
                                 const ::Ice::Current& = Ice::emptyCurrent) override;
        void reportEntityUpdated(const std::string& segmentName,
                                 const ::memoryx::EntityBasePtr& entityBaseOld,
                                 const ::memoryx::EntityBasePtr& entityBaseNew,
                                 const ::Ice::Current& = Ice::emptyCurrent) override;
        void reportEntityRemoved(const std::string& segmentName,
                                 const ::memoryx::EntityBasePtr& entity,
                                 const ::Ice::Current& = Ice::emptyCurrent) override;
        void reportSnapshotLoaded(const std::string& segmentName,
                                  const ::Ice::Current& = Ice::emptyCurrent) override;
        void reportSnapshotCompletelyLoaded(const Ice::Current& c = Ice::emptyCurrent) override;
        void reportMemoryCleared(const std::string& segmentName,
                                 const ::Ice::Current& = Ice::emptyCurrent) override;
 
        // overwrite setMutex, so thatwe can inform the debugdrawer
        void setMutex3D(RecursiveMutexPtr const& mutex3D) override;
    signals:
 
        /* Emitted when: entities are fetched from the working memory, an entity is removed from the working memory or the working memory has been cleared. */
        void signalRebuildVisualization();
        /* Emitted when: an entity is created/updated in the working memory. */
        void entityChanged(memoryx::EntityBasePtr entity, QString entityType, bool isNew);
 
        void signalAgentsRemoved();
        void signalObjectRemoved();
 
        void signalRefetchEntities();
 
        void signalClearEntities(const std::string& segmentName);
 
        void signalRemoveTabs();
 
    public slots:
 
 
        /*!
         * Fetches all entities stored in the objectInstances, agentInstances and worldState segments in the working memory.
         */
        void refetchEntitiesFromMemory();
 
        void startTimerInGUIThread();
 
        /*!
         * Shows information about a double-clicked item in the tree widget.
         * \param item Double-clicked item.
         * \param column Double-clicked column.
         */
        void treeItemDoubleClicked(QTreeWidgetItem* item, int column);
        /*!
         * Rebuilds the visualisation for all entities.
         */
        void rebuildVisualization();
        /*!
         * Updates the visualisation for an entity.
         * \param entityID Entity ID.
         * \param entityType Entity type. Supported options are: objectInstances, agentInstances and worldState.
         * \param \isNew TRUE if it is a new entity.
         */
        void updateEntityVisu(memoryx::EntityBasePtr entity, QString entityType, bool isNew);
        /*!
         * Setups visualisation for all entities stored in currentEntities if the uncertainty visualisation is checked.
         * \param isChecked TRUE if the uncertainty visualisation option is checked.
         */
        void uncertaintyVisuToggled(bool isChecked);
        void existenceCertaintyToggled(bool isChecked);
        /*!
         * Setups the visualisation for all entities stored in currentEntities.
         */
        void setupVisualization();
        /*!
         * Updates the item corresponding to an entity in the tree widget.
         * \param entityId Entity ID.
         * \param entityType Entity type. Supported options are: objectInstances, agentInstances and worldState.
         * \param isNew TRUE if it is a new entity.
         */
        void updateEntityTree(memoryx::EntityBasePtr entity, QString entityType, bool isNew);
 
        /*!
         * \brief clearWorkingMemory deletes the current contents of WorkingMemory
         */
        void clearWorkingMemory();
 
        /*!
         * \brief Sets the visibility for debugDrawer's visualization layers
         * \param show Whether they should be visible.
         */
        void showVisuLayers(bool show);
 
        /*!
         * \brief Sets the visibility for agents
         * \param show Whether they should be visible.
         */
        void showAgents(bool show);
 
 
        void showPlane(bool show);
 
        /*!
         * \brief agentsRemoved One or multiple agent(s) have been removed from memory. This method updates all Qt related things (visu/gui).
         */
        void agentsRemoved();
 
        /*!
         * \brief objectsRemoved One or multiple object(s) have been removed from memory. This method updates all Qt related things (visu/gui).
         */
        void objectsRemoved();
 
        /*!
         * Removes the item corresponding to an entity from the tree widget.
         * \param entityId Entity ID.
         * \param entityType Entity type.
         */
        void removeEntityInTree(const std::string entityId, const std::string entityType);
 
 
        /*!
         * Clears all entity data in the current agents and current entities maps. All cleared entities/agents are stored to the deletedAgents/deletedEntities map.
         */
        void clearEntitiesData();
        /*!
         * Clears all visu data of deletedAgents/deletedEntites all items in the tree widget.
         */
        void clearEntitiesVisu(const std::string& segmentName = "");
 
    protected:
        void exportToVRML();
 
        /*!
         * Updates joint values for all agents.
         * \param event QTimerEvent.
         */
        void timerEvent(QTimerEvent*) override;
 
        /*!
         * Connects signals to the slots.
         */
        void connectSlots();
 
        Ui::WorkingMemoryGuiPlugin ui;
 
        bool verbose;
 
        struct RobotDefinition
        {
            VirtualRobot::RobotPtr robot; ///< Robot used for visualisation purposes.
            SoNode* visualization = nullptr; ///< Robot visualisation.
            armarx::RobotStateComponentInterfacePrx robotStateComponent;
 
            /// Time when timestamped robot was updated.
            IceUtil::Time timestamp = IceUtil::Time::seconds(0);
            VirtualRobot::RobotPtr timestampedRobot;
        };
 
        struct ObjectDefinition
        {
            EntityWrappers::SimoxObjectWrapperPtr simoxWrapper;
            ObjectInstancePtr obj;
            VirtualRobot::ManipulationObjectPtr
                manipObj; ///< Object used for visualisation purposes.
            SoNode* visualization = nullptr; ///< Object visualisation.
            VirtualRobot::CoinVisualizationPtr cv;
        };
 
        struct SegmentTab
        {
            int tabIndex;
            QTreeWidget* tree;
            QWidget* tab;
        };
 
        /* Maps entityId with data if the entity is of type ObjectInstance. */
        using EntityEntryMap = std::map<std::string, ObjectDefinition>;
 
        std::map<std::string, struct SegmentTab> segmentTabs; /*!< Current segment tabs. */
        memoryx::NameList segmentNames; /*!< Name of current segments. */
 
        EntityEntryMap currentEntities; /*!< Current entities of type ObjectInstance. */
        EntityEntryMap
            deletedEntities; /*!< Entities that have been deleted (during last loop) of type ObjectInstance. */
 
        bool show3DViewer;
        std::atomic<bool> showObjInstanceUncertainties;
 
        SoSeparator* rootVisu; /*!< Root visualisation node. */
        SoSeparator* objectsVisu; /*!< object visualisation node. */
        SoSeparator* agentsVisu; /*!< agent visualisation node. */
        SoSeparator* debugLayerVisu;
        SoSeparator* planeVisu;
 
        DebugLayerControlWidget* debugLayerControlWidget;
        /*!
         * Updates an entity of type ObjectInstance in the current entities map.
         * obj Pointer to the entity instance.
         */
        void updateObjectData(const ObjectInstancePtr obj);
 
        /*!
         * Creates visualisation for entities of type ObjectInstance.
         * \param objWrapper Simox wrapper of the entity.
         */
        void createObjectVisualization(ObjectDefinition& o);
 
        /*!
         * Updates visualisation for entities of type AgentInstance.
         * \param objWrapper Simox wrapper of the entity.
         */
        void updateVisualization(const EntityWrappers::SimoxObjectWrapperPtr objWrapper);
 
        /*!
         * Setups visualisation for entities of type ObjectInstance.
         * \param objWrapper Simox wrapper of the entity.
         */
        void setupEntityVisu(const ObjectDefinition& objWrapper);
 
        /*!
         * Updates an entity of type AgentInstance in the agents map.
         * \param entity Pointer to the entity instance.
         * \return True on success.
         */
        bool addAgent(const AgentInstancePtr agent);
        /*!
         * Creates visualisation for entities of type AgentInstance.
         * \param entityId Entity ID.
         */
        void createAgentVisualisation(const std::string entityId);
        /*!
         * Updates visualisation for entities of type AgentInstance.
         * \param entityId Entity ID.
         */
        void updateAgentVisualisation(const std::string entityId);
        void updateAgentVisualisation(const EntityBasePtr entity);
 
 
        /*!
         * At tab for a given segment name if the tab does not already exist.
         * \param segmentName segment name.
         * \return index of new tab
         */
        int addTab(std::string segmentName);
 
        /*!
         * Removes all segment-tabs.
         */
        void removeTabs();
 
        /*!
         * Removes a single tab for a given segment name.
         * \param segmentName segment name.
         */
        void removeTab(std::string segmentName);
 
        void processPackages(const std::string& packages);
 
        /*!
         * \brief createEntity Updates currentEntities or currentAgents with data.
         * \param entity
         * \param entityType
         * \return
         */
 
        bool createEntity(memoryx::EntityBasePtr entity, const std::string& entityType);
 
    private:
        std::recursive_mutex mutexEntities;
        std::map<std::string, std::pair<long, armarx::FramedPosePtr>>
            lastUpdated; // a map containing the timestamp (us) and the pose when the entity got last updated.
 
        AbstractWorkingMemoryInterfacePrx memoryPrx;
        ObjectInstanceMemorySegmentBasePrx objectInstancesPrx;
        AgentInstancesSegmentBasePrx agentInstancesProxy;
        WorldStateSegmentBasePrx worldStateProxy;
        PersistentObjectClassSegmentBasePrx classesSegmentPrx;
        PriorKnowledgeInterfacePrx priorKnowledgePrx;
 
        GridFileManagerPtr fileManager;
 
        QPointer<WorkingMemoryConfigDialog> dialog;
 
        std::string settings_priorMemoryName;
        std::string settings_workingMemoryName;
        std::string settings_workingMemoryUpdatesTopic;
        std::string settings_objectInstancesSegmentName;
        std::string settings_agentInstancesSegmentName;
        std::string settings_worldStateSegmentName;
        std::string settings_packages;
 
        int robotUpdateTimerId;
 
 
        bool visuSetting_transparentExistanceCertatinty;
        bool visuSetting_showObjectModels;
 
        /* Maps entityId with data for entities of type AgentInstance. */
        using AgentEntryMap = std::map<std::string, RobotDefinition>;
 
        void updateAgent(const RobotDefinition& agent);
 
        AgentEntryMap currentAgents; /*!< Current entities of type AgentInstance. */
        AgentEntryMap
            deletedAgents; /*!< Deleted entities of type AgentInstance (removed during last loop, visu needs to be updated). */
 
        memoryx::EntityDrawerComponentPtr debugDrawer;
        QToolBar* customToolbar;
 
        QPointer<QDialog> sceneConfigDialog;
        QPointer<armarx::DebugDrawerConfigWidget> debugDrawerConfigWidget;
 
        // ArmarXWidgetController interface
    public:
        QPointer<QWidget> getCustomTitlebarWidget(QWidget* parent) override;
        void convertObjectInstanceToGlobalPose(ObjectInstance& obj);
    };
 
    using WorkingMemoryGuiPluginPtr = std::shared_ptr<WorkingMemoryController>;
} // namespace memoryx