VirtualRobotWriter.cpp

Go to the documentation of this file.

#include "VirtualRobotWriter.h"
 
#include <optional>
#include <thread>
 
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/XML/RobotIO.h>
 
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/time/Clock.h>
 
#include "RobotAPI/libraries/armem/core/forward_declarations.h"
#include "RobotAPI/libraries/armem_robot/types.h"
 
#include "constants.h"
 
namespace armarx::armem::robot_state
{
 
    VirtualRobotWriter::~VirtualRobotWriter() = default;
 
    void
    VirtualRobotWriter::connect(armem::client::MemoryNameSystem& memoryNameSystem)
    {
        RobotWriter::connect(memoryNameSystem);
    }
 
    // TODO(fabian.reister): register property defs
    void
    VirtualRobotWriter::registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def)
    {
        RobotWriter::registerPropertyDefinitions(def);
    }
 
    PackagePath
    resolvePackagePath(const std::string& filename)
    {
        const static auto packages = armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
        PackagePath packagePath(armarx::ArmarXDataPath::getProject(packages, filename), filename);
        return packagePath;
    }
 
    bool
    VirtualRobotWriter::storeDescription(const VirtualRobot::Robot& robot,
                                         const armem::Time& timestamp)
    {
        const auto filename = robot.getFilename();
 
        const robot::RobotDescription desc{.name = robot.getType(),
                                           .xml = resolvePackagePath(filename)};
 
        return RobotWriter::storeDescription(desc, timestamp);
    }
 
    bool
    VirtualRobotWriter::storeState(const VirtualRobot::Robot& robot, const armem::Time& timestamp)
    {
        const robot::RobotState robotState{.timestamp = timestamp,
                                           .globalPose =
                                               robot::RobotState::Pose(robot.getGlobalPose()),
                                           .jointMap = robot.getJointValues(),
                                           .proprioception = std::nullopt};
 
        return RobotWriter::storeState(
            robotState,
            robot.getType(),
            robot.getName(),
            constants::robotRootNodeName /*robot.getRootNode()->getName()*/);
    }
 
 
} // namespace armarx::armem::robot_state