SimpleVirtualRobot.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/>.
 *
 * @author     Fabian Reister ( fabian dot reister at kit dot edu )
 * @date       2021
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "SimpleVirtualRobot.h"
 
#include <SimoxUtility/json/json.hpp>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/XML/RobotIO.h>
 
#include <ArmarXCore/core/time/Frequency.h>
#include <ArmarXCore/core/time/Metronome.h>
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
 
 
namespace armarx::simple_virtual_robot
{
    SimpleVirtualRobot::SimpleVirtualRobot()
    {
        addPlugin(virtualRobotWriterPlugin);
    }
 
    armarx::PropertyDefinitionsPtr
    SimpleVirtualRobot::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
 
        // defs->topic(debugObserver);
 
        defs->optional(properties.oneShot, "p.oneShot", "If true, commit once after connecting, then stop.");
        defs->optional(properties.updateFrequency, "p.updateFrequency", "Memory update frequency (write).");
 
        defs->optional(properties.robot.name, "p.robot.name",
                       "Optional override for the robot name. If not set, the default name from the robot model is used.");
        defs->optional(properties.robot.package, "p.robot.package", "Package of the Simox robot XML.");
        defs->optional(properties.robot.path, "p.robot.path", "Local path of the Simox robot XML.");
 
        defs->optional(properties.robot.jointValues, "p.robot.jointValues", "Specify a certain joint configuration.");
 
        defs->optional(properties.robot.globalPositionX, "p.robot.globalPositionX", "");
        defs->optional(properties.robot.globalPositionY, "p.robot.globalPositionY", "");
        defs->optional(properties.robot.globalPositionYaw, "p.robot.globalPositionYaw", "");
 
        return defs;
    }
 
    std::string
    SimpleVirtualRobot::getDefaultName() const
    {
        return "SimpleVirtualRobot";
    }
 
    void
    SimpleVirtualRobot::onInitComponent()
    {
    }
 
    void
    SimpleVirtualRobot::onConnectComponent()
    {
        task = new armarx::SimpleRunningTask<>([this]()
        {
            this->run();
        });
        task->start();
    }
 
    void
    SimpleVirtualRobot::onDisconnectComponent()
    {
        task->stop();
    }
 
    void
    SimpleVirtualRobot::onExitComponent()
    {
    }
 
 
 
    VirtualRobot::RobotPtr
    SimpleVirtualRobot::loadRobot(const Properties::Robot& p) const
    {
        if (p.package.empty() or p.path.empty())
        {
            ARMARX_IMPORTANT << "Robot model not specified. "
                             << "Please specify the Simox robot XMl file in the properties.";
            return nullptr;
        }
 
        PackagePath path(p.package, p.path);
        ARMARX_INFO << "Load robot from " << path << ".";
 
        VirtualRobot::RobotPtr robot =
            VirtualRobot::RobotIO::loadRobot(path.toSystemPath(), VirtualRobot::RobotIO::eStructure);
 
        ARMARX_CHECK_NOT_NULL(robot) << "Failed to load robot `" << path  << "`.";
 
        if(not p.jointValues.empty())
        {
            ARMARX_DEBUG << "Parsing: " << p.jointValues;
 
            const nlohmann::json j = nlohmann::json::parse(p.jointValues);
            ARMARX_DEBUG << "JSON parsed as: " << j;
 
            std::map<std::string, float> jointValues;
            nlohmann::from_json(j, jointValues);
 
            ARMARX_VERBOSE << "The following joint values are given by the user: " << jointValues;
            robot->setJointValues(jointValues);
        }
 
        const Eigen::Isometry3f global_T_robot = Eigen::Translation3f{properties.robot.globalPositionX, properties.robot.globalPositionY, 0} * Eigen::AngleAxisf{properties.robot.globalPositionYaw, Eigen::Vector3f::UnitZ()};
        robot->setGlobalPose(global_T_robot.matrix());
 
        if (not p.name.empty())
        {
            robot->setName(p.name);
        }
 
        return robot;
    }
 
 
    void
    SimpleVirtualRobot::run()
    {
        armarx::Metronome metronome(armarx::Frequency::HertzDouble(properties.updateFrequency));
 
        while (task and not task->isStopped())
        {
            if (robot == nullptr)
            {
                robot = loadRobot(properties.robot);
                if (robot)
                {
                    ARMARX_INFO << "Start to commit robot '" << robot->getName()
                                << "' to the robot state memory.";
                }
                else
                {
                    ARMARX_INFO << "Loading robot failed.";
                    return;
                }
            }
 
            const armem::Time now = armem::Time::Now();
            ARMARX_DEBUG << "Report robot.";
 
            bool success = virtualRobotWriterPlugin->get().storeDescription(*robot, now);
            ARMARX_CHECK(success);
 
            success = virtualRobotWriterPlugin->get().storeState(*robot, now);
            ARMARX_CHECK(success);
 
            metronome.waitForNextTick();
 
            if (properties.oneShot)
            {
                // Done.
                return;
            }
        }
    }
 
} // namespace armarx::simple_virtual_robot