SemanticRelationAnalyzer.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    VisionX::ArmarXObjects::SemanticRelationAnalyzer
 * @author     Fabian Paus ( fabian dot paus at kit dot edu )
 * @date       2019
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "SemanticRelationAnalyzer.h"
 
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
#include <VirtualRobot/CollisionDetection/CollisionChecker.h>
#include <VirtualRobot/CollisionDetection/CollisionCheckerImplementation.h>
#include <VirtualRobot/CollisionDetection/CollisionModel.h>
#include <VirtualRobot/ManipulationObject.h>
#include <VirtualRobot/Visualization/TriMeshModel.h>
 
#include <ArmarXCore/util/CPPUtility/GetTypeString.h>
 
#include <VisionX/libraries/SemanticObjectRelations/SimoxObjectShape.h>
#include <VisionX/libraries/SemanticObjectRelations/hooks.h>
#include <VisionX/libraries/SemanticObjectRelations/ice_serialization.h>
#include <VisionX/libraries/SemanticObjectRelations/memory_serialization.h>
 
#include <SemanticObjectRelations/Shapes/json/ShapeSerializers.h>
#include <SemanticObjectRelations/SupportAnalysis/json.h>
#include <SemanticObjectRelations/Visualization/ContactDetectionVisualizer.h>
#include <SemanticObjectRelations/Visualization/SupportAnalysisVisualizer.h>
 
namespace armarx
{
    SemanticRelationAnalyzerPropertyDefinitions::SemanticRelationAnalyzerPropertyDefinitions(
        std::string prefix) :
        armarx::ComponentPropertyDefinitions(prefix)
    {
        defineOptionalProperty<std::string>(
            "WorkingMemoryName", "WorkingMemory", "Name of WorkingMemory component");
        defineOptionalProperty<std::string>(
            "PriorKnowledgeName", "PriorKnowledge", "Name of PriorKnowledge component");
        defineOptionalProperty<std::string>("RobotStateComponentName",
                                            "RobotStateComponent",
                                            "Name of RobotStateComponent component");
        defineOptionalProperty<std::string>(
            "DebugDrawerTopicName", "DebugDrawerUpdates", "The name of the debug drawer topic");
        defineOptionalProperty<int>("UpdatePeriodInMS", 10, "Update period in ms");
 
        defineOptionalProperty<semrel::VisuLevel>(
            "VisuLevel",
            semrel::VisuLevel::RESULT,
            "Visualization level: Determines what is shown in the debug drawer\n"
            "Possible values: live, verbose, result, user, disabled")
            .map("live", semrel::VisuLevel::LIVE_VISU)
            .map("verbose", semrel::VisuLevel::VERBOSE)
            .map("result", semrel::VisuLevel::RESULT)
            .map("user", semrel::VisuLevel::USER)
            .map("disabled", semrel::VisuLevel::DISABLED);
 
        defineOptionalProperty<float>(
            "ContactMarginInMM", 5.0f, "Margin (mm) used during contact detection between objects");
    }
 
    static semrel::ContactPointList
    calculateContactPoints(semrel::ShapeList const& objects)
    {
        semrel::ContactPointList result;
 
        VirtualRobot::CollisionCheckerPtr collisionChecker =
            VirtualRobot::CollisionChecker::getGlobalCollisionChecker();
        for (std::size_t i = 0; i < objects.size(); ++i)
        {
            armarx::semantic::SimoxObjectShape const& shapeA =
                dynamic_cast<armarx::semantic::SimoxObjectShape const&>(*objects[i]);
            for (std::size_t j = i + 1; j < objects.size(); ++j)
            {
                armarx::semantic::SimoxObjectShape const& shapeB =
                    dynamic_cast<armarx::semantic::SimoxObjectShape const&>(*objects[j]);
                Eigen::Affine3f poseB(shapeB.object->getGlobalPose());
 
                VirtualRobot::CollisionModelPtr collModelA = shapeA.object->getCollisionModel();
                VirtualRobot::CollisionModelPtr collModelB = shapeB.object->getCollisionModel();
                VirtualRobot::MultiCollisionResult collisions =
                    collisionChecker->checkMultipleCollisions(collModelA, collModelB);
 
                //auto& facesA = collModelA->getTriMeshModel()->faces;
                auto& facesB = collModelB->getTriMeshModel()->faces;
                auto& normalsB = collModelB->getTriMeshModel()->normals;
                for (VirtualRobot::SingleCollisionPair const& collision : collisions.pairs)
                {
                    //VirtualRobot::MathTools::TriangleFace const& faceOnA = facesA.at(collision.id1);
                    VirtualRobot::MathTools::TriangleFace const& faceOnB = facesB.at(collision.id2);
 
                    Eigen::Vector3f globalPointOnB = collision.contact2;
 
                    Eigen::Vector3f localNormalOnB;
                    if (faceOnB.idNormal1 == UINT_MAX)
                    {
                        localNormalOnB = poseB.linear() * faceOnB.normal;
                    }
                    else
                    {
                        Eigen::Vector3f n1B = normalsB[faceOnB.idNormal1];
                        Eigen::Vector3f n2B = normalsB[faceOnB.idNormal2];
                        Eigen::Vector3f n3B = normalsB[faceOnB.idNormal3];
 
                        localNormalOnB = (n1B + n2B + n3B).normalized();
                    }
                    Eigen::Vector3f globalNormalOnB = poseB.linear() * localNormalOnB;
 
                    result.emplace_back(&shapeA, &shapeB, globalPointOnB, globalNormalOnB);
                }
            }
        }
 
        return result;
    }
 
