CollisionCheckerComponent.cpp

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
 */
 
#include "CollisionCheckerComponent.h"
 
#include <stdlib.h>
#include <cmath>
#include <sstream>
#include <iterator>
 
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
 
#include <MemoryX/libraries/memorytypes/entity/AgentInstance.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectClass.h>
#include <MemoryX/libraries/helpers/VirtualRobotHelpers/SimoxObjectWrapper.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
 
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
#include <VirtualRobot/CollisionDetection/CollisionChecker.h>
 
#include <SimoxUtility/algorithm/string/string_tools.h>
 
#include <boost/regex.hpp>
 
using namespace armarx;
 
CollisionCheckerComponent::CollisionCheckerComponent() :
    connected(false)
{
 
}
 
void CollisionCheckerComponent::onInitComponent()
{
    interval = getProperty<int>("interval").getValue();
 
    useWorkingMemory = getProperty<bool>("UseWorkingMemory").getValue();
    if (useWorkingMemory)
    {
        workingMemoryName = getProperty<std::string>("WorkingMemoryName").getValue();
        ARMARX_INFO << "Using WorkingMemory \"" << workingMemoryName << "\" to get SceneObjects." << std::endl;
        usingProxy(workingMemoryName);
        usingTopic(getProperty<std::string>("WorkingMemoryListenerTopicName").getValue());
    }
    else
    {
        robotStateComponentName = getProperty<std::string>("RobotStateComponentName").getValue();
        ARMARX_INFO << "Using RobotStateComponent \"" << robotStateComponentName << "\" to get SceneObjects." << std::endl;
        usingProxy(robotStateComponentName);
    }
 
    std::string collisionPairsString = getProperty<std::string>("CollisionPairs").getValue();
    boost::regex regexCollisionPair("[\\[\\{\\(](([^:,\\[\\{\\(\\]\\}\\)]+:)?([^,\\[\\{\\(\\]\\}\\)]+,|[\\[\\{\\(]([^,\\[\\{\\(\\]\\}\\)]+,)*[^,\\[\\{\\(\\]\\}\\)]+[\\]\\}\\)],)){2}[0-9]*[.]?[0-9]*[\\]\\}\\)]");
 
    boost::sregex_token_iterator iterCollisionPair(collisionPairsString.begin(), collisionPairsString.end(), regexCollisionPair, 0);
    boost::sregex_token_iterator end;
 
    for (; iterCollisionPair != end; ++iterCollisionPair)
    {
        std::string collisionPairString = *iterCollisionPair;
        while ((collisionPairString[0] == '{' && collisionPairString[collisionPairString.size() - 1] == '}') ||
               (collisionPairString[0] == '(' && collisionPairString[collisionPairString.size() - 1] == ')') ||
               (collisionPairString[0] == '[' && collisionPairString[collisionPairString.size() - 1] == ']'))
        {
            collisionPairString = collisionPairString.substr(1, collisionPairString.size() - 2);
        }
 
        if ((collisionPairString[0] == '{' || collisionPairString[0] == '(' || collisionPairString[0] == '[') &&
            (collisionPairString[collisionPairString.size() - 1] == '}' || collisionPairString[collisionPairString.size() - 1] == ')' || collisionPairString[collisionPairString.size() - 1] == ']'))
        {
            ARMARX_ERROR << "Could not parse collision pair: " << *iterCollisionPair << ". Ignoring..." << std::endl;
            continue;
        }
 
        boost::regex regex("([^:,\\[\\{\\(\\]\\}\\)]+:)?([\\{\\[\\(][^\\{\\[\\(\\]\\}\\)]+[\\]\\}\\)]|[^\\{\\[\\(\\]\\}\\),]+)");
 
        boost::sregex_token_iterator iter(collisionPairString.begin(), collisionPairString.end(), regex, 0);
        boost::sregex_token_iterator end;
 
        ARMARX_CHECK_EXPRESSION(iter != end);
 
        std::string robotName1;
        std::vector<std::string> nodeNames1;
        bool usesNodeSet1;
        std::string nodeSetName1;
        parseNodeSet(*iter, robotName1, nodeNames1, usesNodeSet1, nodeSetName1);
 
        ++iter;
        ARMARX_CHECK_EXPRESSION(iter != end);
 
        std::string robotName2;
        std::vector<std::string> nodeNames2;
        bool usesNodeSet2;
        std::string nodeSetName2;
        parseNodeSet(*iter, robotName2, nodeNames2, usesNodeSet2, nodeSetName2);
 
        ++iter;
        ARMARX_CHECK_EXPRESSION(iter != end);
 
        addCollisionPair(robotName1, nodeNames1, usesNodeSet1, nodeSetName1, robotName2, nodeNames2, usesNodeSet2, nodeSetName2, std::atof(((std::string)*iter).c_str()));
 
        ARMARX_CHECK_EXPRESSION(++iter == end);
    }
 
    collisionChecker = VirtualRobot::CollisionChecker::getGlobalCollisionChecker();
 
    offeringTopic(getProperty<std::string>("CollisionListenerTopicName").getValue());
    offeringTopic(getProperty<std::string>("DistanceListenerTopicName").getValue());
 
    useDebugDrawer = getProperty<bool>("UseDebugDrawer").getValue();
}
 
 
void CollisionCheckerComponent::onConnectComponent()
{
    {
        std::scoped_lock lockData(dataMutex);
 
        robots.clear();
 
        if (useWorkingMemory)
        {
            workingMemoryPrx = getProxy<memoryx::WorkingMemoryInterfacePrx>(workingMemoryName);
            objectInstancesPrx = workingMemoryPrx->getObjectInstancesSegment();
            agentInstancesPrx = workingMemoryPrx->getAgentInstancesSegment();
            fileManager.reset(new memoryx::GridFileManager(workingMemoryPrx->getCommonStorage()));
            for (memoryx::AgentInstanceBasePtr& agent : agentInstancesPrx->getAllAgentInstances())
            {
                RobotStateComponentInterfacePrx robotStateComponentPrx = agent->getSharedRobot()->getRobotStateComponent();
                VirtualRobot::RobotPtr robot = armarx::RemoteRobot::createLocalCloneFromFile(robotStateComponentPrx);
 
                robots.push_back({robot, robotStateComponentPrx});
            }
        }
        else
        {
            RobotStateComponentInterfacePrx robotStateComponentPrx = getProxy<RobotStateComponentInterfacePrx>(robotStateComponentName);
 
            VirtualRobot::RobotPtr robot = armarx::RemoteRobot::createLocalCloneFromFile(robotStateComponentPrx);
 
            RobotPair r = {robot, robotStateComponentPrx};
            robots.push_back(r);
        }
 
        {
            std::unique_lock lock(connectedMutex);
            connected = true;
        }
 
 
        for (SceneObjectPair& pair : sceneObjectPairs)
        {
            if (!resolveCollisionPair(pair))
            {
                std::ostringstream nodeNames1Str;
                std::copy(pair.nodeNames1.begin(), pair.nodeNames1.end() - 1, std::ostream_iterator<std::string>(nodeNames1Str, ","));
                nodeNames1Str << pair.nodeNames1.back();
 
                std::ostringstream nodeNames2Str;
                std::copy(pair.nodeNames2.begin(), pair.nodeNames2.end() - 1, std::ostream_iterator<std::string>(nodeNames2Str, ","));
                nodeNames2Str << pair.nodeNames2.back();
                ARMARX_WARNING << "Could not resolve collision pair: {" << pair.robotName1 << (pair.robotName1 == "" ? "" : ":") << "(" << nodeNames1Str.str() << ")," << pair.robotName2 << (pair.robotName2 == "" ? "" : ":") << "(" << nodeNames2Str.str() << ")," << pair.warningDistance << "}" << std::endl;
            }
        }
    }
 
    collisionListenerPrx = getTopic<CollisionListenerPrx>(getProperty<std::string>("CollisionListenerTopicName").getValue());
    distanceListenerPrx = getTopic<DistanceListenerPrx>(getProperty<std::string>("DistanceListenerTopicName").getValue());
 
    if (!reportTask)
    {
        reportTask = new PeriodicTask<CollisionCheckerComponent>(this, &CollisionCheckerComponent::reportDistancesAndCollisions, interval, false, "ReportCollisionsTask");
    }
 
    reportTask->start();
 
    if (useDebugDrawer)
    {
        debugDrawerTopicPrx = getTopic<DebugDrawerInterfacePrx>(getProperty<std::string>("DebugDrawerTopicName").getValue());
 
        if (!debugDrawerTopicPrx)
        {
            ARMARX_ERROR << "Failed to obtain debug drawer proxy." << std::endl;
            useDebugDrawer = false;
        }
    }
}
 
 
void CollisionCheckerComponent::onDisconnectComponent()
{
    {
        std::unique_lock lock(connectedMutex);
        connected = false;
    }
    reportTask->stop();
 
    std::scoped_lock lockData(dataMutex);
    for (SceneObjectPair& pair : sceneObjectPairs)
    {
        pair.objects1 = VirtualRobot::SceneObjectSetPtr();
        pair.objects2 = VirtualRobot::SceneObjectSetPtr();
    }
    robots.clear();
 
    if (useDebugDrawer && debugDrawerTopicPrx)
    {
        debugDrawerTopicPrx->clearLayer("distanceVisu");
        debugDrawerTopicPrx->removeLayer("distanceVisu");
    }
}
 
 
void CollisionCheckerComponent::onExitComponent()
{
 
}
 
