diff --git a/source/RobotAPI/components/armem/client/CMakeLists.txt b/source/RobotAPI/components/armem/client/CMakeLists.txt index a7594d23e1668080fc0006afc44b1a09abfa8587..16f823e37de7efd21d6ac9f9633976a989ca0294 100644 --- a/source/RobotAPI/components/armem/client/CMakeLists.txt +++ b/source/RobotAPI/components/armem/client/CMakeLists.txt @@ -1,2 +1,3 @@ add_subdirectory(ExampleMemoryClient) add_subdirectory(ArticulatedObjectExampleMemoryWriterClient) +add_subdirectory(VirtualRobotReaderExampleClient) diff --git a/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/CMakeLists.txt b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..cd132bb87336bf793e86e79b7e048ea044921491 --- /dev/null +++ b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/CMakeLists.txt @@ -0,0 +1,29 @@ +armarx_component_set_name("VirtualRobotReaderExampleClient") + +find_package(IVT QUIET) +armarx_build_if(IVT_FOUND "IVT not available") + +set(COMPONENT_LIBS + ArmarXCore + ArmarXCoreInterfaces # for DebugObserverInterface + ArmarXGuiComponentPlugins + RobotAPICore + RobotAPIInterfaces + armem_robot_state +) + +set(SOURCES + VirtualRobotReaderExampleClient.cpp +) + +set(HEADERS + VirtualRobotReaderExampleClient.h +) + +armarx_add_component("${SOURCES}" "${HEADERS}") + +# add unit tests +# add_subdirectory(test) + +#generate the application +armarx_generate_and_add_component_executable(COMPONENT_NAMESPACE "armarx::robot_state") diff --git a/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp new file mode 100644 index 0000000000000000000000000000000000000000..1391e372accc073e3a15eaff4d951384ba6bdd34 --- /dev/null +++ b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp @@ -0,0 +1,111 @@ + + +#include "VirtualRobotReaderExampleClient.h" + + +#include <memory> + +#include <Eigen/Geometry> + +#include <IceUtil/Time.h> + +#include <VirtualRobot/Robot.h> +#include <VirtualRobot/XML/RobotIO.h> +#include <VirtualRobot/VirtualRobot.h> + +#include <ArmarXCore/core/PackagePath.h> +#include <ArmarXCore/core/system/ArmarXDataPath.h> +#include <ArmarXCore/core/exceptions/local/ExpressionException.h> +#include <ArmarXCore/core/time/CycleUtil.h> +#include <ArmarXCore/core/logging/Logging.h> +#include <ArmarXCore/core/time/TimeUtil.h> + +#include <RobotAPI/libraries/armem/client/query/Builder.h> +#include <RobotAPI/libraries/armem/client/query/query_fns.h> +#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h> +#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h> +#include <RobotAPI/libraries/armem/core/Time.h> + + +namespace armarx::robot_state +{ + VirtualRobotReaderExampleClient::VirtualRobotReaderExampleClient() : + virtualRobotReader(*this) {} + + armarx::PropertyDefinitionsPtr VirtualRobotReaderExampleClient::createPropertyDefinitions() + { + armarx::PropertyDefinitionsPtr defs = + new ComponentPropertyDefinitions(getConfigIdentifier()); + + defs->topic(debugObserver); + + virtualRobotReader.registerPropertyDefinitions(defs); + + return defs; + } + + std::string VirtualRobotReaderExampleClient::getDefaultName() const + { + return "VirtualRobotReaderExampleClient"; + } + + void VirtualRobotReaderExampleClient::onInitComponent() {} + + void VirtualRobotReaderExampleClient::onConnectComponent() + { + virtualRobotReader.connect(); + + task = new RunningTask<VirtualRobotReaderExampleClient>(this, &VirtualRobotReaderExampleClient::run); + task->start(); + } + + void VirtualRobotReaderExampleClient::onDisconnectComponent() + { + task->stop(); + } + + void VirtualRobotReaderExampleClient::onExitComponent() {} + + void VirtualRobotReaderExampleClient::run() + { + ARMARX_IMPORTANT << "Running virtual robot synchronization example."; + + std::shared_ptr<VirtualRobot::Robot> virtualRobot{nullptr}; + + CycleUtil cycle(IceUtil::Time::milliSeconds(100)); + IceUtil::Time start = TimeUtil::GetTime(); + + CycleUtil c(100); + + while (not task->isStopped()) + { + // initialize + if(virtualRobot == nullptr) + { + TIMING_START(getRobot); + + virtualRobot = virtualRobotReader.getRobot("Armar6", IceUtil::Time::now()); + + if(virtualRobot == nullptr) + { + ARMARX_WARNING << "Could not create virtual robot."; + c.waitForCycleDuration(); + continue; + } + // only print timing once the robot is loadable & loaded + TIMING_END_STREAM(getRobot, ARMARX_INFO); + } + + ARMARX_INFO << "Synchronizing robot"; + + const IceUtil::Time now = TimeUtil::GetTime(); + + TIMING_START(synchronizeRobot); + virtualRobotReader.synchronizeRobot(*virtualRobot, now); + TIMING_END_STREAM(synchronizeRobot, ARMARX_INFO); + + c.waitForCycleDuration(); + } + } + +} // namespace armarx::robot_state diff --git a/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.h b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.h new file mode 100644 index 0000000000000000000000000000000000000000..aeb985c2edadcfac0cc2854c7789ec0004bb9cfa --- /dev/null +++ b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.h @@ -0,0 +1,88 @@ + +/* + * 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 + */ + +#pragma once + + +// ArmarX +#include <ArmarXCore/core/Component.h> +#include <ArmarXCore/interface/observers/ObserverInterface.h> +#include <ArmarXCore/util/tasks.h> +#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h> + +// RobotAPI +#include <RobotAPI/interface/armem/server/MemoryInterface.h> +#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h> +#include <RobotAPI/libraries/armem/client/ComponentPlugin.h> +#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h> + +#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h> + +namespace armarx::robot_state +{ + + /** + * @defgroup Component-ExampleClient ExampleClient + * @ingroup RobotAPI-Components + * A description of the component ExampleClient. + * + * @class ExampleClient + * @ingroup Component-ExampleClient + * @brief Brief description of class ExampleClient. + * + * Detailed description of class ExampleClient. + */ + class VirtualRobotReaderExampleClient : + virtual public armarx::Component, + virtual public armarx::armem::client::ComponentPluginUser + { + public: + VirtualRobotReaderExampleClient(); + + /// @see armarx::ManagedIceObject::getDefaultName() + std::string getDefaultName() const override; + + protected: + + armarx::PropertyDefinitionsPtr createPropertyDefinitions() override; + + void onInitComponent() override; + void onConnectComponent() override; + void onDisconnectComponent() override; + void onExitComponent() override; + + void run(); + + + private: + + armarx::RunningTask<VirtualRobotReaderExampleClient>::pointer_type task; + + armarx::DebugObserverInterfacePrx debugObserver; + + // std::unique_ptr<::armarx::armem::articulated_object::Writer> articulatedObjectWriter; + + armem::robot_state::VirtualRobotReader virtualRobotReader; + + }; + +} // namespace armarx::robot_state