VisualizationObject.cpp

Go to the documentation of this file.

#include "VisualizationObject.h"
 
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
 
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
 
#include <VirtualRobot/ManipulationObject.h>
#include <VirtualRobot/SceneObject.h>
#include <VirtualRobot/XML/RobotIO.h>
#include <VirtualRobot/XML/ObjectIO.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
#include <VirtualRobot/Import/MeshImport/AssimpReader.h>
 
#include <SimoxUtility/algorithm/string/string_tools.h>
 
namespace armarx::viz::coin
{
 
    namespace
    {
        std::string findObjectInArmarXObjects(const std::string& filename)
        {
            IceUtil::Time start = IceUtil::Time::now();
            std::string objectName = std::filesystem::path(filename).filename().stem();
            // ARMARX_INFO << "Trying to find object '" << objectName << "' in ArmarXObjects.";
 
            std::string fullFilename;
            std::stringstream ss;
 
            armarx::ObjectFinder objectFinder;
            if (std::optional<armarx::ObjectInfo> info = objectFinder.findObject("", objectName))
            {
                fullFilename = info->simoxXML().absolutePath;
                ss << "Found '" << objectName << "' in ArmarXObjects as " << *info << " \nat '" << fullFilename << "'.";
            }
            else
            {
                ss << "Did not find '" << objectName << "' in ArmarXObjects.";
            }
            ss << "\n(Lookup took " << (IceUtil::Time::now() - start).toMilliSecondsDouble() << " ms.)";
            ARMARX_INFO << ss.str();
 
            return fullFilename;
        }
 
 
        VirtualRobot::ManipulationObjectPtr loadObject(std::string const& project, std::string const& filename)
        {
            VirtualRobot::ManipulationObjectPtr  result;
 
            if (filename.empty())
            {
                ARMARX_INFO << deactivateSpam() << "No filename provided for object";
                return result;
            }
 
            bool checkArmarXObjects = true;
 
 
            // This check cannot be done here!
            // filename is a relative path in the project
            // Also the code below does check whether the file is readable!
            //            std::ifstream fs(filename);
            //            if (!fs.good())
            //            {
 
            //                ARMARX_WARNING << "unable to read from file " << VAROUT(filename);
            //                return result;
            //            }
 
            ArmarXDataPath::FindPackageAndAddDataPath(project);
            std::string fullFilename;
            if (!ArmarXDataPath::SearchReadableFile(filename, fullFilename))
            {
                fullFilename = "";
                if (checkArmarXObjects)
                {
                    fullFilename = findObjectInArmarXObjects(filename);
                }
                if (fullFilename.empty())
                {
                    ARMARX_INFO << deactivateSpam()
                                << "Unable to find readable file for name "
                                << filename;
                    return result;
                }
            }
 
            try
            {
                ARMARX_INFO << "Loading object from " << fullFilename;
 
                std::filesystem::path path{fullFilename};
                const std::string ext = simox::alg::to_lower(path.extension());
 
                if (ext == ".wrl" || ext == ".iv")
                {
                    VirtualRobot::VisualizationFactoryPtr factory = VirtualRobot::VisualizationFactory::fromName("inventor", nullptr);
                    VirtualRobot::VisualizationNodePtr vis = factory->getVisualizationFromFile(fullFilename);
                    result = VirtualRobot::ManipulationObjectPtr(new VirtualRobot::ManipulationObject(filename, vis));
                }
                else if (ext == ".xml" || ext == ".moxml")
                {
                    result = VirtualRobot::ObjectIO::loadManipulationObject(fullFilename);
 
                }
                else if (VirtualRobot::AssimpReader::can_load(fullFilename))
                {
                    const auto& tri = VirtualRobot::AssimpReader{}.readFileAsTriMesh(fullFilename);
                    result = VirtualRobot::ManipulationObjectPtr(new VirtualRobot::ManipulationObject(filename, tri));
                }
                else
                {
                    ARMARX_WARNING << "Could not load object from file: " << fullFilename
                                   << "\nunknown extension '" << ext << "'";
                }
            }
            catch (std::exception const& ex)
            {
                ARMARX_WARNING << "Could not load object from file: " << fullFilename
                               << "\nReason: " << ex.what();
            }
 
            return result;
        }
 