armarx::PropertyDefinitionsPtr CollisionCheckerComponent::createPropertyDefinitions()
{
    return armarx::PropertyDefinitionsPtr(new CollisionCheckerComponentPropertyDefinitions(
            getConfigIdentifier()));
}
 
bool armarx::CollisionCheckerComponent::resolveCollisionPair(SceneObjectPair& pair)
{
    std::shared_lock lockConnected(connectedMutex);
    if (!connected)
    {
        return false;
    }
 
    if (pair.usesNodeSet1 && pair.nodeSetName1 != "")
    {
        pair.nodeNames1.clear();
        if (pair.robotName1 != "")
        {
            for (RobotPair& robotPair : robots)
            {
                if (robotPair.robot->getName() == pair.robotName1)
                {
                    VirtualRobot::SceneObjectSetPtr set = robotPair.robot->getRobotNodeSet(pair.nodeSetName1);
                    if (set)
                    {
                        for (VirtualRobot::SceneObjectPtr so : set->getSceneObjects())
                        {
                            pair.nodeNames1.push_back(so->getName());
                        }
                    }
                    else
                    {
                        ARMARX_WARNING << "Robot \"" << pair.robotName1 << "\" has no set with name \"" << pair.nodeSetName1 << "\"" << std::endl;
                    }
                    break;
                }
            }
        }
        else
        {
            ARMARX_WARNING << "No robot name given for RobotNodeSet \"" << pair.nodeSetName1 << "\"" << std::endl;
        }
    }
 
    VirtualRobot::SceneObjectSetPtr objects1(new VirtualRobot::SceneObjectSet());
    for (std::string& nodeName : pair.nodeNames1)
    {
        VirtualRobot::SceneObjectPtr sc = getSceneObject(pair.robotName1, nodeName);
        if (!sc)
        {
            ARMARX_DEBUG << "failed to get object: \"" << pair.robotName1 << "\" : \"" << nodeName << "\"" << std::endl;
            return false;
        }
        if (!sc->getCollisionModel())
        {
            ARMARX_WARNING << pair.robotName1 << (pair.robotName1 == "" ? "" : ".") << nodeName << " does not have a collision model. Ignoring..." << std::endl;
            continue;
        }
        objects1->addSceneObject(sc);
    }
 
    if (pair.usesNodeSet2 && pair.nodeSetName2 != "")
    {
        pair.nodeNames2.clear();
        if (pair.robotName2 != "")
        {
            for (RobotPair& robotPair : robots)
            {
                if (robotPair.robot->getName() == pair.robotName2)
                {
                    VirtualRobot::SceneObjectSetPtr set = robotPair.robot->getRobotNodeSet(pair.nodeSetName2);
                    if (set)
                    {
                        for (VirtualRobot::SceneObjectPtr so : set->getSceneObjects())
                        {
                            pair.nodeNames2.push_back(so->getName());
                        }
                    }
                    else
                    {
                        ARMARX_WARNING << "Robot \"" << pair.robotName2 << "\" has no set with name \"" << pair.nodeSetName2 << "\"" << std::endl;
                    }
                    break;
                }
            }
        }
        else
        {
            ARMARX_WARNING << "No robot name given for RobotNodeSet \"" << pair.nodeSetName2 << "\"" << std::endl;
        }
    }
 
    VirtualRobot::SceneObjectSetPtr objects2(new VirtualRobot::SceneObjectSet());
    for (std::string& nodeName : pair.nodeNames2)
    {
        VirtualRobot::SceneObjectPtr sc = getSceneObject(pair.robotName2, nodeName);
        if (!sc)
        {
            ARMARX_DEBUG << "failed to get object: \"" << pair.robotName2 << "\" : \"" << nodeName << "\"" << std::endl;
            return false;
        }
        if (!sc->getCollisionModel())
        {
            ARMARX_WARNING << pair.robotName2 << (pair.robotName2 == "" ? "" : ".") << nodeName << " does not have a collision model. Ignoring..." << std::endl;
            continue;
        }
        objects2->addSceneObject(sc);
    }
 
    if (!objects1 || !objects2 || objects1->getSize() == 0 || objects2->getSize() == 0)
    {
        return false;
    }
 
    pair.objects1 = objects1;
    pair.objects2 = objects2;
    return true;
}
 
