ShapesSupportRelations.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::ShapesSupportRelations
 * @author     Rainer Kartmann ( rainer dot kartmann at kit dot edu )
 * @date       2019
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "ShapesSupportRelations.h"
 
#include <SemanticObjectRelations/SupportAnalysis/json.h>
 
#include <RobotAPI/libraries/core/FramedPose.h>
#include <VisionX/libraries/SemanticObjectRelations/hooks.h>
#include <VisionX/libraries/SemanticObjectRelations/ice_serialization.h>
 
 
namespace armarx::semantic
{
    ShapesSupportRelationsPropertyDefinitions::ShapesSupportRelationsPropertyDefinitions(std::string prefix) :
        armarx::ComponentPropertyDefinitions(prefix)
    {
        // ICE
 
        defineOptionalProperty<std::string>("ice.ShapesTopicName", "ShapesTopic",
                                            "Name of the topic on which shapes are reported.");
 
        defineOptionalProperty<std::string>("ice.SemanticGraphName", "Support",
                                            "Name of the graph when reported to the SemanticGraphStorage topic.");
        defineOptionalProperty<std::string>("ice.SemanticGraphTopicName", "SemanticGraphTopic",
                                            "Name of SemanticGraphStorage topic.");
 
        defineOptionalProperty<std::string>("ice.RobotStateComponentName", "RobotStateComponent",
                                            "Name of the robot state component.");
 
        defineOptionalProperty<std::string>("pc.PointCloudFrameName", GlobalFrame,
                                            "Name of the frame of the point cloud coordinates. \n"
                                            "Used to determine the gravity vector.");
 
        defineOptionalProperty<std::string>("ice.DebugObserverName", "DebugObserver",
                                            "Name of the topic the DebugObserver listens to.");
 
 
        // PARAMETERS
 
        defineOptionalProperty<float>(
            "GR.ContactMargin", 10.,
            "Distance by which objects are increased for contact computation [mm].")
        .setMin(0.);
 
        defineOptionalProperty<float>(
            "GR.VerticalSepPlaneAngleMax", 10.,
            "Maximal angle [degree] between gravity and separating plane for \n"
            "separating plane to be considered vertical.")
        .setMin(0.);
 
        defineOptionalProperty<bool>(
            "GR.VerticalSepPlaneAssumeSupport", false,
            "If true, edges are added if the separating plane is vertical.");
 
 
        defineOptionalProperty<bool>(
            "UD.enabled", true,
            "Enable or disble uncertainty detection (UD).");
 
        defineOptionalProperty<float>(
            "UD.SupportAreaRatioMin", 0.7f,
            "Minimal support area ratio of an object to consider it safe.")
        .setMin(0.).setMax(1.);
 
 
 
        // VISUALIZATION
 
        properties::defineVisualizationLevel(*this, properties::defaults::visualizationLevelName,
                                             semrel::VisuLevel::DISABLED);
    }
 
