diff --git a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp index 5c7aba61746d88dd5fc3e04e5e0898f3724d201b..d1e48cdea6aa941033225c291e50a3da312f9475 100644 --- a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp +++ b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp @@ -20,8 +20,8 @@ */ #include "ArticulatedObjectLocalizerExample.h" -#include "ArmarXCore/core/logging/Logging.h" -#include "RobotAPI/libraries/armem_objects/types.h" +#include "ArmarXCore/core/services/tasks/PeriodicTask.h" + #include <memory> @@ -33,6 +33,7 @@ #include <VirtualRobot/XML/RobotIO.h> #include <VirtualRobot/VirtualRobot.h> +#include <ArmarXCore/core/logging/Logging.h> #include <ArmarXCore/core/PackagePath.h> #include <ArmarXCore/core/system/ArmarXDataPath.h> #include <ArmarXCore/core/exceptions/local/ExpressionException.h> @@ -43,6 +44,7 @@ #include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h> #include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h> #include <RobotAPI/libraries/armem/core/Time.h> +#include <RobotAPI/libraries/armem_objects/types.h> namespace armarx::articulated_object @@ -58,7 +60,7 @@ namespace armarx::articulated_object defs->topic(debugObserver); - // defs->optional(memoryName, "mem.MemoryName", "Name of the memory to use."); + defs->optional(p.updateFrequency, "updateFrequency", "Memory update frequency (write)."); articulatedObjectWriter->registerPropertyDefinitions(defs); articulatedObjectReader->registerPropertyDefinitions(defs); @@ -78,7 +80,10 @@ namespace armarx::articulated_object articulatedObjectWriter->connect(); articulatedObjectReader->connect(); - task = new RunningTask<ArticulatedObjectLocalizerExample>(this, &ArticulatedObjectLocalizerExample::run); + ARMARX_IMPORTANT << "Running example."; + start = armem::Time::now(); + + task = new PeriodicTask<ArticulatedObjectLocalizerExample>(this, &ArticulatedObjectLocalizerExample::run, 1000 / p.updateFrequency); task->start(); } @@ -134,51 +139,34 @@ namespace armarx::articulated_object void ArticulatedObjectLocalizerExample::run() { - ARMARX_IMPORTANT << "Running example."; - - std::shared_ptr<VirtualRobot::Robot> dishwasher; - - - CycleUtil cycle(IceUtil::Time::milliSeconds(100)); - IceUtil::Time start = TimeUtil::GetTime(); - - CycleUtil c(100); - - while (not task->isStopped()) + if (dishwasher == nullptr) { - - if (dishwasher == nullptr) - { - dishwasher = createDishwasher(); - } + dishwasher = createDishwasher(); if (dishwasher == nullptr) // still { - c.waitForCycleDuration(); - continue; + return; } + } - ARMARX_DEBUG << "Reporting articulated objects"; + ARMARX_DEBUG << "Reporting articulated objects"; - const IceUtil::Time now = TimeUtil::GetTime(); - const float t = float((now - start).toSecondsDouble()); + const IceUtil::Time now = TimeUtil::GetTime(); + const float t = float((now - start).toSecondsDouble()); - // move joints at certain frequency - const float k = (1 + std::sin(t / (M_2_PIf32))) / 2; // in [0,1] + // move joints at certain frequency + const float k = (1 + std::sin(t / (M_2_PIf32))) / 2; // in [0,1] - const std::map<std::string, float> jointValues - { - {"dishwasher_door_joint", M_PIf32 / 2 * k}, - {"drawer_joint", 350 * k} - }; - - dishwasher->setJointValues(jointValues); + const std::map<std::string, float> jointValues + { + {"dishwasher_door_joint", M_PIf32 / 2 * k}, + {"drawer_joint", 350 * k} + }; - armarx::armem::articulated_object::ArticulatedObject armemDishwasher = convert(*dishwasher, IceUtil::Time::now()); - articulatedObjectWriter->store(armemDishwasher); + dishwasher->setJointValues(jointValues); - c.waitForCycleDuration(); - } + armarx::armem::articulated_object::ArticulatedObject armemDishwasher = convert(*dishwasher, IceUtil::Time::now()); + articulatedObjectWriter->store(armemDishwasher); } } // namespace armarx::articulated_object diff --git a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h index 9c6edb32ee395199fc15d504a993a9d0f75f92f5..75c0764cfb9c6103a82c68c0faba2b984ffabb86 100644 --- a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h +++ b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h @@ -3,6 +3,8 @@ // ArmarX +#include "ArmarXCore/core/services/tasks/PeriodicTask.h" +#include "RobotAPI/libraries/armem/core/Time.h" #include <ArmarXCore/core/Component.h> #include <ArmarXCore/interface/observers/ObserverInterface.h> #include <ArmarXCore/util/tasks.h> @@ -57,15 +59,23 @@ namespace armarx::articulated_object private: VirtualRobot::RobotPtr createDishwasher(); + std::shared_ptr<VirtualRobot::Robot> dishwasher; + /// Reference timestamp for object movement + armem::Time start; - armarx::RunningTask<ArticulatedObjectLocalizerExample>::pointer_type task; + armarx::PeriodicTask<ArticulatedObjectLocalizerExample>::pointer_type task; armarx::DebugObserverInterfacePrx debugObserver; std::unique_ptr<::armarx::armem::articulated_object::Writer> articulatedObjectWriter; std::unique_ptr<::armarx::armem::articulated_object::Reader> articulatedObjectReader; + struct Properties + { + float updateFrequency{25.F}; + } p; + }; } // namespace armarx::articulated_object