RobotToArViz.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    RobotAPI::ArmarXObjects::RobotToArViz
 * @author     Rainer Kartmann ( rainer dot kartmann at kit dot edu )
 * @date       2020
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "RobotToArViz.h"
 
#include <filesystem>
 
#include <ArmarXCore/core/time/Metronome.h>
 
 
namespace armarx
{
    RobotToArVizPropertyDefinitions::RobotToArVizPropertyDefinitions(std::string prefix) :
        armarx::ComponentPropertyDefinitions(prefix)
    {
    }
 
    armarx::PropertyDefinitionsPtr RobotToArViz::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr defs(new RobotToArVizPropertyDefinitions(getConfigIdentifier()));
 
        // defs->optional(updateFrequency, "updateFrequency", "Target number of updates per second.");
        defs->defineOptionalProperty("updateFrequency", updateFrequencyHz, "Target number of updates per second.");
        defs->optional(gui.useCollisionModel, "UseCollisionModel", "Use the collision model for visualization");
 
        defs->optional(gui.showRobotNodeFrames, "ShowRobotNodeFrames",
                       "If true, show frames of robot nodes (can be changed in RemoteGui).");
 
        return defs;
    }
 
 
    std::string RobotToArViz::getDefaultName() const
    {
        return "RobotToArViz";
    }
 
 
    void RobotToArViz::onInitComponent()
    {
        getProperty(updateFrequencyHz, "updateFrequency");
    }
 
 
    void RobotToArViz::onConnectComponent()
    {
        // Load robot.
        if (!RobotState::hasRobot(robotName))
        {
            this->robot = RobotState::addRobot(robotName, VirtualRobot::RobotIO::RobotDescription::eStructure);
        }
 
        // Initialize robot visu element.
        this->robotViz = viz::Robot(robot->getName());
 
        // Set file location. Try to use ArmarX data directory.
 
        if (!trySetFilePathFromDataDir(this->robotViz, robot->getFilename()))
        {
            // Use absolute path.
            this->robotViz.file("", robot->getFilename());
        }
 
        createRemoteGuiTab();
        RemoteGui_startRunningTask();
 
        task = new SimpleRunningTask<>([this]()
        {
            this->updateRobotRun();
        });
        task->start();
    }
 
 
    void RobotToArViz::onDisconnectComponent()
    {
        task->stop();
        task = nullptr;
    }
 
 
    void RobotToArViz::onExitComponent()
    {
    }
 
    void RobotToArViz::createRemoteGuiTab()
    {
        using namespace RemoteGui::Client;
 
        GridLayout root;
        int row = 0;
        {
            tab.showRobotNodeFrames.setValue(gui.showRobotNodeFrames);
            root.add(Label("Show Robot Node Frames"), {row, 0}).add(tab.showRobotNodeFrames, {row, 1});
            row += 1;
 
            tab.useCollisionModel.setValue(gui.useCollisionModel);
            root.add(Label("Use Collision Model"), {row, 0}).add(tab.useCollisionModel, {row, 1});
            row += 1;
        }
        RemoteGui_createTab(getName(), root, &tab);
    }
 
    void RobotToArViz::RemoteGui_update()
    {
        if (tab.showRobotNodeFrames.hasValueChanged())
        {
            std::scoped_lock lock(guiMutex);
            gui.showRobotNodeFrames = tab.showRobotNodeFrames.getValue();
        }
        if (tab.useCollisionModel.hasValueChanged())
        {
            std::scoped_lock lock(guiMutex);
            gui.useCollisionModel = tab.useCollisionModel.getValue();
        }
    }
 
 
    void RobotToArViz::updateRobotRun()
    {
        Metronome metronome(Frequency::Hertz(updateFrequencyHz));
        while (task and not task->isStopped())
        {
            updateRobotOnce();
 
            metronome.waitForNextTick();
        }
    }
 
 
    void RobotToArViz::updateRobotOnce()
    {
        RobotState::synchronizeLocalClone(robotName);
 
        Gui gui;
        {
            std::scoped_lock lock(guiMutex);
            gui = this->gui;
        }
 
        std::vector<viz::Layer> layers;
 
        robotViz.joints(robot->getConfig()->getRobotNodeJointValueMap())
        .pose(robot->getGlobalPose());
 
        if (gui.useCollisionModel)
        {
            robotViz.useCollisionModel();
        }
        else
        {
            robotViz.useFullModel();
        }
 
        viz::Layer& layerRobot = layers.emplace_back(arviz.layer(robot->getName()));
        layerRobot.add(robotViz);
 
 
        // Still commit layer if disabled to clear it.
        viz::Layer& layerFrames = layers.emplace_back(arviz.layer(robot->getName() + " Frames"));
        if (gui.showRobotNodeFrames)
        {
            for (const auto& node : robot->getRobotNodes())
            {
                layerFrames.add(viz::Pose(node->getName()).pose(node->getGlobalPose()));
            }
        }
 
        arviz.commit(layers);
    }
 
 
    bool RobotToArViz::trySetFilePathFromDataDir(viz::Robot& robotViz, const std::string& absolutePath)
    {
        // Set file location. Try to use ArmarX data directory.
        std::filesystem::path filepath = absolutePath;
 
        for (auto it = filepath.begin(); it != filepath.end(); ++it)
        {
            if (*it == "data")
            {
                auto jt = it;
                ++jt;
                std::filesystem::path localPath = *jt;
                ++jt;
                for (; jt != filepath.end(); ++jt)
                {
                    localPath /= *jt;
                }
 
                --it;
                std::string package = *it;
 
                robotViz.file(package, localPath.string());
 
                return true;
            }
        }
        return false;
    }
}