VirtualRobotReaderExampleClient.cpp

Go to the documentation of this file.

 
 
#include "VirtualRobotReaderExampleClient.h"
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/time/Metronome.h>
 
#include <RobotAPI/libraries/armem/core/Time.h>
 
namespace armarx::robot_state
{
    VirtualRobotReaderExampleClient::VirtualRobotReaderExampleClient() = default;
 
    armarx::PropertyDefinitionsPtr
    VirtualRobotReaderExampleClient::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr defs =
            new ComponentPropertyDefinitions(getConfigIdentifier());
 
        defs->topic(debugObserver);
 
        defs->optional(properties.robotName, "p.robotName", "The name of the robot to use.");
        defs->optional(properties.updateFrequency,
                       "p.updateFrequency [Hz]",
                       "The frequency of the running loop.");
 
        return defs;
    }
 
    std::string
    VirtualRobotReaderExampleClient::getDefaultName() const
    {
        return "VirtualRobotReaderExampleClient";
    }
 
    void
    VirtualRobotReaderExampleClient::onInitComponent()
    {
    }
 
    void
    VirtualRobotReaderExampleClient::onConnectComponent()
    {
        ARMARX_IMPORTANT << "Running virtual robot synchronization example.";
 
        task = new SimpleRunningTask<>([this]() { this->run(); });
        task->start();
    }
 
    void
    VirtualRobotReaderExampleClient::onDisconnectComponent()
    {
        task->stop();
    }
 
    void
    VirtualRobotReaderExampleClient::onExitComponent()
    {
    }
 
    void
    VirtualRobotReaderExampleClient::run()
    {
        Metronome metronome(Frequency::Hertz(properties.updateFrequency));
        while (task and not task->isStopped())
        {
            const armem::Time now = armem::Time::Now();
 
            // Initialize the robot if needed.
            if (robot == nullptr)
            {
                // The TIMING_* macros are optional, you do not need them.
                TIMING_START(getRobot);
 
                robot = virtualRobotReaderPlugin->get().getRobot(properties.robotName, now);
                if (robot)
                {
                    // Only print timing once the robot is loadable & loaded.
                    TIMING_END_STREAM(getRobot, ARMARX_INFO);
                }
                else
                {
                    ARMARX_WARNING << deactivateSpam(10) << "Could not create virtual robot.";
                }
            }
            if (robot)
            {
                ARMARX_INFO << deactivateSpam(60) << "Synchronizing robot.";
 
                TIMING_START(synchronizeRobot);
                ARMARX_CHECK(virtualRobotReaderPlugin->get().synchronizeRobot(*robot, now));
                TIMING_END_STREAM(synchronizeRobot, ARMARX_INFO);
 
 
                // Do something with the robot (your code follows here, there are just a examples) ...
 
                Eigen::Matrix4f globalPose = robot->getGlobalPose();
                (void)globalPose;
 
                std::vector<std::string> nodeNames = robot->getRobotNodeNames();
                (void)nodeNames;
 
                // End.
            }
 
            metronome.waitForNextTick();
        }
    }
 
} // namespace armarx::robot_state