VirtualRobot::SceneObjectPtr armarx::CollisionCheckerComponent::getSceneObject(const std::string& robotName, const std::string& nodeName)
{
    if (robotName.size() == 0 && useWorkingMemory)
    {
        ARMARX_DEBUG << "Get object from WorkingMemory: " << nodeName << std::endl;
        return getSceneObjectFromWorkingMemory(nodeName);
    }
    else if (robotName.size() == 0) //&& !useWorkingMemory
    {
        //get RobotNode from first robot (should be only one)
        ARMARX_DEBUG << "Get object from first robot: " << nodeName << std::endl;
        return robots[0].robot->getRobotNode(nodeName);
    }
    else
    {
        ARMARX_DEBUG << "Searching for matching robot: " << robotName << std::endl;
        for (RobotPair& r : robots)
        {
            if (r.robot->getName() == robotName)
            {
                ARMARX_DEBUG << "Get object from robot: " << nodeName << std::endl;
                return r.robot->getRobotNode(nodeName);
            }
            else
            {
                ARMARX_DEBUG << r.robot->getName() << " != " << robotName << std::endl;
            }
        }
    }
 
    return VirtualRobot::SceneObjectPtr();
}
 
bool armarx::CollisionCheckerComponent::parseNodeSet(const std::string& setAsString, std::string& robotName, std::vector<std::string>& nodeNames, bool& usesNodeSet, std::string& nodeSetName)
{
    std::vector<std::string> split = simox::alg::split(setAsString, ":");
 
    std::string nodeNamesString;
    switch (split.size())
    {
        case 1:
            robotName = "";
            nodeNamesString = split[0];
            break;
        case 2:
            robotName = split[0];
            nodeNamesString = split[1];
            break;
        default:
            return false;
    }
 
    if (nodeNamesString[0] != '{' && nodeNamesString[0] != '(' && nodeNamesString[0] != '[')
    {
        usesNodeSet = true;
        nodeSetName = nodeNamesString;
        nodeNames.clear();
    }
    else
    {
        usesNodeSet = false;
        nodeSetName = "";
        while ((nodeNamesString[0] == '{' && nodeNamesString[nodeNamesString.size() - 1] == '}') ||
               (nodeNamesString[0] == '(' && nodeNamesString[nodeNamesString.size() - 1] == ')') ||
               (nodeNamesString[0] == '[' && nodeNamesString[nodeNamesString.size() - 1] == ']'))
        {
            nodeNamesString = nodeNamesString.substr(1, nodeNamesString.size() - 2);
        }
 
        ARMARX_CHECK_EXPRESSION(nodeNamesString.size() > 0);
 
        nodeNames = simox::alg::split(nodeNamesString, ",");
    }
    return true;
}
 
