VisualizationRobot.cpp

Go to the documentation of this file.

#include <regex>
#include <fstream>
 
#include "VisualizationRobot.h"
 
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <VirtualRobot/SceneObject.h>
#include <VirtualRobot/XML/RobotIO.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
 
 
namespace armarx::viz::coin
{
    namespace
    {
        VirtualRobot::RobotPtr loadRobot(std::string const& project, std::string const& filename)
        {
            VirtualRobot::RobotPtr  result;
 
            if (filename.empty())
            {
                ARMARX_INFO << deactivateSpam() << "No filename provided for robot.";
                return nullptr;
            }
 
            std::string fullFilename;
            // Suppress warnings when full path is specified (is discouraged, but may happen).
            if (project.empty() && std::filesystem::path(filename).is_absolute())
            {
                ARMARX_INFO << deactivateSpam()
                            << "You specified the absolute path to the robot file:"
                            << "\n\t'" << filename << "'"
                            << "\nConsider specifying the containing ArmarX package and relative data path instead to "
                            << "improve portability to other systems.";
            }
            // We need to always check that the file is readable otherwise, VirtualRobot::RobotIO::loadRobot crashes
            ArmarXDataPath::FindPackageAndAddDataPath(project);
            if (!ArmarXDataPath::SearchReadableFile(filename, fullFilename))
            {
                ARMARX_INFO << deactivateSpam()
                            << "Unable to find readable file for name: " << filename;
                return nullptr;
            }
 
            try
            {
                // Always load full model. Visualization can choose between full and collision model
                VirtualRobot::RobotIO::RobotDescription loadMode = VirtualRobot::RobotIO::eFull;
                // This check is always true since it does not compare the flag to anything ==> if (true)
                // Deactivated for now
                //                if (data::ModelDrawStyle::COLLISION)
                //                {
                //                    loadMode = VirtualRobot::RobotIO::eCollisionModel;
                //                }
                ARMARX_INFO << "Loading robot from " << fullFilename;
                result = VirtualRobot::RobotIO::loadRobot(fullFilename, loadMode);
                if (result)
                {
                    result->setThreadsafe(false);
                    // Do we want to propagate joint values? Probably not...
                    // Closing the hand on the real robot could be implemented on another level
                    result->setPropagatingJointValuesEnabled(false);
                }
                else
                {
                    ARMARX_WARNING << "Could not load robot from file: " << fullFilename
                                   << "\nReason: loadRobot() returned nullptr";
                }
            }
            catch (std::exception const& ex)
            {
                ARMARX_WARNING << "Could not load robot from file: " << fullFilename
                               << "\nReason: " << ex.what();
            }
 
            return result;
        }
 
        struct RobotInstancePool
        {
            std::string project;
            std::string filename;
            std::size_t usedInstances = 0;
            std::vector<VirtualRobot::RobotPtr> robots;
        };
 
        static std::vector<RobotInstancePool> robotCache;
 
        LoadedRobot getRobotFromCache(std::string const& project, std::string const& filename)
        {
            // We can use a global variable, since this code is only executed in the GUI thread
 
            LoadedRobot result;
            result.project = project;
            result.filename = filename;
 
            for (RobotInstancePool& instancePool : robotCache)
            {
                if (instancePool.project == project && instancePool.filename == filename)
                {
                    // There are two possibilities here:
                    if (instancePool.usedInstances < instancePool.robots.size())
                    {
                        // 1) We have still unused instances in the pool ==> Just return one
                        ARMARX_DEBUG << "Reusing robot instance from cache " << VAROUT(project) << ", " << VAROUT(filename);
                        result.robot = instancePool.robots[instancePool.usedInstances];
                        instancePool.usedInstances += 1;
                    }
 
                    else
                    {
                        // 2) We do not have unused instances in the pool ==> Clone one
                        ARMARX_DEBUG << "Cloning robot from cache " << VAROUT(project) << ", " << VAROUT(filename);
 
                        if (instancePool.robots.size() > 0)
                        {
                            VirtualRobot::RobotPtr const& robotToClone = instancePool.robots.front();
                            float scaling = 1.0f;
                            bool preventCloningMeshesIfScalingIs1 = true;
                            result.robot = robotToClone->clone(nullptr, scaling, preventCloningMeshesIfScalingIs1);
 
                            // Insert the cloned robot into the instance pool
                            instancePool.robots.push_back(result.robot);
                            instancePool.usedInstances += 1;
                        }
                        else
                        {
                            ARMARX_WARNING << "Encountered empty robot instance pool while trying to clone new instance"
                                           << "\nRobot: " << VAROUT(project) << ", " << VAROUT(filename)
                                           << "\nUsed instances: " << instancePool.usedInstances
                                           << "\nRobots: " << instancePool.robots.size();
                        }
                    }
                    return result;
                }
            }
 
            ARMARX_DEBUG << "Loading robot from file  " << VAROUT(project) << ", " << VAROUT(filename);
            result.robot = loadRobot(project, filename);
            if (result.robot)
            {
                RobotInstancePool& instancePool = robotCache.emplace_back();
                instancePool.project = project;
                instancePool.filename = filename;
                instancePool.robots.push_back(result.robot);
                instancePool.usedInstances = 1;
            } else
            {
                ARMARX_WARNING << deactivateSpam(5) << "Robot " << VAROUT(project) << ", " << VAROUT(filename) << "could not be loaded!";
            }
            return result;
        }
    }
 
