ArmarXPhysicsWorldVisualization.h

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2013-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * 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    ArmarXSimulation::ArmarXPhysicsWorldVisualization
 * @author     Nikolaus Vahrenkamp ( vahrenkamp at kit dot edu )
 * @date       2014
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <VirtualRobot/SceneObject.h>
#include <ArmarXSimulation/interface/simulator/SimulatorInterface.h>
 
#include <RobotAPI/libraries/core/Pose.h>
#include <RobotAPI/interface/core/RobotState.h>
#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/Component.h>
 
#include <Inventor/nodes/SoSeparator.h>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <memory>
#include <mutex>
 
namespace armarx
{
    /*!
     * \class ArmarXPhysicsWorldVisualizationPropertyDefinitions
     * \brief
     */
    class ArmarXPhysicsWorldVisualizationPropertyDefinitions:
        public ComponentPropertyDefinitions
    {
    public:
        ArmarXPhysicsWorldVisualizationPropertyDefinitions(std::string prefix):
            ComponentPropertyDefinitions(prefix)
        {
            defineOptionalProperty<std::string>("ReportVisuTopicName", "SimulatorVisuUpdates", "The topic on which the visualization updates are published.");
        }
    };
 
 
    /*!
     * \brief The ArmarXPhysicsWorldVisualization class organizes the Coin3D visualization of the physics scene. It connects to the SimulatorVisuUpdate topic
     * in order to retrieve the visualization stream of the current simulated environment.
     * All scene changes (new objects / removed objects) are handled by checking the current setup with the retrieved one.
     */
    class ArmarXPhysicsWorldVisualization :
        virtual public SimulatorListenerInterface,
        virtual public Component
    {
    public:
 
        ArmarXPhysicsWorldVisualization();
 
        ~ArmarXPhysicsWorldVisualization() override;
 
        void releaseResources();
 
        // inherited from Component
        std::string getDefaultName() const override
        {
            return "ArmarXPhysicsWorldVisualization";
        }
        void onInitComponent() override;
        void onConnectComponent() override;
        void onDisconnectComponent() override;
        void onExitComponent() override;
 
        /*! Inherited from SimulatorListenerInterface.
         * This method is called when the simulator publishes a scene update. For now the complete scene is described in the SyeneVisuData format.
         *
        */
        void reportSceneUpdated(const ::armarx::SceneVisuData& actualData, const ::Ice::Current& = Ice::emptyCurrent) override;
 
        /*!
         * \brief getVisu returns the visualization that is synchronized with ArmarXPhysicsWorld
         * \return
         */
        SoSeparator* getVisualization();
 
        /*!
         * \brief Returns the scene without floor (allows for better viewAll handling)
         * \return
         */
        SoSeparator* getViewableScene();
 
 
        /**
         * @see PropertyUser::createPropertyDefinitions()
         */
        PropertyDefinitionsPtr createPropertyDefinitions() override
        {
            return PropertyDefinitionsPtr(new ArmarXPhysicsWorldVisualizationPropertyDefinitions(
                                              getConfigIdentifier()));
        }
 
 
        /*!
         * \brief buildVisu Creates visualization with increased ref counter.
         * \param o
         * \param visuType
         * \return
         */
        SoNode* buildVisu(VirtualRobot::SceneObjectPtr o, VirtualRobot::SceneObject::VisualizationType visuType = VirtualRobot::SceneObject::Full);
        /*!
         * \brief buildVisu Creates visualization with increased ref counter.
         * \param ro
         * \param visuType
         * \return
         */
        SoNode* buildVisu(VirtualRobot::RobotPtr ro, VirtualRobot::SceneObject::VisualizationType visuType = VirtualRobot::SceneObject::Full);
 
        void addVisualization(VirtualRobot::SceneObjectPtr so, VirtualRobot::SceneObject::VisualizationType visuType = VirtualRobot::SceneObject::Full);
        void addVisualization(VirtualRobot::RobotPtr ro, VirtualRobot::SceneObject::VisualizationType visuType = VirtualRobot::SceneObject::Full);
 
        void removeObjectVisualization(const std::string& name);
        void removeVisualization(VirtualRobot::RobotPtr r);
        void removeRobotVisualization(const std::string& name);
        void removeVisualization(VirtualRobot::SceneObjectPtr so);
 
        void enableVisuType(bool fullModel);
        void addFloorVisualization(Eigen::Vector3f floorPos, Eigen::Vector3f floorUp, float floorExtendMM, std::string floorTexture);
 
        void showBaseCoord(bool enable, float scale = 5.0f);
 
        // If set, this mutex is used to generate locks (for visu updates)
        void setMutex(std::shared_ptr<std::recursive_mutex> const& m);
 
        using RecursiveLockPtr = std::shared_ptr<std::unique_lock<std::recursive_mutex>>;
        RecursiveLockPtr getScopedLock();
        RecursiveLockPtr getScopedDataLock();
 
        /*!
         * \brief Show/hide contacts
         * \param enable
         */
        //void enableContactVisu(bool enable);
 
        /*!
         * \brief Update the contact visualization
         * Draws an arrow at each contact
         * Does nothing if enableContacts is false
         */
        //void updateContactVisualization(std::vector<SimDynamics::DynamicsEngine::DynamicsContactInfo> &c);
 
        /*!
         * \brief synchronizeVisualizationData Updates world poses of all objects of the visualization according to the current state of the physics engine.
         * \return True on success.
         */
        bool synchronizeVisualizationData();
 
        float getSyncVisuTime();
 
 
        VirtualRobot::RobotPtr getRobot(const std::string& name);
        VirtualRobot::SceneObjectPtr getObject(const std::string& name);
 
        std::vector<VirtualRobot::RobotPtr> getRobots() const;
        std::vector<VirtualRobot::SceneObjectPtr> getObjects() const;
 
        /**
         * @brief getSyncTimestamp
         * @return time in microseconds
         */
        Ice::Long getSyncTimestamp() const;
    protected:
 
        bool loadObject(const ObjectVisuData& obj);
        bool loadRobot(const std::string& robotName, const std::string& robotFile, const std::string& project, float scaling, bool colModel);
        bool loadObject(const std::string& objectFile, const std::string& project, const std::string& instanceName);
 
        bool synchronizeSceneObjectPoses(NamePoseMap& objMap, NameValueMap& jvMap, VirtualRobot::SceneObjectPtr currentObjVisu);
 
        SoSeparator* coinVisu;
        SoSeparator* dynamicsVisu;
        SoSeparator* baseCoordVisu;
        //SoSeparator* contactsVisu;
        SoSeparator* sceneViewableSep;
        SoSeparator* floorVisu;
 
        std::map<VirtualRobot::SceneObjectPtr, SoNode*> addedVisualizations;
        std::map<VirtualRobot::RobotPtr, SoNode*> addedRobotVisualizations;
 
        float synchronize2TimeMS; // synchronize data from engine to visu and report object
 
        //bool contactVisuEnabled;
 
        //! mutex to protect access to visualization models
        std::shared_ptr<std::recursive_mutex> mutex;
 
        //! mutex to process SceneData
        std::shared_ptr<std::recursive_mutex> mutexData;
 
        SceneVisuData currentSceneData;
        SceneVisuData lastSceneData;
 
        VirtualRobot::SceneObject::VisualizationType modelType;
 
        void removeFloorVisualization();
        bool synchronizeRobots();
        bool synchronizeFloor();
        bool synchronizeObjects();
        bool createPrimitiveObject(ObjectVisuPrimitivePtr objectPrimitiveData, const std::string& name);
    };
 
    using ArmarXPhysicsWorldVisualizationPtr = IceInternal::Handle<ArmarXPhysicsWorldVisualization>;
}