SimulatorToArviz.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    ArmarXSimulation::ArmarXObjects::SimulatorToArviz
 * @author     Peter Reiner ( ufekv at student dot kit dot edu )
 * @date       2020
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "SimulatorToArviz.h"
 
#include <VirtualRobot/RuntimeEnvironment.h>
 
namespace armarx
{
 
 
    armarx::PropertyDefinitionsPtr
    SimulatorToArviz::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr defs =
            new ComponentPropertyDefinitions(getConfigIdentifier());
 
        defs->optional(properties.oldPriorKnowledge.use, "p.oldPriorKnowledge.use");
        defs->optional(properties.oldPriorKnowledge.name, "p.oldPriorKnowledge.name");
 
        return defs;
    }
 
    SimulatorToArviz::SimulatorToArviz()
    {
    }
 
    std::string
    SimulatorToArviz::getDefaultName() const
    {
        return "SimulatorToArviz";
    }
 
    void
    SimulatorToArviz::onInitComponent()
    {
        usingTopic("SimulatorListenerInterface");
        usingTopic("SimulatorVisuUpdates");
 
        if (properties.oldPriorKnowledge.use)
        {
            usingProxy(properties.oldPriorKnowledge.name);
        }
    }
 
    void
    SimulatorToArviz::onConnectComponent()
    {
        if (properties.oldPriorKnowledge.use)
        {
            getProxy(priorKnowledge, properties.oldPriorKnowledge.name);
            objectClassSegment.initFromProxy(priorKnowledge, {});
        }
    }
 
    void
    SimulatorToArviz::onDisconnectComponent()
    {
    }
 
    void
    SimulatorToArviz::onExitComponent()
    {
    }
 
    void
    SimulatorToArviz::reportSceneUpdated(const SceneVisuData& scene, const ::Ice::Current&)
    {
        armarx::viz::Layer floorLayer = arviz.layer("FloorLayer");
        armarx::viz::Layer robotLayer = arviz.layer("RobotLayer");
        armarx::viz::Layer objectLayer = arviz.layer("ObjectLayer");
        handleRobotLayer(scene.robots, robotLayer);
        handleObjectLayer(scene.objects, objectLayer);
 
        if (scene.floor)
        {
            ARMARX_DEBUG << "has floor: " << scene.floor
                         << " with filename: " << scene.floorTextureFile;
            std::string filename = "images/Floor20x20.xml";
            VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(filename);
            floorLayer.add(armarx::viz::Object("Floor").file("", filename));
        }
 
        arviz.commit({floorLayer, robotLayer, objectLayer});
    }
 
    void
    SimulatorToArviz::handleRobotLayer(const RobotVisuList& robotList,
                                       armarx::viz::Layer& robotLayer)
    {
        for (const auto& e : robotList)
        {
            Eigen::Vector3f position(e.pose->position->x, e.pose->position->y, e.pose->position->z);
            Eigen::Quaternionf orientation(e.pose->orientation->qw,
                                           e.pose->orientation->qx,
                                           e.pose->orientation->qy,
                                           e.pose->orientation->qz);
            ARMARX_DEBUG << "Drawing robot " << e.name << " using file: " << e.robotFile
                         << " and joint values " << e.jointValues;
            armarx::viz::Robot robot = armarx::viz::Robot(e.name)
                                           .file(e.project, e.robotFile)
                                           .useFullModel()
                                           .joints(e.jointValues)
                                           .useOriginalColor()
                                           .scale(e.scaling)
                                           .pose(position, orientation);
 
            robotLayer.add(robot);
        }
    }
 
    void
    SimulatorToArviz::handleObjectLayer(const ObjectVisuList& objectList,
                                        armarx::viz::Layer& objectLayer)
    {
        for (const ObjectVisuData& data : objectList)
        {
            if (data.objectPrimitiveData)
            {
                handleObjectPrimitiveData(data, objectLayer);
            }
            else if (not data.filename.empty())
            {
                handleObjectFromFile(data, objectLayer);
            }
            else if (properties.oldPriorKnowledge.use)
            {
                handleObjectFromObjectClass(data, objectLayer);
            }
        }
    }
 
    void
    SimulatorToArviz::handleObjectPrimitiveData(const ObjectVisuData& _object,
                                                armarx::viz::Layer& objectLayer)
    {
        const PoseBasePtr& objectPose = _object.objectPoses.at(_object.name);
        Eigen::Vector3f position(
            objectPose->position->x, objectPose->position->y, objectPose->position->z);
        Eigen::Quaternionf orientation(objectPose->orientation->qw,
                                       objectPose->orientation->qx,
                                       objectPose->orientation->qy,
                                       objectPose->orientation->qz);
 
        switch (_object.objectPrimitiveData->type)
        {
            case ObjectType::Box:
            {
                BoxVisuPrimitivePtr boxObject =
                    BoxVisuPrimitivePtr::dynamicCast(_object.objectPrimitiveData);
                //armarx::viz::Box box = armarx::viz::Box(_object.name)
                objectLayer.add(
                    armarx::viz::Box(_object.name)
                        .size(
                            Eigen::Vector3f{boxObject->width, boxObject->height, boxObject->depth})
                        .color(boxObject->color.r, boxObject->color.g, boxObject->color.b)
                        .position(position)
                        .orientation(orientation));
            }
            break;
            case ObjectType::Sphere:
            {
                SphereVisuPrimitivePtr sphereObject =
                    SphereVisuPrimitivePtr::dynamicCast(_object.objectPrimitiveData);
                //armarx::viz::Sphere sphere = armarx::viz::Sphere(_object.name)
                objectLayer.add(
                    armarx::viz::Sphere(_object.name)
                        .radius(sphereObject->radius)
                        .color(sphereObject->color.r, sphereObject->color.g, sphereObject->color.b)
                        .position(position)
                        .orientation(orientation));
            }
            break;
            case ObjectType::Cylinder:
            {
                CylinderVisuPrimitivePtr cylinderObject =
                    CylinderVisuPrimitivePtr::dynamicCast(_object.objectPrimitiveData);
                //armarx::viz::Cylinder cylinder = armarx::viz::Cylinder(_object.name)
                objectLayer.add(armarx::viz::Cylinder(_object.name)
                                    .radius(cylinderObject->radius)
                                    .height(cylinderObject->length)
                                    .color(cylinderObject->color.r,
                                           cylinderObject->color.g,
                                           cylinderObject->color.b)
                                    .position(position)
                                    .orientation(orientation));
            }
            break;
        }
    }
 
    void
    SimulatorToArviz::handleObjectFromFile(const ObjectVisuData& _object,
                                           armarx::viz::Layer& objectLayer)
    {
        const PoseBasePtr& objectPose = _object.objectPoses.at(_object.name);
 
        Eigen::Vector3f position(
            objectPose->position->x, objectPose->position->y, objectPose->position->z);
        Eigen::Quaternionf orientation(objectPose->orientation->qw,
                                       objectPose->orientation->qx,
                                       objectPose->orientation->qy,
                                       objectPose->orientation->qz);
 
        objectLayer.add(armarx::viz::Object(_object.name)
                            .file(_object.project, _object.filename)
                            .position(position)
                            .orientation(orientation));
    }
 
    void
    SimulatorToArviz::handleObjectFromObjectClass(const ObjectVisuData& _object,
                                                  armarx::viz::Layer& objectLayer)
    {
 
        const PoseBasePtr& objectPose = _object.objectPoses.at(_object.name);
 
        Eigen::Vector3f position(
            objectPose->position->x, objectPose->position->y, objectPose->position->z);
        Eigen::Quaternionf orientation(objectPose->orientation->qw,
                                       objectPose->orientation->qx,
                                       objectPose->orientation->qy,
                                       objectPose->orientation->qz);
        std::string filename = getClassFilename(_object.objectClassName);
        std::string projectname = "";
        objectLayer.add(armarx::viz::Object(_object.name)
                            .file(projectname, filename)
                            .position(position)
                            .orientation(orientation));
    }
 
    std::string
    SimulatorToArviz::getClassFilename(const std::string& className)
    {
        auto it = classFilenameCache.find(className);
        if (it != classFilenameCache.end())
        {
            return it->second;
        }
        else
        {
            std::optional<memoryx::ObjectClassWrapper> objectClass =
                objectClassSegment.getClass(className);
            if (!objectClass)
            {
                return "";
            }
            memoryx::EntityWrappers::SimoxObjectWrapperPtr wrapper =
                objectClass->classInMemory
                    ->getWrapper<memoryx::EntityWrappers::SimoxObjectWrapper>();
            std::string filename = wrapper->getManipulationObjectFileName();
 
            classFilenameCache[className] = filename;
 
            return filename;
        }
    }
} // namespace armarx