    VisualizationRobot::~VisualizationRobot()
    {
        for (RobotInstancePool& instancePool : robotCache)
        {
            if (instancePool.project == loaded.project && instancePool.filename == loaded.filename)
            {
                ARMARX_DEBUG << "Removing robot from chace " << VAROUT(loaded.project) << ", " << VAROUT(loaded.filename);
                std::vector<VirtualRobot::RobotPtr>& robots = instancePool.robots;
                auto robotIter = std::find(robots.begin(), robots.end(), loaded.robot);
                if (robotIter != robots.end())
                {
                    // Do not erase the robot, but rather move it to the back
                    // We can later reuse the unused instances at the back of the vector
                    std::swap(*robotIter, robots.back());
                    if (instancePool.usedInstances > 0)
                    {
                        instancePool.usedInstances -= 1;
                    }
                    else
                    {
                        ARMARX_WARNING << "Expected there to be at least one used instance "
                                       << "while trying to put robot instance back into the pool"
                                       << "\nRobot: " << VAROUT(loaded.project) << ", " << VAROUT(loaded.filename)
                                       << "\nUsed instances: " << instancePool.usedInstances;
                    }
                }
            }
        }
    }
 
    bool VisualizationRobot::update(ElementType const& element)
    {
        IceUtil::Time time_start = IceUtil::Time::now();
        (void) time_start;
 
        bool robotChanged = loaded.project != element.project || loaded.filename != element.filename;
        if (robotChanged)
        {
            // The robot file changed, so reload the robot
            loaded = getRobotFromCache(element.project, element.filename);
        }
        if (!loaded.robot)
        {
            ARMARX_WARNING << deactivateSpam(10)
                           << "Robot will not visualized since it could not be loaded."
                           << "\nID: " << element.id
                           << "\nProject: " << element.project
                           << "\nFilename: " << element.filename;
            return true;
        }
        //IceUtil::Time time_load = IceUtil::Time::now();
 
        bool drawStyleChanged = loadedDrawStyle != element.drawStyle;
        if (robotChanged || drawStyleChanged)
        {
            recreateVisualizationNodes(element.drawStyle);
            loadedDrawStyle = element.drawStyle;
        }
        //IceUtil::Time time_style = IceUtil::Time::now();
 
        // Set pose, configuration and so on
 
        VirtualRobot::Robot& robot = *loaded.robot;
 
        // We do not need to update the global pose in the robot
        // Since we only visualize the results, we can simply use the
        // pose from the Element base class.
 
        // Eigen::Matrix4f pose = defrost(element.pose);
        // robot.setGlobalPose(pose, false);
 
        // Check joint values for changes
 
        for (const auto& pair : element.jointValues)
        {
            std::string const& nodeName = pair.first;
            float newJointValue = pair.second;
            VirtualRobot::RobotNodePtr robotNode = robot.getRobotNode(nodeName);
 
            if (robotNode == nullptr)
            {
                continue;
            }
 
            const float oldJointValue = robotNode->getJointValue();
            const float diff = std::abs(newJointValue - oldJointValue);
            const bool jointValuesChanged = diff > 0.001f;
            if (jointValuesChanged)
            {
                // Only set the joint values if they changed
                // This call causes internal updates to the visualization even if joint angles are the same!
                robot.setJointValues(element.jointValues);
                break;
            }
        }
        //IceUtil::Time time_set = IceUtil::Time::now();
 
        if (loadedDrawStyle & data::ModelDrawStyle::OVERRIDE_COLOR)
        {
            if (loadedColor.r != element.color.r
                || loadedColor.g != element.color.g
                || loadedColor.b != element.color.b
                || loadedColor.a != element.color.a)
            {
                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);
                }
                loadedColor = element.color;
            }
        }
        //IceUtil::Time time_color = IceUtil::Time::now();
 
        // Setting the joint angles takes > 0.5 ms! This is unexpected...
        //        ARMARX_INFO << "Total:   " << (time_color - time_start).toMilliSecondsDouble()
        //                    << "\nLoad:  " << (time_load - time_start).toMilliSecondsDouble()
        //                    << "\nStyle: " << (time_style - time_load).toMilliSecondsDouble()
        //                    << "\nSet:   " << (time_set - time_style).toMilliSecondsDouble()
        //                    << "\nColor: " << (time_color - time_set).toMilliSecondsDouble();
 
        return true;
    }
 
    void VisualizationRobot::recreateVisualizationNodes(int drawStyle)
    {
        VirtualRobot::SceneObject::VisualizationType visuType = VirtualRobot::SceneObject::Full;
        if (drawStyle & data::ModelDrawStyle::COLLISION)
        {
            visuType = VirtualRobot::SceneObject::Collision;
        }
 
        node->removeAllChildren();
 
        auto& robot = *loaded.robot;
        for (size_t i = 0; i < robot.getRobotNodes().size(); ++i)
        {
            VirtualRobot::RobotNodePtr robNode = robot.getRobotNodes()[i];
            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);
 
            auto robNodeVisu = robNode->getVisualization<VirtualRobot::CoinVisualization>(visuType);
            if (robNodeVisu)
            {
                SoNode* sepRobNode = robNodeVisu->getCoinVisualization();
 
                if (sepRobNode)
                {
                    nodeSep->addChild(sepRobNode);
                }
            }
 
            node->addChild(nodeSep);
        }
    }
 
    void clearRobotCache()
    {
        robotCache.clear();
    }
}