    std::string ShapesSupportRelations::getDefaultName() const
    {
        return "ShapesSupportRelations";
    }
 
 
    void ShapesSupportRelations::onInitComponent()
    {
        // Register offered topices and used proxies here.
 
        usingTopicFromProperty("ice.ShapesTopicName");
 
        offeringTopicFromProperty("ice.SemanticGraphTopicName");
        getProperty(graphName, "ice.SemanticGraphName");
 
        offeringTopicFromProperty("ice.DebugObserverName");
        debugDrawer.offeringTopic(*this);  // Calls this->offeringTopic().
 
        // usingProxyFromProperty("ice.RobotStateComponentName");  // RobotStateComponent is not required.
        getProperty(pointCloudFrameName, "pc.PointCloudFrameName");
 
        supportAnalysis.setContactMargin(getProperty<float>("GR.ContactMargin"));
        supportAnalysis.setVertSepPlaneAngleMax(getProperty<float>("GR.VerticalSepPlaneAngleMax"));
        supportAnalysis.setVertSepPlaneAssumeSupport(getProperty<bool>("GR.VerticalSepPlaneAssumeSupport"));
 
        supportAnalysis.setUncertaintyDetectionEnabled(getProperty<bool>("UD.enabled"));
        supportAnalysis.setSupportAreaRatioMin(getProperty<float>("UD.SupportAreaRatioMin"));
    }
 
 
    void ShapesSupportRelations::onConnectComponent()
    {
        // Get topics and proxies here. Pass the *InterfacePrx type as template argument.
        // Robot state component may not be available.
        getProxyFromProperty(robotStateComponent, "ice.RobotStateComponentName", false, "", false);
        getTopicFromProperty(graphTopic, "ice.SemanticGraphTopicName");
 
        if (!robotStateComponent)
        {
            ARMARX_INFO << "RobotStateComponent is not available. Global gravity vector "
                        << gravityInGlobal.transpose() << " will be used.";
        }
 
        getTopicFromProperty(debugObserver, "ice.DebugObserverName");
        debugDrawer.getTopic(*this);  // Calls this->getTopic().
 
        setArmarXHooksAsImplementation(debugDrawer, getName());
        properties::setMinimumVisuLevel(*this);
    }
 
 
    void ShapesSupportRelations::onDisconnectComponent()
    {
        robotStateComponent = nullptr;
    }
 
 
    void ShapesSupportRelations::onExitComponent()
    {
    }
 
 
    void ShapesSupportRelations::reportShapes(
        const std::string& name, const data::ShapeList& objectsIce, const Ice::Current&)
    {
        const semrel::ShapeList objects = semantic::fromIce(objectsIce);
        const semrel::ShapeMap objectsMap = semrel::toShapeMap(objects);
 
        std::set<semrel::ShapeID> safeObjectsIDs;
        // Use the lowest object as safe object?
 
        const data::Graph graph = extractSupportGraph(objectsMap, safeObjectsIDs);
 
        std::stringstream graphName;
        graphName << this->graphName << " (" << name << ")";
        graphTopic->reportGraph(graphName.str(), graph);
    }
 
 
    data::Graph ShapesSupportRelations::extractSupportGraph(
        const data::ShapeList& objectsIce, const Ice::LongSeq& safeObjectIDsIce, const Ice::Current&)
    {
        const semrel::ShapeList objects = semantic::fromIce(objectsIce);
        const semrel::ShapeMap objectsMap = semrel::toShapeMap(objects);
 
        std::set<semrel::ShapeID> safeObjectIDs;
        for (long id : safeObjectIDsIce)
        {
            safeObjectIDs.insert(semrel::ShapeID { id });
        }
 
        const data::Graph ice = extractSupportGraph(objectsMap, safeObjectIDs);
        return ice;
    }
 
    data::Graph ShapesSupportRelations::extractSupportGraph(
        const semrel::ShapeMap& objects, const std::set<semrel::ShapeID>& safeObjectIDs)
    {
        // Update gravity vector.
        supportAnalysis.setGravityVector(getGravityFromRobotStateComponent());
 
        // Perform support analysis.
        const semrel::SupportGraph supportGraph = supportAnalysis.performSupportAnalysis(objects, safeObjectIDs);
        ARMARX_INFO << "Support Graph: " << supportGraph.str();
 
        const data::Graph graphIce = semantic::toIce(supportGraph, objects);
        return graphIce;
    }
 
 
    Eigen::Vector3f ShapesSupportRelations::getGravityFromRobotStateComponent()
    {
        if (!robotStateComponent || pointCloudFrameName == armarx::GlobalFrame)
        {
            return gravityInGlobal;
        }
        else
        {
            VirtualRobot::RobotPtr robot = RemoteRobot::createLocalClone(
                                               robotStateComponent, "", {}, VirtualRobot::RobotIO::eStructure);
 
            FramedDirection dir(gravityInGlobal, armarx::GlobalFrame, "");
            dir.changeFrame(robot, pointCloudFrameName);
            return dir.toEigen();
        }
    }
 
 
    armarx::PropertyDefinitionsPtr ShapesSupportRelations::createPropertyDefinitions()
    {
        return armarx::PropertyDefinitionsPtr(new ShapesSupportRelationsPropertyDefinitions(
                getConfigIdentifier()));
    }
 
 
}