CollisionCheckerComponent.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    RobotComponents::ArmarXObjects::CollisionCheckerComponent
 * @author     Adrian Knobloch ( adrian dot knobloch at student dot kit dot edu )
 * @date       2016
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
 
#include <mutex>
 
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/SceneObject.h>
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
 
#include <RobotAPI/interface/core/RobotState.h>
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
#include <RobotAPI/libraries/core/Pose.h>
 
#include <RobotComponents/interface/components/CollisionCheckerInterface.h>
 
#include <MemoryX/core/GridFileManager.h>
#include <MemoryX/interface/components/WorkingMemoryInterface.h>
 
namespace armarx
{
    /**
     * @class CollisionCheckerComponentPropertyDefinitions
     * @brief
     */
    class CollisionCheckerComponentPropertyDefinitions : public armarx::ComponentPropertyDefinitions
    {
    public:
        CollisionCheckerComponentPropertyDefinitions(std::string prefix) :
            armarx::ComponentPropertyDefinitions(prefix)
        {
            defineRequiredProperty<bool>("UseWorkingMemory",
                                         "If true, the WorkingMemory is used to get all "
                                         "SceneObjects. Otherwise a RobotStateComponent is used.");
            defineOptionalProperty<std::string>(
                "WorkingMemoryName", "WorkingMemory", "The name of the WorkingMemory.");
            defineOptionalProperty<std::string>(
                "WorkingMemoryListenerTopicName",
                "WorkingMemoryUpdates",
                "The name of the topic to listen for WorkingMemory updates.");
            defineOptionalProperty<std::string>("RobotStateComponentName",
                                                "RobotStateComponent",
                                                "The name of the RobotStateComponent.");
            defineOptionalProperty<int>(
                "interval", 100, "The interval of time to check for collisions. (in ms)");
            defineOptionalProperty<std::string>(
                "CollisionPairs",
                "[]",
                "A list of tripels including two SceneObjects or SceneObject lists and a distance "
                "from which to warn. Example: [{Armar3:(Ring L J0,Ring L J1),Armar3:(Ring R "
                "J0,Ring R J1),2.0},{Armar3:Index L J0,tableb,3.0}]");
            defineOptionalProperty<std::string>(
                "DistanceListenerTopicName",
                "DistanceListener",
                "The topic name to get information about the distances.");
            defineOptionalProperty<std::string>(
                "CollisionListenerTopicName",
                "CollisionListener",
                "The topic name to get information about collisions.");
            defineOptionalProperty<bool>(
                "UseDebugDrawer",
                "false",
                "If true, the distances are printet using the debug drawer.");
            defineOptionalProperty<std::string>("DebugDrawerTopicName",
                                                "DebugDrawerUpdates",
                                                "The name of the debug drawer topic.");
        }
    };
 