        static std::vector<LoadedObject> objectcache;
 
        LoadedObject getObjectFromCache(std::string const& project, std::string const& filename)
        {
            // We can use a global variable, since this code is only executed in the GUI thread
 
            LoadedObject result;
 
            for (LoadedObject const& loaded : objectcache)
            {
                if (loaded.project == project && loaded.filename == filename)
                {
                    result = loaded;
                    result.object = loaded.object;
                    return result;
                }
            }
 
            result.project = project;
            result.filename = filename;
            result.object = loadObject(project, filename);
 
 
            objectcache.push_back(result);
 
            return result;
        }
    }
 
    bool VisualizationObject::update(ElementType const& element)
    {
        bool fileChanged = loaded.project != element.project || loaded.filename != element.filename;
        if (fileChanged)
        {
            // The robot file changed, so reload the robot
            loaded = getObjectFromCache(element.project, element.filename);
        }
        if (!loaded.object)
        {
            ARMARX_WARNING << deactivateSpam(120)
                           << "Object will not be visualized since it could not be loaded."
                           << "\nID: " << element.id
                           << "\nProject: " << element.project
                           << "\nFilename: " << element.filename;
            return true;
        }
 
        bool drawStyleChanged = loadedDrawStyle != element.drawStyle;
        if (fileChanged || drawStyleChanged)
        {
            recreateVisualizationNodes(element.drawStyle);
            loadedDrawStyle = element.drawStyle;
        }
 
        if (loadedDrawStyle & data::ModelDrawStyle::OVERRIDE_COLOR)
        {
            int numChildren = node->getNumChildren();
            for (int i = 0; i < numChildren; i++)
            {
                SoSeparator* nodeSep = static_cast<SoSeparator*>(node->getChild(i));
                // The first entry must be a SoMaterial (see recreateVisualizationNodes)
                SoMaterial* m = dynamic_cast<SoMaterial*>(nodeSep->getChild(0));
                if (!m)
                {
                    ARMARX_WARNING << "Error at node with index: " << i;
                    continue;
                }
 
                auto color = element.color;
                const float conv = 1.0f / 255.0f;
                float a = color.a * conv;
                SbColor coinColor(conv * color.r, conv * color.g, conv * color.b);
                m->diffuseColor = coinColor;
                m->ambientColor = coinColor;
                m->transparency = 1.0f - a;
                m->setOverride(true);
            }
        }
 
        return true;
    }
 
    void VisualizationObject::recreateVisualizationNodes(int drawStyle)
    {
        VirtualRobot::SceneObject::VisualizationType visuType = VirtualRobot::SceneObject::Full;
        if (drawStyle & data::ModelDrawStyle::COLLISION)
        {
            visuType = VirtualRobot::SceneObject::Collision;
        }
 
        node->removeAllChildren();
 
        VirtualRobot::ManipulationObject& object = *loaded.object;
 
        SoSeparator* nodeSep = new SoSeparator;
 
        // This material is used to color the nodes individually
        // We require it to be the first node in the separator for updates
        SoMaterial* nodeMat = new SoMaterial;
        nodeMat->setOverride(false);
        nodeSep->addChild(nodeMat);
 
        VirtualRobot::CoinVisualizationPtr nodeVisu = object.getVisualization<VirtualRobot::CoinVisualization>(visuType);
        if (nodeVisu)
        {
            SoNode* sepRobNode = nodeVisu->getCoinVisualization();
 
            if (sepRobNode)
            {
                nodeSep->addChild(sepRobNode);
            }
        }
 
        node->addChild(nodeSep);
    }
 
    void clearObjectCache()
    {
        objectcache.clear();
    }
}