VirtualRobot::SceneObjectPtr armarx::CollisionCheckerComponent::getSceneObjectFromWorkingMemory(const std::string& name)
{
    for (VirtualRobot::SceneObjectPtr& sc : workingMemorySceneObjects)
    {
        if (sc->getName() == name)
        {
            return sc;
        }
    }
    const memoryx::EntityBasePtr entityBase = objectInstancesPrx->getEntityByName(name);
    const memoryx::ObjectInstanceBasePtr object = memoryx::ObjectInstancePtr::dynamicCast(entityBase);
 
    if (!object)
    {
        ARMARX_ERROR << "Could not get object with name " << name << std::endl;
        return VirtualRobot::SceneObjectPtr();
    }
 
    const std::string className = object->getMostProbableClass();
 
    memoryx::ObjectClassList classes = workingMemoryPrx->getPriorKnowledge()->getObjectClassesSegment()->getClassWithSubclasses(className);
 
    if (!classes.size())
    {
        ARMARX_INFO_S << "No classes for most probable class '" << className << "' of object '" << object->getName() << "' with name " << name;
        return VirtualRobot::SceneObjectPtr();
    }
 
    memoryx::ObjectClassPtr objectClass = memoryx::ObjectClassPtr::dynamicCast(classes.at(0));
    const PosePtr objectPose = armarx::PosePtr::dynamicCast(armarx::PoseBasePtr {new armarx::Pose{object->getPositionBase(), object->getOrientationBase()}});
 
    if (!objectClass)
    {
        ARMARX_ERROR << "Can't use object class with ice id " << classes.at(0)->ice_id() << std::endl;
        return VirtualRobot::SceneObjectPtr();
    }
 
    memoryx::EntityWrappers::SimoxObjectWrapperPtr sw = objectClass->addWrapper(new memoryx::EntityWrappers::SimoxObjectWrapper(fileManager));
    VirtualRobot::ManipulationObjectPtr orgMo = sw->getManipulationObject();
    ARMARX_CHECK_EXPRESSION(orgMo);
    std::string moName = orgMo->getName();
    VirtualRobot::ManipulationObjectPtr mo = orgMo->clone(moName);
 
    //move the object to the given position
    if (!objectPose)
    {
        ARMARX_ERROR << "Can't convert pose of " << objectClass->getName() << " to armarx::Pose." << std::endl;
        return VirtualRobot::SceneObjectPtr();
    }
    mo->setGlobalPose(objectPose->toEigen());
    mo->setName(name); //make shure the name of the scene object is equal to the requested name
 
    workingMemorySceneObjects.push_back(mo);
    return mo;
}
 
