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 <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
#include <RobotAPI/libraries/core/FramedPose.h>
 
#include <VisionX/libraries/SemanticObjectRelations/hooks.h>
#include <VisionX/libraries/SemanticObjectRelations/ice_serialization.h>
 
#include <SemanticObjectRelations/SupportAnalysis/json.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()
    {
        return "ShapesSupportRelations";
    }
 
    std::string
    ShapesSupportRelations::getDefaultName() const
    {
        return GetDefaultName();
    }
 
    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()));
    }
 
 
 
    ARMARX_REGISTER_COMPONENT_EXECUTABLE(::armarx::semantic::ShapesSupportRelations, ::armarx::semantic::ShapesSupportRelations::GetDefaultName());
} // namespace armarx::semantic