    /**
     * @defgroup Component-CollisionCheckerComponent CollisionCheckerComponent
     * @ingroup RobotComponents-Components
     * A description of the component CollisionCheckerComponent.
     *
     * @class CollisionCheckerComponent
     * @ingroup Component-CollisionCheckerComponent
     * @brief Brief description of class CollisionCheckerComponent.
     *
     * Detailed description of class CollisionCheckerComponent.
     */
    class CollisionCheckerComponent :
        virtual public armarx::Component,
        virtual public armarx::CollisionCheckerInterface
    {
    private:
        typedef struct
        {
            VirtualRobot::SceneObjectSetPtr objects1;
            std::string robotName1;
            std::vector<std::string> nodeNames1;
            bool usesNodeSet1;
            std::string nodeSetName1;
            VirtualRobot::SceneObjectSetPtr objects2;
            std::string robotName2;
            std::vector<std::string> nodeNames2;
            bool usesNodeSet2;
            std::string nodeSetName2;
            double warningDistance;
        } SceneObjectPair;
 
        typedef struct
        {
            VirtualRobot::RobotPtr robot;
            RobotStateComponentInterfacePrx robotStateComponentPrx;
        } RobotPair;
 
    public:
        CollisionCheckerComponent();
 
        /**
         * @see armarx::ManagedIceObject::getDefaultName()
         */
        std::string
        getDefaultName() const override
        {
            return "CollisionCheckerComponent";
        }
 
        void addCollisionPair(const std::string& robotName1,
                              const std::vector<std::string>& nodeNames1,
                              const std::string& robotName2,
                              const std::vector<std::string>& nodeNames2,
                              double warningDistance,
                              const Ice::Current& = Ice::emptyCurrent) override;
 
        void removeCollisionPair(const std::string& robotName1,
                                 const std::vector<std::string>& nodeNames1,
                                 const std::string& robotName2,
                                 const std::vector<std::string>& nodeNames2,
                                 const Ice::Current& = Ice::emptyCurrent) override;
 
        bool hasCollisionPair(const std::string& robotName1,
                              const std::vector<std::string>& nodeNames1,
                              const std::string& robotName2,
                              const std::vector<std::string>& nodeNames2,
                              const Ice::Current& = Ice::emptyCurrent) const override;
 
        void setWarningDistance(const std::string& robotName1,
                                const std::vector<std::string>& nodeNames1,
                                const std::string& robotName2,
                                const std::vector<std::string>& nodeNames2,
                                double warningDistance,
                                const Ice::Current& = Ice::emptyCurrent) override;
        double getWarningDistance(const std::string& robotName1,
                                  const std::vector<std::string>& nodeNames1,
                                  const std::string& robotName2,
                                  const std::vector<std::string>& nodeNames2,
                                  const Ice::Current& = Ice::emptyCurrent) const override;
 
        CollisionPairList
        getAllCollisionPairs(const Ice::Current& = Ice::emptyCurrent) const override;
 
        int getInterval(const Ice::Current& = Ice::emptyCurrent) const override;
        void setInterval(int interval, const Ice::Current& = Ice::emptyCurrent) override;
 
    protected:
        /**
         * @see armarx::ManagedIceObject::onInitComponent()
         */
        void onInitComponent() override;
 
        /**
         * @see armarx::ManagedIceObject::onConnectComponent()
         */
        void onConnectComponent() override;
 
        /**
         * @see armarx::ManagedIceObject::onDisconnectComponent()
         */
        void onDisconnectComponent() override;
 
        /**
         * @see armarx::ManagedIceObject::onExitComponent()
         */
        void onExitComponent() override;
 
        /**
         * @see PropertyUser::createPropertyDefinitions()
         */
        armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
 
    private:
        bool resolveCollisionPair(SceneObjectPair& pair);
        VirtualRobot::SceneObjectPtr getSceneObject(const std::string& robotName,
                                                    const std::string& nodeName);
        bool parseNodeSet(const std::string& setAsString,
                          std::string& robotName,
                          std::vector<std::string>& nodeNames,
                          bool& usesNodeSet,
                          std::string& nodeSetName);
        void reportDistancesAndCollisions();
        VirtualRobot::SceneObjectPtr getSceneObjectFromWorkingMemory(const std::string& name);
        void synchronizeObjectsFromWorkingMemory();
 
        virtual void addCollisionPair(const std::string& robotName1,
                                      const std::vector<std::string>& nodeNames1,
                                      const bool usesNodeSet1,
                                      const std::string& nodeSetName1,
                                      const std::string& robotName2,
                                      const std::vector<std::string>& nodeNames2,
                                      const bool usesNodeSet2,
                                      const std::string& nodeSetName2,
                                      double warningDistance);
 
        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& entityOld,
                                 const ::memoryx::EntityBasePtr& entityNew,
                                 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;
 
        bool useWorkingMemory;
 
        std::string workingMemoryName;
        memoryx::WorkingMemoryInterfacePrx workingMemoryPrx;
        memoryx::ObjectInstanceMemorySegmentBasePrx objectInstancesPrx;
        memoryx::AgentInstancesSegmentBasePrx agentInstancesPrx;
        memoryx::GridFileManagerPtr fileManager;
        std::vector<VirtualRobot::SceneObjectPtr> workingMemorySceneObjects;
        std::map<std::string, armarx::PosePtr> currentPositions;
        std::mutex wmPositionMutex;
 
        std::string robotStateComponentName;
 
        std::vector<RobotPair> robots;
 
        std::vector<SceneObjectPair> sceneObjectPairs;
 
        VirtualRobot::CollisionCheckerPtr collisionChecker;
 
        CollisionListenerPrx collisionListenerPrx;
        DistanceListenerPrx distanceListenerPrx;
 
        int interval;
        PeriodicTask<CollisionCheckerComponent>::pointer_type reportTask;
        mutable std::mutex dataMutex;
 
        bool connected;
        std::shared_mutex connectedMutex;
 
        DebugDrawerInterfacePrx debugDrawerTopicPrx;
        bool useDebugDrawer;
    };
} // namespace armarx