void armarx::CollisionCheckerComponent::synchronizeObjectsFromWorkingMemory()
{
    std::scoped_lock lockPosition(wmPositionMutex);
    for (VirtualRobot::SceneObjectPtr& sc : workingMemorySceneObjects)
    {
        auto it = currentPositions.find(sc->getName());
        if (it != currentPositions.end())
        {
            sc->setGlobalPose(it->second->toEigen());
        }
    }
}
 
void armarx::CollisionCheckerComponent::reportDistancesAndCollisions()
{
    std::scoped_lock lockData(dataMutex);
    for (RobotPair& r : robots)
    {
        armarx::RemoteRobot::synchronizeLocalClone(r.robot, r.robotStateComponentPrx->getSynchronizedRobot());
    }
    synchronizeObjectsFromWorkingMemory();
 
    for (SceneObjectPair& pair : sceneObjectPairs)
    {
        if (!pair.objects1 || !pair.objects2)
        {
            continue;
        }
        double distance;
        if (useDebugDrawer && debugDrawerTopicPrx)
        {
            std::ostringstream nodeNames1Str;
            std::copy(pair.nodeNames1.begin(), pair.nodeNames1.end() - 1, std::ostream_iterator<std::string>(nodeNames1Str, ","));
            nodeNames1Str << pair.nodeNames1.back();
 
            std::ostringstream nodeNames2Str;
            std::copy(pair.nodeNames2.begin(), pair.nodeNames2.end() - 1, std::ostream_iterator<std::string>(nodeNames2Str, ","));
            nodeNames2Str << pair.nodeNames2.back();
 
            Eigen::Vector3f p1, p2;
            distance = collisionChecker->calculateDistance(pair.objects1, pair.objects2, p1, p2);
            Vector3BasePtr p1a(new Vector3(p1)), p2a(new Vector3(p2));
            DrawColor color;
            if (distance <= pair.warningDistance)
            {
                color.r = 1;
                color.g = 1;
                color.b = 0;
            }
            else
            {
                color.r = 0;
                color.g = 0.9;
                color.b = 0;
            }
            debugDrawerTopicPrx->setLineVisu("distanceVisu", pair.robotName1 + (pair.robotName1 == "" ? "" : ": ") + nodeNames1Str.str() + " - " + pair.robotName2 + (pair.robotName2 == "" ? "" : ": ") + nodeNames2Str.str(), p1a, p2a, 1, color);
        }
        else
        {
            distance = collisionChecker->calculateDistance(pair.objects1, pair.objects2);
        }
 
        distanceListenerPrx->reportDistance(pair.robotName1, pair.nodeNames1, pair.robotName2, pair.nodeNames2, distance);
 
        if (distance <= pair.warningDistance)
        {
            collisionListenerPrx->reportCollisionWarning(pair.robotName1, pair.nodeNames1, pair.robotName2, pair.nodeNames2, distance);
        }
 
        if (collisionChecker->checkCollision(pair.objects1, pair.objects2))
        {
            collisionListenerPrx->reportCollision(pair.robotName1, pair.nodeNames1, pair.robotName2, pair.nodeNames2, distance);
        }
    }
}
 