    static void
    filterObjectsWithoutCollisionModel(std::vector<memoryx::ObjectInstanceWrapper>& objects)
    {
        // Filter out object without collision model
        auto removeIter = std::remove_if(
            objects.begin(),
            objects.end(),
            [](memoryx::ObjectInstanceWrapper const& object)
            {
                // Check whether this thing has a collision model
                if (object.manipulationObject->getCollisionModel())
                {
                    if (object.manipulationObject->getCollisionModel()->getTriMeshModel())
                    {
                        return false;
                    }
                    else
                    {
                        ARMARX_WARNING_S << "No tri-mesh model in '"
                                         << object.instanceInMemory->getMostProbableClass() << "'";
                    }
                }
                else
                {
                    ARMARX_WARNING_S << "No collision model in '"
                                     << object.instanceInMemory->getMostProbableClass() << "'";
                }
                return true;
            });
        objects.erase(removeIter, objects.end());
    }
 
    semantic::data::Graph
    SemanticRelationAnalyzer::extractSupportGraphFromWorkingMemory(const Ice::Current&)
    {
        std::lock_guard<std::mutex> lock(componentMutex);
 
        std::vector<memoryx::ObjectInstanceWrapper> objectInstances =
            objectInstancesSegment_.queryObjects();
        filterObjectsWithoutCollisionModel(objectInstances);
 
        semrel::ShapeList shapes;
        std::set<semrel::ShapeID> safeShapes;
        std::set<std::string> safeObjectClassNames = {"workbench", "booth"};
 
        std::vector<std::string> names;
        int uniqueID = 0;
        for (auto& object : objectInstances)
        {
            std::string objectClass = object.instanceInMemory->getMostProbableClass();
            // Here we have the manipulation object, i.e. mesh of the object
            if (safeObjectClassNames.count(objectClass) > 0)
            {
                ARMARX_IMPORTANT << "Adding safe object: " << object.instanceInMemory->getName();
                safeShapes.insert(semrel::ShapeID{uniqueID});
            }
            object.manipulationObject->getCollisionModel()->inflateModel(prop_ContactMarginInMM);
            std::unique_ptr<semantic::SimoxObjectShape> shape =
                std::make_unique<semantic::SimoxObjectShape>(object.manipulationObject,
                                                             objectClass);
            shape->entityId = object.instanceInMemory->getId();
 
            shape->setID(semrel::ShapeID{uniqueID});
            uniqueID += 1;
            shapes.push_back(std::move(shape));
        }
 
        ARMARX_INFO << deactivateSpam(1) << "Object instances (" << shapes.size() << "): " << names;
 
        // Do the contact detection our own way (for now)
        semrel::ContactPointList contactPoints = calculateContactPoints(shapes);
        semrel::ContactGraph contactGraph = semrel::buildContactGraph(contactPoints, shapes);
        //semrel::SupportAnalysisVisualizer& visu = semrel::SupportAnalysisVisualizer::get();
        semrel::ContactDetectionVisualizer& visu = semrel::ContactDetectionVisualizer::get();
        visu.drawContactPoints(contactPoints);
 
        semrel::SupportGraph supportGraph =
            supportAnalysis.performSupportAnalysis(shapes, safeShapes, &contactGraph);
        ARMARX_IMPORTANT << "Supprt graph:\n" << supportGraph;
 
        // , semantic::JsonSimoxShapeSerializer()
        semrel::AttributedGraph attributedGraph = semrel::toAttributedGraph(supportGraph);
        attributedGraph.serializeObjects(semrel::toShapeMap(shapes));
        semantic::data::Graph iceGraph = armarx::semantic::toIce(attributedGraph);
        getGraphStorageTopic()->begin_reportGraph("Support", iceGraph);
 
        memoryx::RelationList relations = semantic::toMemory(attributedGraph);
        relationSegment->replaceRelations(relations);
 
        return iceGraph;
    }
 
#define GET_PROP(name) prop_##name = getProperty<decltype(prop_##name)>(#name).getValue()
 
