VirtualRobotReaderExampleClient.cpp

Go to the documentation of this file.

 
 
#include "VirtualRobotReaderExampleClient.h"
 
#include <ArmarXCore/core/time/Metronome.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.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