diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
index 1973976129415be09d640896b3bd2ec68736e60e..47c864d4d985762022e101d9187ec901db9f973b 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
@@ -25,7 +25,14 @@
#include "KinematicUnitConfigDialog.h"
#include <RobotAPI/gui-plugins/KinematicUnitPlugin/ui_KinematicUnitConfigDialog.h>
-
+#include <RobotAPI/interface/core/NameValueMap.h>
+#include <RobotAPI/interface/units/KinematicUnitInterface.h>
+
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+#include "ArmarXCore/core/logging/Logging.h"
+#include "ArmarXCore/core/services/tasks/RunningTask.h"
+#include "ArmarXCore/core/time/Metronome.h"
+#include "ArmarXCore/core/time/forward_declarations.h"
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
@@ -51,6 +58,7 @@
#include <QTableWidget>
#include <QClipboard>
#include <QInputDialog>
+#include <qtimer.h>
#include <Inventor/SoDB.h>
#include <Inventor/Qt/SoQt.h>
@@ -58,6 +66,7 @@
// System
#include <stdio.h>
+#include <memory>
#include <string>
#include <string.h>
#include <stdlib.h>
@@ -79,6 +88,9 @@ using namespace VirtualRobot;
KinematicUnitGuiPlugin::KinematicUnitGuiPlugin()
{
+
+ qRegisterMetaType<DebugInfo>("DebugInfo");
+
addWidget<KinematicUnitWidgetController>();
}
@@ -86,8 +98,7 @@ KinematicUnitWidgetController::KinematicUnitWidgetController() :
kinematicUnitNode(nullptr),
enableValueValidator(true),
historyTime(100000), // 1/10 s
- currentValueMax(5.0f),
- selectedControlMode(ePositionControl)
+ currentValueMax(5.0f)
{
rootVisu = NULL;
debugLayerVisu = NULL;
@@ -97,6 +108,8 @@ KinematicUnitWidgetController::KinematicUnitWidgetController() :
getWidget()->setEnabled(false);
ui.tableJointList->setItemDelegateForColumn(eTabelColumnAngleProgressbar, &delegate);
+
+ ui.radioButtonUnknown->setHidden(true);
}
void KinematicUnitWidgetController::onInitComponent()
@@ -148,20 +161,12 @@ void KinematicUnitWidgetController::onInitComponent()
void KinematicUnitWidgetController::onConnectComponent()
{
// ARMARX_INFO << "Kinematic Unit Gui :: onConnectComponent()";
- reportedJointAngles.clear();
- reportedJointVelocities.clear();
- reportedJointControlModes.clear();
- reportedJointTorques.clear();
jointCurrentHistory.clear();
- reportedJointStatuses.clear();
- reportedJointTemperatures.clear();
- jointAnglesUpdateFrequency = new filters::MedianFilter(100);
+ // jointAnglesUpdateFrequency = new filters::MedianFilter(100);
kinematicUnitInterfacePrx = getProxy<KinematicUnitInterfacePrx>(kinematicUnitName);
- topicName = kinematicUnitInterfacePrx->getReportTopicName();
- usingTopic(topicName);
- lastJointAngleUpdateTimestamp = TimeUtil::GetTime().toSecondsDouble();
+ lastJointAngleUpdateTimestamp = Clock::Now();
robotVisu->removeAllChildren();
robot.reset();
@@ -248,24 +253,48 @@ void KinematicUnitWidgetController::onConnectComponent()
initGUIJointListTable(robotNodeSet);
// Init control mode map
- try
- {
- reportedJointControlModes = kinematicUnitInterfacePrx->getControlModes();
- }
- catch (...)
- {
- }
+ // try
+ // {
+ // reportedJointControlModes = kinematicUnitInterfacePrx->getControlModes();
+ // }
+ // catch (...)
+ // {
+ // }
+
+ const auto initialDebugInfo = kinematicUnitInterfacePrx->getDebugInfo();
- initializeUi();
+ initializeUi(initialDebugInfo);
QMetaObject::invokeMethod(this, "resetSlider");
enableMainWidgetAsync(true);
- updateTimerId = startTimer(1000); // slow timer that only ensures that skipped updates are shown at all
+
+ updateTask = new RunningTask<KinematicUnitWidgetController>(this, &KinematicUnitWidgetController::runUpdate);
+ updateTask->start();
+}
+
+void KinematicUnitWidgetController::runUpdate()
+{
+ Metronome metronome(Frequency::Hertz(10));
+
+ while(kinematicUnitInterfacePrx)
+ {
+ fetchData();
+ metronome.waitForNextTick();
+ }
+
+ ARMARX_INFO << "Connection to kinemetic unit lost. Update task terminates.";
}
void KinematicUnitWidgetController::onDisconnectComponent()
{
- killTimer(updateTimerId);
+ if(updateTask)
+ {
+ updateTask->stop();
+ updateTask->join();
+ updateTask = nullptr;
+ }
+
+ // killTimer(updateTimerId);
enableMainWidgetAsync(false);
{
@@ -284,6 +313,13 @@ void KinematicUnitWidgetController::onDisconnectComponent()
void KinematicUnitWidgetController::onExitComponent()
{
+ if(updateTask)
+ {
+ updateTask->stop();
+ updateTask->join();
+ updateTask = nullptr;
+ }
+
enableMainWidgetAsync(false);
{
@@ -383,13 +419,18 @@ void KinematicUnitWidgetController::copyToClipboard()
{
std::unique_lock lock(mutexNodeSet);
+ ARMARX_CHECK_NOT_NULL(kinematicUnitInterfacePrx);
+ const auto debugInfo = kinematicUnitInterfacePrx->getDebugInfo();
+
+ const auto selectedControlMode = getSelectedControlMode();
+
if (selectedControlMode == ePositionControl)
{
- values = reportedJointAngles;
+ values = debugInfo.jointAngles;
}
else if (selectedControlMode == eVelocityControl)
{
- values = reportedJointVelocities;
+ values = debugInfo.jointVelocities;
}
}
@@ -463,50 +504,61 @@ void KinematicUnitWidgetController::connectSlots()
connect(ui.checkBoxUseDegree, SIGNAL(clicked()), this, SLOT(setControlModePosition()));
connect(ui.checkBoxUseDegree, SIGNAL(clicked()), this, SLOT(setControlModeVelocity()));
connect(ui.checkBoxUseDegree, SIGNAL(clicked()), this, SLOT(setControlModeTorque()));
+
+ connect(this, SIGNAL(onDebugInfoReceived(const DebugInfo&)), this, SLOT(debugInfoReceived(const DebugInfo&)));
}
-void KinematicUnitWidgetController::initializeUi()
+void KinematicUnitWidgetController::initializeUi(const DebugInfo& debugInfo)
{
//signal clicked is not emitted if you call setDown(), setChecked() or toggle().
- ui.radioButtonPositionControl->setChecked(true);
- ui.widgetSliderFactor->setVisible(false);
- setControlModePosition();
- updateControlModesTable();
-}
-void KinematicUnitWidgetController::kinematicUnitZeroPosition()
-{
- if (!robotNodeSet)
- {
- return;
- }
+ // there is no default control mode
+ setControlModeRadioButtonGroup(ControlMode::eUnknown);
- std::unique_lock lock(mutexNodeSet);
- std::vector< RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
- NameValueMap jointAngles;
- NameControlModeMap jointModes;
+ ui.widgetSliderFactor->setVisible(false);
- for (unsigned int i = 0; i < rn.size(); i++)
- {
- jointModes[rn[i]->getName()] = ePositionControl;
- jointAngles[rn[i]->getName()] = 0.0f;
- }
+ fetchData();
- try
- {
- kinematicUnitInterfacePrx->switchControlMode(jointModes);
- kinematicUnitInterfacePrx->setJointAngles(jointAngles);
- }
- catch (...)
- {
- }
+ // setControlModePosition(); // FIXME purge this!
+ // updateControlModesTable();
- // Set slider to 0 if position control mode is used.
- if (selectedControlMode == ePositionControl)
- {
- ui.horizontalSliderKinematicUnitPos->setSliderPosition(SLIDER_ZERO_POSITION);
- }
}
+
+// void KinematicUnitWidgetController::kinematicUnitZeroPosition()
+// {
+// if (!robotNodeSet)
+// {
+// return;
+// }
+
+// std::unique_lock lock(mutexNodeSet);
+// std::vector< RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
+// NameValueMap jointAngles;
+// NameControlModeMap jointModes;
+
+// for (unsigned int i = 0; i < rn.size(); i++)
+// {
+// jointModes[rn[i]->getName()] = ePositionControl;
+// jointAngles[rn[i]->getName()] = 0.0f;
+// }
+
+// try
+// {
+// kinematicUnitInterfacePrx->switchControlMode(jointModes);
+// kinematicUnitInterfacePrx->setJointAngles(jointAngles);
+// }
+// catch (...)
+// {
+// }
+
+// // Set slider to 0 if position control mode is used.
+// if (selectedControlMode == ePositionControl)
+// {
+// ui.horizontalSliderKinematicUnitPos->setSliderPosition(SLIDER_ZERO_POSITION);
+// }
+// }
+
+
void KinematicUnitWidgetController::kinematicUnitZeroVelocity()
{
if (!robotNodeSet)
@@ -534,46 +586,49 @@ void KinematicUnitWidgetController::kinematicUnitZeroVelocity()
{
}
+ const auto selectedControlMode = getSelectedControlMode();
if (selectedControlMode == eVelocityControl)
{
ui.horizontalSliderKinematicUnitPos->setSliderPosition(SLIDER_ZERO_POSITION);
}
}
-void KinematicUnitWidgetController::kinematicUnitZeroTorque()
-{
- if (!robotNodeSet)
- {
- return;
- }
-
- std::unique_lock lock(mutexNodeSet);
- std::vector< RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
- NameValueMap tor;
- NameControlModeMap jointModes;
-
- for (unsigned int i = 0; i < rn.size(); i++)
- {
- jointModes[rn[i]->getName()] = eTorqueControl;
- tor[rn[i]->getName()] = 0.0f;
- }
-
- try
- {
- kinematicUnitInterfacePrx->switchControlMode(jointModes);
- kinematicUnitInterfacePrx->setJointTorques(tor);
- }
- catch (...)
- {
- }
-
- if (selectedControlMode == eVelocityControl)
- {
- ui.horizontalSliderKinematicUnitPos->setSliderPosition(SLIDER_ZERO_POSITION);
- }
-}
+// void KinematicUnitWidgetController::kinematicUnitZeroTorque()
+// {
+// if (!robotNodeSet)
+// {
+// return;
+// }
+
+// std::unique_lock lock(mutexNodeSet);
+// std::vector< RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
+// NameValueMap tor;
+// NameControlModeMap jointModes;
+
+// for (unsigned int i = 0; i < rn.size(); i++)
+// {
+// jointModes[rn[i]->getName()] = eTorqueControl;
+// tor[rn[i]->getName()] = 0.0f;
+// }
+
+// try
+// {
+// kinematicUnitInterfacePrx->switchControlMode(jointModes);
+// kinematicUnitInterfacePrx->setJointTorques(tor);
+// }
+// catch (...)
+// {
+// }
+
+// if (selectedControlMode == eVelocityControl)
+// {
+// ui.horizontalSliderKinematicUnitPos->setSliderPosition(SLIDER_ZERO_POSITION);
+// }
+// }
void KinematicUnitWidgetController::resetSlider()
{
+ const auto selectedControlMode = getSelectedControlMode();
+
if (selectedControlMode == eVelocityControl || selectedControlMode == eTorqueControl)
{
resetSliderToZeroPosition();
@@ -597,6 +652,8 @@ void KinematicUnitWidgetController::resetSlider()
void KinematicUnitWidgetController::resetSliderToZeroPosition()
{
+ const auto selectedControlMode = getSelectedControlMode();
+
if (selectedControlMode == eVelocityControl || selectedControlMode == eTorqueControl)
{
ui.horizontalSliderKinematicUnitPos->setSliderPosition(SLIDER_ZERO_POSITION);
@@ -604,6 +661,30 @@ void KinematicUnitWidgetController::resetSliderToZeroPosition()
}
}
+void KinematicUnitWidgetController::setControlModeRadioButtonGroup(const ControlMode& controlMode)
+{
+ ARMARX_INFO << "Setting control mode of radio button group to " << controlMode;
+
+ switch(controlMode)
+ {
+ case eDisabled:
+ case eUnknown:
+ case ePositionVelocityControl:
+ ui.radioButtonUnknown->setChecked(true);
+ break;
+ case ePositionControl:
+ ui.radioButtonPositionControl->setChecked(true);
+ break;
+ case eVelocityControl:
+ ui.radioButtonVelocityControl->setChecked(true);
+ break;
+ case eTorqueControl:
+ ui.radioButtonTorqueControl->setChecked(true);
+ break;
+ }
+
+}
+
void KinematicUnitWidgetController::setControlModePosition()
{
if (!ui.radioButtonPositionControl->isChecked())
@@ -611,9 +692,11 @@ void KinematicUnitWidgetController::setControlModePosition()
return;
}
NameControlModeMap jointModes;
- selectedControlMode = ePositionControl;
+ // selectedControlMode = ePositionControl;
ui.widgetSliderFactor->setVisible(false);
+ // FIXME currentNode should be passed to this function!
+
if (currentNode)
{
QString unit = currentNode->isRotationalJoint()
@@ -698,7 +781,6 @@ void KinematicUnitWidgetController::setControlModeVelocity()
}
- selectedControlMode = eVelocityControl;
ui.widgetSliderFactor->setVisible(true);
ui.horizontalSliderKinematicUnitPos->blockSignals(true);
@@ -709,6 +791,30 @@ void KinematicUnitWidgetController::setControlModeVelocity()
}
}
+ControlMode KinematicUnitWidgetController::getSelectedControlMode() const
+{
+ if(ui.radioButtonPositionControl->isChecked())
+ {
+ return ControlMode::ePositionControl;
+ }
+
+ if (ui.radioButtonVelocityControl->isChecked())
+ {
+ return ControlMode::eVelocityControl;
+ }
+
+ if (ui.radioButtonTorqueControl->isChecked())
+ {
+ return ControlMode::eTorqueControl;
+ }
+
+ // if no button is checked, then the joint is likely initialized but no controller has been loaded yet
+ // (well, the no movement controller should be active)
+ return ControlMode::eUnknown;
+
+
+}
+
void KinematicUnitWidgetController::setControlModeTorque()
{
if (!ui.radioButtonTorqueControl->isChecked())
@@ -739,7 +845,6 @@ void KinematicUnitWidgetController::setControlModeTorque()
ui.horizontalSliderKinematicUnitPos->setMaximum(20000.0);
ui.horizontalSliderKinematicUnitPos->setMinimum(-20000.0);
- selectedControlMode = eTorqueControl;
ui.widgetSliderFactor->setVisible(true);
ui.horizontalSliderKinematicUnitPos->blockSignals(false);
@@ -911,27 +1016,43 @@ void KinematicUnitWidgetController::selectJoint(int i)
{
std::unique_lock lock(mutexNodeSet);
- if (!robotNodeSet || i < 0 || i >= (int)robotNodeSet->getSize())
+ ARMARX_INFO << "Selected index: " << ui.nodeListComboBox->currentIndex();
+
+ if (!robotNodeSet || i < 0 || i >= static_cast<int>(robotNodeSet->getSize()))
{
return;
}
currentNode = robotNodeSet->getAllRobotNodes()[i];
+ ARMARX_IMPORTANT << "Selected joint is `" << currentNode->getName() << "`.";
/*
//ui.lcdNumberKinematicUnitJointValue->display(pos*180.0f/(float)M_PI);
ui.lcdNumberKinematicUnitJointValue->display(posT);
*/
- if (selectedControlMode == ePositionControl)
+
+ const auto controlModes = kinematicUnitInterfacePrx->getControlModes();
+ if(controlModes.count(currentNode->getName()) == 0)
+ {
+ ARMARX_ERROR << "Could not retrieve control mode for joint `" << currentNode->getName() << "` from kinematic unit!";
+ return;
+ }
+
+ // const auto selectedControlMode = getSelectedControlMode();
+ const auto controlMode = controlModes.at(currentNode->getName());
+ setControlModeRadioButtonGroup(controlMode);
+
+
+ if (controlMode == ePositionControl)
{
setControlModePosition();
}
- else if (selectedControlMode == eVelocityControl)
+ else if (controlMode == eVelocityControl)
{
setControlModeVelocity();
ui.horizontalSliderKinematicUnitPos->setSliderPosition(SLIDER_ZERO_POSITION);
}
- else if (selectedControlMode == eTorqueControl)
+ else if (controlMode == eTorqueControl)
{
setControlModeTorque();
ui.horizontalSliderKinematicUnitPos->setSliderPosition(SLIDER_ZERO_POSITION);
@@ -963,7 +1084,8 @@ void KinematicUnitWidgetController::sliderValueChanged(int pos)
// NameControlModeMap::const_iterator it;
// it = reportedJointControlModes.find(currentNode->getName());
// const ControlMode currentControlMode = it->second;
- const ControlMode currentControlMode = selectedControlMode;
+ const ControlMode currentControlMode = getSelectedControlMode();
+
const bool isDeg = ui.checkBoxUseDegree->isChecked();
const bool isRot = currentNode->isRotationalJoint();
@@ -990,7 +1112,7 @@ void KinematicUnitWidgetController::sliderValueChanged(int pos)
catch (...)
{
}
- updateModel();
+ // updateModel();
}
}
else if (currentControlMode == eVelocityControl)
@@ -1009,7 +1131,7 @@ void KinematicUnitWidgetController::sliderValueChanged(int pos)
catch (...)
{
}
- updateModel();
+ // updateModel();
}
}
else if (currentControlMode == eTorqueControl)
@@ -1028,7 +1150,7 @@ void KinematicUnitWidgetController::sliderValueChanged(int pos)
catch (...)
{
}
- updateModel();
+ // updateModel();
}
}
else
@@ -1039,7 +1161,7 @@ void KinematicUnitWidgetController::sliderValueChanged(int pos)
-void KinematicUnitWidgetController::updateControlModesTable()
+void KinematicUnitWidgetController::updateControlModesTable(const NameControlModeMap& reportedJointControlModes)
{
if (!getWidget() || !robotNodeSet)
{
@@ -1111,7 +1233,7 @@ void KinematicUnitWidgetController::updateControlModesTable()
}
}
-void KinematicUnitWidgetController::updateJointStatusesTable()
+void KinematicUnitWidgetController::updateJointStatusesTable(const NameStatusMap& reportedJointStatuses)
{
if (!getWidget() || !robotNodeSet || reportedJointStatuses.empty())
{
@@ -1187,25 +1309,9 @@ QString KinematicUnitWidgetController::translateStatus(ErrorStatus status)
}
}
-void KinematicUnitWidgetController::updateJointAnglesTable()
+void KinematicUnitWidgetController::updateJointAnglesTable(const NameValueMap& reportedJointAngles)
{
-
- float dirty_squaresum = 0;
-
std::unique_lock lock(mutexNodeSet);
- if (jointAnglesUpdateFrequency && jointAnglesUpdateFrequency->getValue())
- {
-
- double deltaT = jointAnglesUpdateFrequency->getValue()->getDouble();
- if (deltaT != 0)
- {
- ui.labelUpdateFreq->setText(QString::number(static_cast<int>(1.0 / deltaT)) + " Hz");
- }
- else
- {
- ui.labelUpdateFreq->setText("- Hz");
- }
- }
if (!robotNodeSet)
{
@@ -1226,7 +1332,6 @@ void KinematicUnitWidgetController::updateJointAnglesTable()
}
const float currentValue = it->second;
- dirty_squaresum += currentValue * currentValue;
QModelIndex index = ui.tableJointList->model()->index(i, eTabelColumnAngleProgressbar);
float conversionFactor = ui.checkBoxUseDegree->isChecked() &&
@@ -1237,22 +1342,21 @@ void KinematicUnitWidgetController::updateJointAnglesTable()
}
//update only if values changed
- if ((fabs(dirty_squaresum_old[0] - dirty_squaresum)) > 0.0000005)
- {
- updateModel();
- dirty_squaresum_old[0] = dirty_squaresum;
- // ARMARX_INFO << "update model" << flush;
- }
+ // if ((fabs(dirty_squaresum_old[0] - dirty_squaresum)) > 0.0000005)
+ // {
+ // updateModel();
+ // dirty_squaresum_old[0] = dirty_squaresum;
+ // // ARMARX_INFO << "update model" << flush;
+ // }
}
-void KinematicUnitWidgetController::updateJointVelocitiesTable()
+void KinematicUnitWidgetController::updateJointVelocitiesTable(const NameValueMap& reportedJointVelocities)
{
if (!getWidget())
{
return;
}
- float dirty_squaresum = 0;
std::unique_lock lock(mutexNodeSet);
if (!robotNodeSet)
{
@@ -1272,7 +1376,6 @@ void KinematicUnitWidgetController::updateJointVelocitiesTable()
}
float currentValue = it->second;
- dirty_squaresum += currentValue * currentValue;
if (ui.checkBoxUseDegree->isChecked() && rn[i]->isRotationalJoint())
{
currentValue *= 180.0 / M_PI;
@@ -1281,15 +1384,9 @@ void KinematicUnitWidgetController::updateJointVelocitiesTable()
newItem = new QTableWidgetItem(Text);
ui.tableJointList->setItem(i, eTabelColumnVelocity, newItem);
}
-
- if ((fabs(dirty_squaresum_old[1] - dirty_squaresum)) > 0.0000005)
- {
- updateModel();
- dirty_squaresum_old[1] = dirty_squaresum;
- }
}
-void KinematicUnitWidgetController::updateJointTorquesTable()
+void KinematicUnitWidgetController::updateJointTorquesTable(const NameValueMap& reportedJointTorques)
{
@@ -1317,7 +1414,7 @@ void KinematicUnitWidgetController::updateJointTorquesTable()
}
}
-void KinematicUnitWidgetController::updateJointCurrentsTable()
+void KinematicUnitWidgetController::updateJointCurrentsTable(const NameValueMap& reportedJointCurrents, const NameStatusMap& reportedJointStatuses)
{
@@ -1328,7 +1425,9 @@ void KinematicUnitWidgetController::updateJointCurrentsTable()
}
std::vector< RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
QTableWidgetItem* newItem;
- NameValueMap reportedJointCurrents = jointCurrentHistory.back().second;
+
+ // FIXME history!
+ // NameValueMap reportedJointCurrents = jointCurrentHistory.back().second;
NameValueMap::const_iterator it;
for (unsigned int i = 0; i < rn.size(); i++)
@@ -1345,10 +1444,10 @@ void KinematicUnitWidgetController::updateJointCurrentsTable()
ui.tableJointList->setItem(i, eTabelColumnCurrent, newItem);
}
- highlightCriticalValues();
+ highlightCriticalValues(reportedJointStatuses);
}
-void KinematicUnitWidgetController::updateMotorTemperaturesTable()
+void KinematicUnitWidgetController::updateMotorTemperaturesTable(const NameValueMap& reportedJointTemperatures)
{
@@ -1378,132 +1477,95 @@ void KinematicUnitWidgetController::updateMotorTemperaturesTable()
}
-void KinematicUnitWidgetController::reportJointAngles(const NameValueMap& jointAngles, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
-{
- if (!aValueChanged && reportedJointAngles.size() > 0)
- {
- return;
- }
-
-
- std::unique_lock lock(mutexNodeSet);
- jointAnglesUpdateFrequency->update(timestamp, new Variant((double)(timestamp - lastJointAngleUpdateTimestamp) * 1e-6));
- lastJointAngleUpdateTimestamp = timestamp;
- mergeMaps(reportedJointAngles, jointAngles);
- if (skipper.checkFrequency(c, 30))
- {
- // ARMARX_INFO << deactivateSpam(1) << "updating angles";
- emit jointAnglesReported();
- }
- // else
- // {
- // ARMARX_INFO << deactivateSpam(1) << "skipping angles";
- // }
-}
-
-void KinematicUnitWidgetController::reportJointVelocities(const NameValueMap& jointVelocities, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
-{
- if (!aValueChanged && reportedJointVelocities.size() > 0)
- {
- return;
- }
-
- std::unique_lock lock(mutexNodeSet);
- mergeMaps(reportedJointVelocities, jointVelocities);
- if (skipper.checkFrequency(c))
- {
- emit jointVelocitiesReported();
- }
-}
-
-void KinematicUnitWidgetController::reportJointTorques(const NameValueMap& jointTorques, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
-{
- if (!aValueChanged && reportedJointTorques.size() > 0)
- {
- return;
- }
-
- std::unique_lock lock(mutexNodeSet);
- reportedJointTorques = jointTorques;
- if (skipper.checkFrequency(c))
- {
- emit jointTorquesReported();
- }
-}
-
-void KinematicUnitWidgetController::reportJointAccelerations(const NameValueMap& jointAccelerations, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
-{
-
-}
-
-void KinematicUnitWidgetController::reportControlModeChanged(const NameControlModeMap& jointModes, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
-{
- // if(!aValueChanged && reportedJointControlModes.size() > 0)
- // return;
- std::unique_lock lock(mutexNodeSet);
- mergeMaps(reportedJointControlModes, jointModes);
- if (skipper.checkFrequency(c))
- {
- emit jointControlModesReported();
- }
-}
-
-void KinematicUnitWidgetController::reportJointCurrents(const NameValueMap& jointCurrents, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
-{
- std::unique_lock lock(mutexNodeSet);
-
- if (aValueChanged && jointCurrents.size() > 0)
- {
- jointCurrentHistory.push_back(std::pair<Ice::Long, NameValueMap>(timestamp, jointCurrents));
- }
-
- while (jointCurrentHistory.size() > 1 && jointCurrentHistory.back().first - jointCurrentHistory.front().first > historyTime)
- {
- jointCurrentHistory.pop_front();
- }
-
-
- if (jointCurrentHistory.size() > 0)
- {
- if (skipper.checkFrequency(c))
- {
- emit jointCurrentsReported();
- }
- }
-}
-
-void KinematicUnitWidgetController::reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
-{
- if (!aValueChanged && reportedJointStatuses.size() > 0)
- {
- return;
- }
-
- std::unique_lock lock(mutexNodeSet);
- mergeMaps(this->reportedJointTemperatures, jointMotorTemperatures);
- if (skipper.checkFrequency(c))
- {
- emit jointMotorTemperaturesReported();
- }
-}
-
-void KinematicUnitWidgetController::reportJointStatuses(const NameStatusMap& jointStatuses, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
-{
- if (!aValueChanged && reportedJointStatuses.size() > 0)
- {
- return;
- }
-
- std::unique_lock lock(mutexNodeSet);
- mergeMaps(reportedJointStatuses, jointStatuses);
- if (skipper.checkFrequency(c))
- {
- emit jointStatusesReported();
- }
-}
-
-
-void KinematicUnitWidgetController::updateModel()
+// void KinematicUnitWidgetController::reportJointAngles(const NameValueMap& jointAngles, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
+// {
+// reportedJointAngles = kinematicUnitInterfacePrx->getJointAngles();
+// emit jointAnglesReported();
+
+// }
+
+// void KinematicUnitWidgetController::reportJointVelocities(const NameValueMap& jointVelocities, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
+// {
+// reportedJointVelocities = kinematicUnitInterfacePrx->getJointVelocities();
+// emit jointVelocitiesReported();
+
+// }
+
+// void KinematicUnitWidgetController::reportJointTorques(const NameValueMap& jointTorques, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
+// {
+// reportedJointTorques = kinematicUnitInterfacePrx->getJointTorques();
+// emit jointTorquesReported();
+// }
+
+// void KinematicUnitWidgetController::reportJointAccelerations(const NameValueMap& jointAccelerations, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
+// {
+
+// }
+
+// void KinematicUnitWidgetController::reportControlModeChanged(const NameControlModeMap& jointModes, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
+// {
+// reportedJointControlModes = kinematicUnitInterfacePrx->getControlModes();
+// emit jointControlModesReported();
+// }
+
+// void KinematicUnitWidgetController::reportJointCurrents(const NameValueMap& jointCurrents, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
+// {
+// std::unique_lock lock(mutexNodeSet);
+
+// const auto jointCurrents = kinematicUnitInterfacePrx->getJointCurrent();
+
+// if (aValueChanged && jointCurrents.size() > 0)
+// {
+// jointCurrentHistory.push_back(std::pair<Ice::Long, NameValueMap>(timestamp, jointCurrents));
+// }
+
+// while (jointCurrentHistory.size() > 1 && jointCurrentHistory.back().first - jointCurrentHistory.front().first > historyTime)
+// {
+// jointCurrentHistory.pop_front();
+// }
+
+
+// if (jointCurrentHistory.size() > 0)
+// {
+// if (skipper.checkFrequency(c))
+// {
+// emit jointCurrentsReported();
+// }
+// }
+// }
+
+// void KinematicUnitWidgetController::reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
+// {
+// if (!aValueChanged && reportedJointStatuses.size() > 0)
+// {
+// return;
+// }
+
+// std::unique_lock lock(mutexNodeSet);
+// mergeMaps(this->reportedJointTemperatures, jointMotorTemperatures);
+// if (skipper.checkFrequency(c))
+// {
+// emit jointMotorTemperaturesReported();
+// }
+// }
+
+// void KinematicUnitWidgetController::reportJointStatuses(const NameStatusMap& jointStatuses, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
+// {
+// if (!aValueChanged && reportedJointStatuses.size() > 0)
+// {
+// return;
+// }
+
+// std::unique_lock lock(mutexNodeSet);
+// mergeMaps(reportedJointStatuses, jointStatuses);
+// if (skipper.checkFrequency(c))
+// {
+// emit jointStatusesReported();
+// }
+// }
+
+
+void KinematicUnitWidgetController::updateModel(const NameValueMap& reportedJointAngles)
{
// ARMARX_INFO << "updateModel()" << flush;
std::unique_lock lock(mutexNodeSet);
@@ -1514,7 +1576,7 @@ void KinematicUnitWidgetController::updateModel()
robot->setJointValues(reportedJointAngles);
}
-void KinematicUnitWidgetController::highlightCriticalValues()
+void KinematicUnitWidgetController::highlightCriticalValues(const NameStatusMap& reportedJointStatuses)
{
if (!enableValueValidator)
{
@@ -1619,18 +1681,42 @@ float KinematicUnitWidgetController::cutJitter(float value)
return (abs(value) < static_cast<float>(ui.jitterThresholdSpinBox->value())) ? 0 : value;
}
-void KinematicUnitWidgetController::timerEvent(QTimerEvent*)
+// void KinematicUnitWidgetController::timerEvent(QTimerEvent*)
+// {
+// ARMARX_INFO << "timerEvent";
+// updateGui();
+// }
+
+void KinematicUnitWidgetController::fetchData()
{
- updateJointAnglesTable();
- updateJointVelocitiesTable();
- updateJointTorquesTable();
- updateJointCurrentsTable();
- updateControlModesTable();
- updateJointStatusesTable();
- updateMotorTemperaturesTable();
+ ARMARX_INFO << "updateGui";
+
+ if(not kinematicUnitInterfacePrx)
+ {
+ ARMARX_WARNING << "KinematicUnit is not available!";
+ return;
+ }
+
+ const auto debugInfo = kinematicUnitInterfacePrx->getDebugInfo();
+
+ emit onDebugInfoReceived(debugInfo);
}
+void KinematicUnitWidgetController::debugInfoReceived(const DebugInfo& debugInfo)
+{
+ ARMARX_INFO << "debug info received";
+
+ updateModel(debugInfo.jointAngles);
+ updateJointAnglesTable(debugInfo.jointAngles);
+ updateJointVelocitiesTable(debugInfo.jointVelocities);
+ updateJointTorquesTable(debugInfo.jointTorques);
+ updateJointCurrentsTable(debugInfo.jointCurrents, debugInfo.jointStatus);
+ updateControlModesTable(debugInfo.jointModes);
+ updateJointStatusesTable(debugInfo.jointStatus);
+ updateMotorTemperaturesTable(debugInfo.jointMotorTemperatures);
+
+}
void RangeValueDelegate::paint(QPainter* painter, const QStyleOptionViewItem& option, const QModelIndex& index) const
{
@@ -1684,7 +1770,15 @@ void RangeValueDelegate::paint(QPainter* painter, const QStyleOptionViewItem& op
}
}
-KinematicUnitWidgetController::~KinematicUnitWidgetController() {}
+KinematicUnitWidgetController::~KinematicUnitWidgetController()
+{
+ if(updateTask)
+ {
+ updateTask->stop();
+ updateTask->join();
+ updateTask = nullptr;
+ }
+}
void KinematicUnitWidgetController::on_pushButtonFromJson_clicked()
{
diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h
index d0a98a0a1a57efb6958b103b81732e022afbedf4..3da0e8b517a8e7f1afd80f8d31c7eea450c63629 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h
@@ -32,6 +32,7 @@
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include "ArmarXCore/core/services/tasks/RunningTask.h"
#include <ArmarXCore/core/Component.h>
/* Qt headers */
@@ -58,6 +59,8 @@
#include <mutex>
+#include <ArmarXCore/core/time.h>
+
namespace armarx
{
class KinematicUnitConfigDialog;
@@ -110,8 +113,8 @@ namespace armarx
a joint with the slider below.
*/
class KinematicUnitWidgetController :
- public ArmarXComponentWidgetControllerTemplate<KinematicUnitWidgetController>,
- public KinematicUnitListener
+ public ArmarXComponentWidgetControllerTemplate<KinematicUnitWidgetController>
+ // public KinematicUnitListener
{
Q_OBJECT
public:
@@ -189,16 +192,20 @@ namespace armarx
void jointStatusesReported();
void jointMotorTemperaturesReported();
+ void onDebugInfoReceived(const DebugInfo& debugInfo);
+
public slots:
// KinematicUnit
- void kinematicUnitZeroPosition();
+ // void kinematicUnitZeroPosition();
void kinematicUnitZeroVelocity();
- void kinematicUnitZeroTorque();
+ // void kinematicUnitZeroTorque();
+
void setControlModePosition();
void setControlModeVelocity();
void setControlModeTorque();
+
void selectJoint(int i);
void selectJointFromTableWidget(int row, int column);
void sliderValueChanged(int i);
@@ -210,19 +217,22 @@ namespace armarx
void resetSlider();
void resetSliderToZeroPosition();
- void updateJointAnglesTable();
- void updateJointVelocitiesTable();
- void updateJointTorquesTable();
- void updateJointCurrentsTable();
- void updateMotorTemperaturesTable();
- void updateJointStatusesTable();
- void updateControlModesTable();
+ void updateJointAnglesTable(const NameValueMap& reportedJointAngles);
+ void updateJointVelocitiesTable(const NameValueMap& reportedJointVelocities);
+ void updateJointTorquesTable(const NameValueMap& reportedJointTorques);
+ void updateJointCurrentsTable(const NameValueMap& reportedJointCurrents, const NameStatusMap& reportedJointStatuses);
+ void updateMotorTemperaturesTable(const NameValueMap& reportedMotorTemperatures);
+ void updateJointStatusesTable(const NameStatusMap& reportedJointStatuses);
+ void updateControlModesTable(const NameControlModeMap& reportedJointControlModes);
+
void updateKinematicUnitListInDialog();
+ void fetchData();
+
protected:
void connectSlots();
- void initializeUi();
+ void initializeUi(const DebugInfo& debugInfo);
QString translateStatus(OperationStatus status);
QString translateStatus(ErrorStatus status);
@@ -254,18 +264,18 @@ namespace armarx
armarx::DebugDrawerComponentPtr debugDrawer;
// interface implementation
- void reportControlModeChanged(const NameControlModeMap& jointModes, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c) override;
- void reportJointAngles(const NameValueMap& jointAngles, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c) override;
- void reportJointVelocities(const NameValueMap& jointVelocities, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c) override;
- void reportJointTorques(const NameValueMap& jointTorques, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c) override;
- void reportJointAccelerations(const NameValueMap& jointAccelerations, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c) override;
- void reportJointCurrents(const NameValueMap& jointCurrents, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c) override;
- void reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c) override;
- void reportJointStatuses(const NameStatusMap& jointStatuses, Ice::Long timestamp, bool aValueChanged, const Ice::Current&) override;
+ // void reportControlModeChanged(const NameControlModeMap& jointModes, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c) override;
+ // void reportJointAngles(const NameValueMap& jointAngles, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c) override;
+ // void reportJointVelocities(const NameValueMap& jointVelocities, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c) override;
+ // void reportJointTorques(const NameValueMap& jointTorques, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c) override;
+ // void reportJointAccelerations(const NameValueMap& jointAccelerations, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c) override;
+ // void reportJointCurrents(const NameValueMap& jointCurrents, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c) override;
+ // void reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c) override;
+ // void reportJointStatuses(const NameStatusMap& jointStatuses, Ice::Long timestamp, bool aValueChanged, const Ice::Current&) override;
- void updateModel();
+ void updateModel(const NameValueMap& jointAngles);
- void highlightCriticalValues();
+ void highlightCriticalValues(const NameStatusMap& reportedJointStatuses);
protected slots:
@@ -274,6 +284,8 @@ namespace armarx
void on_pushButtonFromJson_clicked();
void synchronizeRobotJointAngles();
+ void debugInfoReceived(const DebugInfo& debugInfo);
+
private:
@@ -288,23 +300,23 @@ namespace armarx
bool enableValueValidator;
bool viewerEnabled = true;
Ice::Long historyTime;
- DatafieldFilterBasePtr jointAnglesUpdateFrequency;
- long lastJointAngleUpdateTimestamp;
+ // DatafieldFilterBasePtr jointAnglesUpdateFrequency;
+ armarx::DateTime lastJointAngleUpdateTimestamp;
float currentValueMax;
- NameValueMap reportedJointAngles;
- NameValueMap reportedJointVelocities;
- NameControlModeMap reportedJointControlModes;
- NameValueMap reportedJointTorques;
- NameValueMap reportedJointTemperatures;
+ // NameValueMap reportedJointAngles;
+ // NameValueMap reportedJointVelocities;
+ // NameControlModeMap reportedJointControlModes;
+ // NameValueMap reportedJointTorques;
+ // NameValueMap reportedJointTemperatures;
std::deque<std::pair<Ice::Long, NameValueMap>> jointCurrentHistory;
- NameStatusMap reportedJointStatuses;
+ // NameStatusMap reportedJointStatuses;
std::vector<float> dirty_squaresum_old;
QPointer<QWidget> __widget;
QPointer<KinematicUnitConfigDialog> dialog;
- ControlMode selectedControlMode;
+ // ControlMode selectedControlMode;
RangeValueDelegate delegate;
/**
@@ -325,13 +337,22 @@ namespace armarx
// QObject interface
protected:
- void timerEvent(QTimerEvent*) override;
- int updateTimerId = 0;
- IceReportSkipper skipper = {20.0f};
+ // void timerEvent(QTimerEvent*) override;
+ // int updateTimerId = 0;
+ // IceReportSkipper skipper = {20.0f};
+ // std::unique_ptr<QTimer> timer;
+
+ void runUpdate();
+
+ armarx::RunningTask<KinematicUnitWidgetController>::pointer_type updateTask;
+
+ ControlMode getSelectedControlMode() const;
+ void setControlModeRadioButtonGroup(const ControlMode& controlMode);
+
+
};
using FloatVector = ::std::vector< ::Ice::Float>;
using KinematicUnitGuiPluginPtr = std::shared_ptr<KinematicUnitWidgetController>;
}
-
diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/kinematicunitguiplugin.ui b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/kinematicunitguiplugin.ui
index a265252ea20b34ad1b381292a2748316e88460b3..9273d2f6328aafd8be7a40ba052b6f1c12364726 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/kinematicunitguiplugin.ui
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/kinematicunitguiplugin.ui
@@ -81,7 +81,7 @@
<string>Position Control</string>
</property>
<property name="checked">
- <bool>true</bool>
+ <bool>false</bool>
</property>
</widget>
</item>
@@ -108,6 +108,19 @@
</property>
</widget>
</item>
+ <item>
+ <widget class="QRadioButton" name="radioButtonUnknown">
+ <property name="text">
+ <string></string>
+ </property>
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="visible">
+ <bool>false</bool>
+ </property>
+ </widget>
+ </item>
<item>
<spacer name="horizontalSpacer">
<property name="orientation">