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 <ArmarXSimulation/interface/simulator/SimulatorInterface.h>
 
#include <MemoryX/interface/components/PriorKnowledgeInterface.h>
#include <MemoryX/interface/components/WorkingMemoryInterface.h>
#include <MemoryX/interface/observers/ObjectMemoryObserverInterface.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);
    };
} // namespace armarx