void CollisionCheckerComponent::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&)
{
    addCollisionPair(robotName1, nodeNames1, false, "", robotName2, nodeNames2, false, "", warningDistance);
}
 
void CollisionCheckerComponent::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)
{
    if (hasCollisionPair(robotName1, nodeNames1, robotName2, nodeNames2))
    {
        return;
    }
    SceneObjectPair pair = {VirtualRobot::SceneObjectSetPtr(), robotName1, nodeNames1, usesNodeSet1, nodeSetName1, VirtualRobot::SceneObjectSetPtr(), robotName2, nodeNames2, usesNodeSet2, nodeSetName2, warningDistance};
    std::scoped_lock lockData(dataMutex);
    resolveCollisionPair(pair);
    sceneObjectPairs.push_back(pair);
}
 
void CollisionCheckerComponent::removeCollisionPair(const std::string& robotName1, const std::vector<std::string>& nodeNames1, const std::string& robotName2, const std::vector<std::string>& nodeNames2, const Ice::Current&)
{
    std::scoped_lock lockData(dataMutex);
    for (auto it = sceneObjectPairs.begin(); it != sceneObjectPairs.end(); it++)
    {
        if ((it->robotName1 == robotName1 && it->nodeNames1 == nodeNames1 &&
             it->robotName2 == robotName2 && it->nodeNames2 == nodeNames2) ||
            (it->robotName1 == robotName2 && it->nodeNames1 == nodeNames2 &&
             it->robotName2 == robotName1 && it->nodeNames2 == nodeNames1))
        {
            sceneObjectPairs.erase(it);
            break;
        }
    }
}
 
bool CollisionCheckerComponent::hasCollisionPair(const std::string& robotName1, const std::vector<std::string>& nodeNames1, const std::string& robotName2, const std::vector<std::string>& nodeNames2, const Ice::Current&) const
{
    std::scoped_lock lockData(dataMutex);
    for (const SceneObjectPair& pair : sceneObjectPairs)
    {
        if ((pair.robotName1 == robotName1 && pair.nodeNames1 == nodeNames1 &&
             pair.robotName2 == robotName2 && pair.nodeNames2 == nodeNames2) ||
            (pair.robotName1 == robotName2 && pair.nodeNames1 == nodeNames2 &&
             pair.robotName2 == robotName1 && pair.nodeNames2 == nodeNames1))
        {
            return true;
        }
    }
    return false;
}
 
