SceneModifier.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    ArmarXSimulation::SceneModifier
* @author     Nikolaus Vahrenkamp
* @date       2015
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#pragma once
 
#include <Eigen/Core>
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/system/ImportExportComponent.h>
#include <ArmarXCore/interface/observers/ConditionHandlerInterface.h>
 
#include <MemoryX/interface/components/WorkingMemoryInterface.h>
#include <MemoryX/interface/components/PriorKnowledgeInterface.h>
#include <MemoryX/interface/observers/ObjectMemoryObserverInterface.h>
 
#include <ArmarXSimulation/interface/simulator/SimulatorInterface.h>
 
namespace armarx
{
 
    class SceneModifierPropertyDefinitions:
        public armarx::ComponentPropertyDefinitions
    {
    public:
        SceneModifierPropertyDefinitions(std::string prefix):
            ComponentPropertyDefinitions(prefix)
        {
            defineRequiredProperty<std::string>("SceneModificationType", "Possible modes are 'addObject', 'removeObject', and 'moveObject'. No object manipulation with 'none'.");
            defineRequiredProperty<std::string>("ObjectClassName", "Name of object class in PriorKnowledge");
            defineRequiredProperty<std::string>("ObjectInstanceName", "Name of object instance in working memory");
            defineOptionalProperty<float>("PositionX", 0.0f, "Position of object (x)");
            defineOptionalProperty<float>("PositionY", 0.0f, "Position of object (y)");
            defineOptionalProperty<float>("PositionZ", 0.0f, "Position of object (z)");
            defineOptionalProperty<float>("OrientationRoll", 0.0f, "Orientation of object (roll)");
            defineOptionalProperty<float>("OrientationPitch", 0.0f, "Orientation of object (pitch)");
            defineOptionalProperty<float>("OrientationYaw", 0.0f, "Orientation of object (yaw)");
            defineOptionalProperty<bool>("StaticObject", false, "Specify if the object is static (fixed in the world) or dynamic (can move in the world)");
            defineOptionalProperty<std::string>("WorkingMemoryName", "WorkingMemory", "Name of the WorkingMemory component that should be used");
            defineOptionalProperty<std::string>("PriorKnowledgeName", "PriorKnowledge", "Name of the PriorKnowledge component that should be used");
            defineOptionalProperty<std::string>("SimulatorName", "Simulator", "Name of the Simulator component that should be used");
 
            // define optional properties:
            for (int i = 0; i < 10; i++)
            {
                std::stringstream ss1;
                ss1 << "SceneModificationType_" << i;
                defineOptionalProperty<std::string>(ss1.str(), "none", "Optional additional object. Possible modes are 'addObject', 'removeObject', and 'moveObject'. No object manipulation with 'none'.");
 
                std::stringstream ss2;
                ss2 << "ObjectClassName_" << i;
                defineOptionalProperty<std::string>(ss2.str(), "", "Optional additional object. Name of object class in PriorKnowledge");
 
                std::stringstream ss3;
                ss3 << "ObjectInstanceName_" << i;
                defineOptionalProperty<std::string>(ss3.str(), "", "Optional additional object. Name of object instance in working memory");
 
                std::stringstream ss4;
                ss4 << "PositionX_" << i;
                defineOptionalProperty<float>(ss4.str(), 0.0f, "Optional additional object. Position of object (x)");
 
                std::stringstream ss5;
                ss5 << "PositionY_" << i;
                defineOptionalProperty<float>(ss5.str(), 0.0f, "Optional additional object. Position of object (y)");
 
                std::stringstream ss6;
                ss6 << "PositionZ_" << i;
                defineOptionalProperty<float>(ss6.str(), 0.0f, "Optional additional object. Position of object (z)");
 
                std::stringstream ss7;
                ss7 << "OrientationRoll_" << i;
                defineOptionalProperty<float>(ss7.str(), 0.0f, "Optional additional object. Orientation of object (roll)");
 
                std::stringstream ss8;
                ss8 << "OrientationPitch_" << i;
                defineOptionalProperty<float>(ss8.str(), 0.0f, "Optional additional object. Orientation of object (pitch)");
 
                std::stringstream ss9;
                ss9 << "OrientationYaw_" << i;
                defineOptionalProperty<float>(ss9.str(), 0.0f, "Optional additional object. Orientation of object (yaw)");
 
                std::stringstream ss10;
                ss10 << "StaticObject_" << i;
                defineOptionalProperty<bool>(ss10.str(), false, "Optional additional object. Specify if the object is static (fixed in the world) or dynamic (can move in the world)");
 
            }
 
            defineOptionalProperty<bool>("LocalizeObject", false, "Start localization of object. This will make the object visible in the WM gui.");
            defineOptionalProperty<int>("LocalizeObject_seconds", 10, "How long should the object be localized");
 
            defineOptionalProperty<std::string>("AttachObjectToRobot.ObjectName", "", "Attach object to robot: object instance name");
            defineOptionalProperty<std::string>("AttachObjectToRobot.RobotName", "", "Attach object to robot: name of robot");
            defineOptionalProperty<std::string>("AttachObjectToRobot.RobotNodeName", "", "Attach object to robot: name of RobotNode (i.e. tcp)");
        }
    };
 
    /**
     * @defgroup Component-SceneModifier SceneModifier
     * @ingroup ArmarXSimulation-Components
 
     * @brief The SceneModifier class is capable of modifying a simulated scene.
     * Possible modifications are adding, moving or removing objects or attaching them to a robot node.
     *
     * This application allows to adjust a default scene in order to add or move objects for a custom scenario setup.
     *
     * @see @ref ArmarXSimulation-HowTos-Customize-Scene
     *
     * @class SceneModifier
     * @ingroup Component-SceneModifier
     *
     */
    class ARMARXCOMPONENT_IMPORT_EXPORT SceneModifier :
        virtual public armarx::Component
    {
    public:
        /**
         * @see PropertyUser::createPropertyDefinitions()
         */
        armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
        {
            return armarx::PropertyDefinitionsPtr(
                       new SceneModifierPropertyDefinitions(
                           getConfigIdentifier()));
        }
 
        // inherited from Component
        std::string getDefaultName() const override
        {
            return "SceneModifier";
        }
        void onInitComponent() override;
        void onExitComponent() override;
        void onConnectComponent() override;
 
        void onDisconnectComponent() override;
    protected:
        Eigen::Matrix4f getGlobalPose(const std::string& suffix);
 
        void addObject(const std::string& suffix);
        void removeObject(const std::string& suffix);
        void moveObject(const std::string& suffix);
 
        memoryx::WorkingMemoryInterfacePrx workingMemoryProxy;
        memoryx::PriorKnowledgeInterfacePrx priorKnowledgeProxy;
        SimulatorInterfacePrx simulatorPrx;
        memoryx::ObjectMemoryObserverInterfacePrx memoryObserver;
        armarx::ConditionHandlerInterfacePrx conditionHandler;
        armarx::ChannelRefBasePtr classChannel1;
        void localizeSingleObject(const std::string& objClass, int sleepS);
        void attachObject(const std::string& objName, const std::string& robotName, const std::string& robotNodeName);
        bool checkForConfigs(const std::string& suffix);
        bool getStatic(const std::string& suffix);
    };
}