    void
    SemanticRelationAnalyzer::onInitComponent()
    {
        // Register shape serializer for
 
        GET_PROP(WorkingMemoryName);
        GET_PROP(PriorKnowledgeName);
        GET_PROP(RobotStateComponentName);
        GET_PROP(DebugDrawerTopicName);
        GET_PROP(UpdatePeriodInMS);
        GET_PROP(ContactMarginInMM);
 
        offeringTopic(prop_DebugDrawerTopicName);
 
        semantic::ArmarXLog::setAsImplementation(getName());
    }
 
    void
    SemanticRelationAnalyzer::onConnectComponent()
    {
        workingMemory = getProxy<memoryx::WorkingMemoryInterfacePrx>(prop_WorkingMemoryName);
        priorKnowledge = getProxy<memoryx::PriorKnowledgeInterfacePrx>(prop_PriorKnowledgeName);
        robotStateComponent =
            getProxy<RobotStateComponentInterfacePrx>(prop_RobotStateComponentName);
        debugDrawer = getTopic<DebugDrawerInterfacePrx>(prop_DebugDrawerTopicName);
 
        semantic::ArmarXVisualizer::setAsImplementation(debugDrawer);
        semrel::VisuLevel visuLevel = getProperty<semrel::VisuLevel>("VisuLevel").getValue();
        semrel::VisualizerInterface::get().setMinimumVisuLevel(visuLevel);
 
        relationSegment = workingMemory->getRelationsSegment();
        objectInstancesSegment_.initFromProxies(priorKnowledge, workingMemory, robotStateComponent);
 
        jsonSerializer.reset(
            new semantic::JsonSimoxShapeSerializer(&objectInstancesSegment_.objectClassSegment));
        semantic::JsonSimoxShapeSerializer::registerSerializer(jsonSerializer, true);
 
        // We call the method once for test purposes
        extractSupportGraphFromWorkingMemory(Ice::Current());
 
        updateTask = new PeriodicTask<SemanticRelationAnalyzer>(
            this, &SemanticRelationAnalyzer::update, prop_UpdatePeriodInMS);
        updateTask->start();
    }
 
    void
    SemanticRelationAnalyzer::onDisconnectComponent()
    {
        updateTask->stop();
        updateTask = nullptr;
    }
 
    void
    SemanticRelationAnalyzer::onExitComponent()
    {
    }
 
    armarx::PropertyDefinitionsPtr
    SemanticRelationAnalyzer::createPropertyDefinitions()
    {
        return armarx::PropertyDefinitionsPtr(
            new SemanticRelationAnalyzerPropertyDefinitions(getConfigIdentifier()));
    }
 
    static std::string const LAYER_NAME = "SemanticRelationAnalyzer";
 
    void
    SemanticRelationAnalyzer::update()
    {
        // extractSupportGraphFromWorkingMemory(Ice::Current());
    }
 
} // namespace armarx
 
std::string
armarx::SemanticRelationAnalyzer::GetDefaultName()
{
    return "SemanticRelationAnalyzer";
}
 
std::string
armarx::SemanticRelationAnalyzer::getDefaultName() const
{
    return GetDefaultName();
}
 
ARMARX_REGISTER_COMPONENT_EXECUTABLE(::armarx::SemanticRelationAnalyzer, ::armarx::SemanticRelationAnalyzer::GetDefaultName());