void CollisionCheckerComponent::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&)
{
    std::scoped_lock lockData(dataMutex);
    for (SceneObjectPair& pair : sceneObjectPairs)
    {
        if ((pair.robotName1 == robotName1 && pair.nodeNames1 == nodeNames1 &&
             pair.robotName2 == robotName2 && pair.nodeNames2 == nodeNames2) ||
            (pair.robotName1 == robotName2 && pair.nodeNames1 == nodeNames2 &&
             pair.robotName2 == robotName1 && pair.nodeNames2 == nodeNames1))
        {
            pair.warningDistance = warningDistance;
            break;
        }
    }
}
 
double CollisionCheckerComponent::getWarningDistance(const std::string& robotName1, const std::vector<std::string>& nodeNames1, const std::string& robotName2, const std::vector<std::string>& nodeNames2, const Ice::Current&) const
{
    std::scoped_lock lockData(dataMutex);
    for (const SceneObjectPair& pair : sceneObjectPairs)
    {
        if ((pair.robotName1 == robotName1 && pair.nodeNames1 == nodeNames1 &&
             pair.robotName2 == robotName2 && pair.nodeNames2 == nodeNames2) ||
            (pair.robotName1 == robotName2 && pair.nodeNames1 == nodeNames2 &&
             pair.robotName2 == robotName1 && pair.nodeNames2 == nodeNames1))
        {
            return pair.warningDistance;
        }
    }
    return INFINITY;
}
 
CollisionPairList CollisionCheckerComponent::getAllCollisionPairs(const Ice::Current&) const
{
    std::scoped_lock lockData(dataMutex);
    CollisionPairList list;
    for (const SceneObjectPair& pair : sceneObjectPairs)
    {
        armarx::CollisionPair p = {pair.robotName1, pair.nodeNames1, pair.robotName2, pair.nodeNames2, pair.warningDistance};
        list.push_back(p);
    }
    return list;
}
 
int CollisionCheckerComponent::getInterval(const Ice::Current&) const
{
    return interval;
}
 
void CollisionCheckerComponent::setInterval(int interval, const Ice::Current&)
{
    std::shared_lock lockConnected(connectedMutex);
    this->interval = interval;
    if (connected && reportTask && reportTask->isRunning())
    {
        reportTask->stop();
        reportTask = new PeriodicTask<CollisionCheckerComponent>(this, &CollisionCheckerComponent::reportDistancesAndCollisions, interval, false, "ReportCollisionsTask");
        reportTask->start();
    }
    else
    {
        reportTask = new PeriodicTask<CollisionCheckerComponent>(this, &CollisionCheckerComponent::reportDistancesAndCollisions, interval, false, "ReportCollisionsTask");
    }
}
 
void CollisionCheckerComponent::reportEntityCreated(const std::string& segmentName, const memoryx::EntityBasePtr& entity, const Ice::Current&)
{
    if (segmentName == objectInstancesPrx->getSegmentName())
    {
        std::scoped_lock lockData(dataMutex);
        for (SceneObjectPair& pair : sceneObjectPairs)
        {
            if ((!pair.objects1 || !pair.objects2) &&
                ((pair.robotName1 == "" && std::find(pair.nodeNames1.begin(), pair.nodeNames1.end(), entity->getName()) != pair.nodeNames1.end()) ||
                 (pair.robotName2 == "" && std::find(pair.nodeNames2.begin(), pair.nodeNames2.end(), entity->getName()) != pair.nodeNames2.end())))
            {
                resolveCollisionPair(pair);
            }
        }
    }
    else if (segmentName == agentInstancesPrx->getSegmentName())
    {
        std::scoped_lock lockData(dataMutex);
        memoryx::AgentInstanceBasePtr agent = agentInstancesPrx->getAgentInstanceById(entity->getId());
        RobotStateComponentInterfacePrx robotStateComponentPrx = agent->getSharedRobot()->getRobotStateComponent();
        VirtualRobot::RobotPtr robot = armarx::RemoteRobot::createLocalCloneFromFile(robotStateComponentPrx);
 
        robots.push_back({robot, robotStateComponentPrx});
 
 
        for (SceneObjectPair& pair : sceneObjectPairs)
        {
            if ((!pair.objects1 || !pair.objects2) &&
                (pair.robotName1 == entity->getName() ||
                 pair.robotName2 == entity->getName()))
            {
                resolveCollisionPair(pair);
            }
        }
    }
}
 
