diff --git a/source/RobotAPI/components/KITProstheticHandUnit/KITProstheticHandUnit.cpp b/source/RobotAPI/components/KITProstheticHandUnit/KITProstheticHandUnit.cpp index 1f9d710a5ac1fa801bc4b28f9f24b2eb609cd5f0..f7bc0e29d1593cb0c481acf61fd8c331ac62a166 100644 --- a/source/RobotAPI/components/KITProstheticHandUnit/KITProstheticHandUnit.cpp +++ b/source/RobotAPI/components/KITProstheticHandUnit/KITProstheticHandUnit.cpp @@ -25,6 +25,8 @@ #include <ArmarXCore/core/exceptions/local/ExpressionException.h> +#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h> + #include "KITProstheticHandUnit.h" using namespace armarx; @@ -50,6 +52,91 @@ void KITProstheticHandUnit::onInitHandUnit() addShapeName("G6"); addShapeName("G7"); addShapeName("G8"); + + offeringTopic(getProperty<std::string>("DebugObserverName")); + if (!getProperty<std::string>("RemoteGuiName").getValue().empty()) + { + usingProxy(getProperty<std::string>("RemoteGuiName")); + } +} + +void KITProstheticHandUnit::onStartHandUnit() +{ + _debugObserver = getTopic<DebugObserverInterfacePrx>(getProperty<std::string>("DebugObserverName")); + if (!getProperty<std::string>("RemoteGuiName").getValue().empty()) + { + _remoteGuiPrx = getProxy<RemoteGuiInterfacePrx>(getProperty<std::string>("RemoteGuiName").getValue()); + + + RemoteGui::detail::VBoxLayoutBuilder rootLayoutBuilder = RemoteGui::makeVBoxLayout(); + + auto addFinger = [&](std::string name, float min, float max, float val, int steps) + { + rootLayoutBuilder.addChild( + RemoteGui::makeHBoxLayout() + .addChild(RemoteGui::makeTextLabel(name)) + .addChild(RemoteGui::makeTextLabel("min " + std::to_string(min))) + .addChild(RemoteGui::makeFloatSlider(name).min(min).max(max).value(val).steps(steps)) + .addChild(RemoteGui::makeTextLabel("max " + std::to_string(max))) + ); + rootLayoutBuilder.addChild( + RemoteGui::makeHBoxLayout() + .addChild(RemoteGui::makeTextLabel(name + " Pos ")) + .addChild(RemoteGui::makeLabel(name + "_pos").value("0")) + .addChild(new RemoteGui::HSpacer()) + ); + rootLayoutBuilder.addChild( + RemoteGui::makeHBoxLayout() + .addChild(RemoteGui::makeTextLabel(name + " PWM ")) + .addChild(RemoteGui::makeLabel(name + "_pwm").value("0")) + .addChild(new RemoteGui::HSpacer()) + ); + }; + + addFinger("Thumb", 0, 1, _lastGuiValueThumb, _driver->getMaxPosThumb()); + addFinger("Fingers", 0, 1, _lastGuiValueFingers, _driver->getMaxPosFingers()); + + rootLayoutBuilder.addChild(new RemoteGui::VSpacer()); + + _guiTask = new SimplePeriodicTask<>([&]() + { + _guiTab.receiveUpdates(); + _driver->getMaxPosThumb(); + const float t = _guiTab.getValue<float>("Thumb").get(); + const float f = _guiTab.getValue<float>("Fingers").get(); + + bool updateT = t != _lastGuiValueThumb; + bool updateF = f != _lastGuiValueFingers; + _lastGuiValueThumb = t; + _lastGuiValueFingers = f; + + if (updateT && updateF) + { + setJointAngles({{"Thumb", t}, {"Fingers", f}}); + } + else if (updateT) + { + setJointAngles({{"Thumb", t}}); + } + else if (updateF) + { + setJointAngles({{"Fingers", f}}); + } + + _guiTab.getValue<std::string>("Thumb_pos").set(std::to_string(_driver->getThumbPos())); + _guiTab.getValue<std::string>("Thumb_pwm").set(std::to_string(_driver->getThumbPWM())); + _guiTab.getValue<std::string>("Fingers_pos").set(std::to_string(_driver->getFingerPos())); + _guiTab.getValue<std::string>("Fingers_pwm").set(std::to_string(_driver->getFingerPWM())); + _guiTab.sendUpdates(); + }, 10); + + RemoteGui::WidgetPtr rootLayout = rootLayoutBuilder; + + _remoteGuiPrx->createTab("KITProstheticHandUnit", rootLayout); + _guiTab = RemoteGui::TabProxy(_remoteGuiPrx, "KITProstheticHandUnit"); + + _guiTask->start(); + } } void KITProstheticHandUnit::onExitHandUnit() diff --git a/source/RobotAPI/components/KITProstheticHandUnit/KITProstheticHandUnit.h b/source/RobotAPI/components/KITProstheticHandUnit/KITProstheticHandUnit.h index 1111d12fb925c7acf8cc824f07d3c4c586a0bed8..f4e13d0cce288ebe47bb08f7793d6868947bc62f 100644 --- a/source/RobotAPI/components/KITProstheticHandUnit/KITProstheticHandUnit.h +++ b/source/RobotAPI/components/KITProstheticHandUnit/KITProstheticHandUnit.h @@ -22,8 +22,12 @@ #pragma once - #include <ArmarXCore/core/Component.h> +#include <ArmarXCore/core/services/tasks/TaskUtil.h> +#include <ArmarXCore/interface/observers/ObserverInterface.h> + +#include <ArmarXGui/interface/RemoteGuiInterface.h> +#include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h> #include <RobotAPI/components/units/HandUnit.h> #include <RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.h> @@ -42,7 +46,8 @@ namespace armarx armarx::HandUnitPropertyDefinitions(prefix) { defineRequiredProperty<std::string>("MAC", "The KIT Prosthetic Hand's Bluetooth MAC"); - //defineOptionalProperty<std::string>("PropertyName", "DefaultValue", "Description"); + defineOptionalProperty<std::string>("RemoteGuiName", "", "Name of the remote gui"); + defineOptionalProperty<std::string>("DebugObserverName", "DebugObserver", "Name of the debug observer that should be used"); } }; @@ -70,7 +75,7 @@ namespace armarx } void onInitHandUnit() override; - void onStartHandUnit() override {} + void onStartHandUnit() override; void onExitHandUnit() override; void setJointAngles(const NameValueMap& targetJointAngles, const Ice::Current& = GlobalIceCurrent) override; NameValueMap getCurrentJointValues(const Ice::Current&) override; @@ -90,5 +95,13 @@ namespace armarx private: std::unique_ptr<BLEProthesisInterface> _driver; std::map<std::string, std::map<std::string, float>> _shapes; + + DebugObserverInterfacePrx _debugObserver; + //gui + RemoteGuiInterfacePrx _remoteGuiPrx; + SimplePeriodicTask<>::pointer_type _guiTask; + RemoteGui::TabProxy _guiTab; + float _lastGuiValueThumb{0}; + float _lastGuiValueFingers{0}; }; }