void CollisionCheckerComponent::reportEntityUpdated(const std::string& segmentName, const memoryx::EntityBasePtr& entityOld, const memoryx::EntityBasePtr& entityNew, const Ice::Current&)
{
    if (segmentName == objectInstancesPrx->getSegmentName())
    {
        std::scoped_lock lockPosition(wmPositionMutex);
        const memoryx::ObjectInstanceBasePtr object = memoryx::ObjectInstancePtr::dynamicCast(entityNew);
        currentPositions[entityNew->getName()] = armarx::PosePtr::dynamicCast(armarx::PoseBasePtr {new armarx::Pose{object->getPositionBase(), object->getOrientationBase()}});
    }
}
 
void CollisionCheckerComponent::reportEntityRemoved(const std::string& segmentName, const memoryx::EntityBasePtr& entity, const Ice::Current&)
{
    if (segmentName == objectInstancesPrx->getSegmentName())
    {
        std::scoped_lock lockData(dataMutex);
        for (SceneObjectPair& pair : sceneObjectPairs)
        {
            if ((pair.robotName1 == "" && std::find(pair.nodeNames1.begin(), pair.nodeNames1.end(), entity->getName()) != pair.nodeNames1.end()) ||
                (pair.robotName2 == "" && std::find(pair.nodeNames2.begin(), pair.nodeNames2.end(), entity->getName()) != pair.nodeNames2.end()))
            {
                pair.objects1.reset();
                pair.objects2.reset();
            }
        }
        for (auto it = workingMemorySceneObjects.begin(); it != workingMemorySceneObjects.end(); ++it)
        {
            if ((*it)->getName() == entity->getName())
            {
                workingMemorySceneObjects.erase(it);
                break;
            }
        }
    }
    else if (segmentName == agentInstancesPrx->getSegmentName())
    {
        std::scoped_lock lockData(dataMutex);
        for (auto it = robots.begin(); it != robots.end(); ++it)
        {
            if ((*it).robot->getName() == entity->getName())
            {
                robots.erase(it);
                break;
            }
        }
 
        for (SceneObjectPair& pair : sceneObjectPairs)
        {
            if (pair.robotName1 == entity->getName() ||
                pair.robotName2 == entity->getName())
            {
                pair.objects1.reset();
                pair.objects2.reset();
            }
        }
    }
}
 
void CollisionCheckerComponent::reportSnapshotLoaded(const std::string& segmentName, const Ice::Current&)
{
    if (segmentName == objectInstancesPrx->getSegmentName())
    {
        std::scoped_lock lockData(dataMutex);
        for (SceneObjectPair& pair : sceneObjectPairs)
        {
            if (!pair.objects1 || !pair.objects2)
            {
                resolveCollisionPair(pair);
            }
        }
    }
}
 
void CollisionCheckerComponent::reportSnapshotCompletelyLoaded(const Ice::Current& c)
{
 
}
 
void CollisionCheckerComponent::reportMemoryCleared(const std::string& segmentName, const Ice::Current&)
{
    if (segmentName == objectInstancesPrx->getSegmentName())
    {
        std::scoped_lock lockData(dataMutex);
        for (SceneObjectPair& pair : sceneObjectPairs)
        {
            if (pair.robotName1 == "" || pair.robotName2 == "")
            {
                pair.objects1.reset();
                pair.objects2.reset();
            }
        }
    }
}