From bf82109d3e5573636ac05e29fcf338c8655e7e88 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Tue, 3 May 2022 15:28:12 +0200
Subject: [PATCH] Add synchronization in onConnect (to be sure)
---
.../KinematicUnitGuiPlugin.cpp | 64 ++++++++++---------
.../KinematicUnitGuiPlugin.h | 4 ++
2 files changed, 37 insertions(+), 31 deletions(-)
diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
index 51633af95..197397612 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
@@ -147,7 +147,7 @@ void KinematicUnitWidgetController::onInitComponent()
void KinematicUnitWidgetController::onConnectComponent()
{
- ARMARX_INFO << "kinematic unit gui :: onConnectComponent()";
+ // ARMARX_INFO << "Kinematic Unit Gui :: onConnectComponent()";
reportedJointAngles.clear();
reportedJointVelocities.clear();
reportedJointControlModes.clear();
@@ -156,10 +156,11 @@ void KinematicUnitWidgetController::onConnectComponent()
reportedJointStatuses.clear();
reportedJointTemperatures.clear();
-
jointAnglesUpdateFrequency = new filters::MedianFilter(100);
kinematicUnitInterfacePrx = getProxy<KinematicUnitInterfacePrx>(kinematicUnitName);
topicName = kinematicUnitInterfacePrx->getReportTopicName();
+ usingTopic(topicName);
+
lastJointAngleUpdateTimestamp = TimeUtil::GetTime().toSecondsDouble();
robotVisu->removeAllChildren();
@@ -168,10 +169,9 @@ void KinematicUnitWidgetController::onConnectComponent()
std::string rfile;
Ice::StringSeq includePaths;
- // get robot filename
+ // Get robot filename
try
{
-
Ice::StringSeq packages = kinematicUnitInterfacePrx->getArmarXPackages();
packages.push_back(Application::GetProjectName());
ARMARX_VERBOSE << "ArmarX packages " << packages;
@@ -200,7 +200,7 @@ void KinematicUnitWidgetController::onConnectComponent()
}
catch (...)
{
- ARMARX_ERROR << "Unable to retrieve robot filename";
+ ARMARX_ERROR << "Unable to retrieve robot filename.";
}
try
@@ -208,6 +208,10 @@ void KinematicUnitWidgetController::onConnectComponent()
ARMARX_INFO << "Loading robot from file " << rfile;
robot = loadRobotFile(rfile);
}
+ catch (const std::exception& e)
+ {
+ ARMARX_ERROR << "Failed to init robot: " << e.what();
+ }
catch (...)
{
ARMARX_ERROR << "Failed to init robot";
@@ -216,35 +220,34 @@ void KinematicUnitWidgetController::onConnectComponent()
if (!robot || !robot->hasRobotNodeSet(robotNodeSetName))
{
getObjectScheduler()->terminate();
-
if (getWidget()->parentWidget())
{
getWidget()->parentWidget()->close();
}
-
- std::cout << "returning" << std::endl;
return;
}
- // check robot name and disable setZero Button if necessary
+ // Check robot name and disable setZero Button if necessary
if (not simox::alg::starts_with(robot->getName(), "Armar3"))
{
- ARMARX_IMPORTANT << "Disable the SetZero button because the robot name is " << robot->getName();
+ ARMARX_IMPORTANT << "Disable the SetZero button because the robot name is '" << robot->getName() << "'.";
ui.pushButtonKinematicUnitPos1->setDisabled(true);
}
kinematicUnitFile = rfile;
robotNodeSet = robot->getRobotNodeSet(robotNodeSetName);
-
- initGUIComboBox(robotNodeSet); // init the pull down menu (QT: ComboBox)
- initGUIJointListTable(robotNodeSet);
-
kinematicUnitVisualization = getCoinVisualization(robot);
kinematicUnitNode = kinematicUnitVisualization->getCoinVisualization();
robotVisu->addChild(kinematicUnitNode);
- // init control mode map
+ // Fetch the current joint angles.
+ synchronizeRobotJointAngles();
+
+ initGUIComboBox(robotNodeSet); // init the pull down menu (QT: ComboBox)
+ initGUIJointListTable(robotNodeSet);
+
+ // Init control mode map
try
{
reportedJointControlModes = kinematicUnitInterfacePrx->getControlModes();
@@ -255,7 +258,6 @@ void KinematicUnitWidgetController::onConnectComponent()
initializeUi();
- usingTopic(topicName);
QMetaObject::invokeMethod(this, "resetSlider");
enableMainWidgetAsync(true);
updateTimerId = startTimer(1000); // slow timer that only ensures that skipped updates are shown at all
@@ -355,13 +357,11 @@ void KinematicUnitWidgetController::saveSettings(QSettings* settings)
settings->setValue("enableValueValidator", enableValueValidator);
settings->setValue("viewerEnabled", viewerEnabled);
assert(historyTime % 1000 == 0);
- settings->setValue("historyTime", (int)(historyTime / 1000));
+ settings->setValue("historyTime", static_cast<int>(historyTime / 1000));
settings->setValue("currentValueMax", currentValueMax);
}
-
-
void KinematicUnitWidgetController::showVisuLayers(bool show)
{
if (debugDrawer)
@@ -410,7 +410,9 @@ void KinematicUnitWidgetController::updateGuiElements()
// modelUpdateCB();
}
-void KinematicUnitWidgetController::updateKinematicUnitListInDialog() {}
+void KinematicUnitWidgetController::updateKinematicUnitListInDialog()
+{
+}
void KinematicUnitWidgetController::modelUpdateCB()
{
@@ -418,7 +420,6 @@ void KinematicUnitWidgetController::modelUpdateCB()
SoNode* KinematicUnitWidgetController::getScene()
{
-
if (viewerEnabled)
{
ARMARX_INFO << "Returning scene ";
@@ -500,7 +501,7 @@ void KinematicUnitWidgetController::kinematicUnitZeroPosition()
{
}
- //set slider to 0 if position control mode is used
+ // Set slider to 0 if position control mode is used.
if (selectedControlMode == ePositionControl)
{
ui.horizontalSliderKinematicUnitPos->setSliderPosition(SLIDER_ZERO_POSITION);
@@ -615,9 +616,9 @@ void KinematicUnitWidgetController::setControlModePosition()
if (currentNode)
{
- QString unit = currentNode->isRotationalJoint() ?
- (ui.checkBoxUseDegree->isChecked() ? "deg" : "rad") :
- "mm";
+ QString unit = currentNode->isRotationalJoint()
+ ? (ui.checkBoxUseDegree->isChecked() ? "deg" : "rad")
+ : "mm";
ui.labelUnit->setText(unit);
const bool isDeg = ui.checkBoxUseDegree->isChecked();
const bool isRot = currentNode->isRotationalJoint();
@@ -644,9 +645,7 @@ void KinematicUnitWidgetController::setControlModePosition()
// Therefore, we first have to fetch the actual joint values and use that one.
// However, this should actually not be necessary, as the robot model should be updated
// via the topics.
-
- const NameValueMap currentJointAngles = kinematicUnitInterfacePrx->getJointAngles();
- robot->setJointValues(currentJointAngles);
+ synchronizeRobotJointAngles();
}
float pos = currentNode->getJointValue() * conversionFactor;
@@ -745,10 +744,7 @@ void KinematicUnitWidgetController::setControlModeTorque()
ui.horizontalSliderKinematicUnitPos->blockSignals(false);
resetSlider();
-
}
-
-
}
VirtualRobot::RobotPtr KinematicUnitWidgetController::loadRobotFile(std::string fileName)
@@ -1729,3 +1725,9 @@ void KinematicUnitWidgetController::on_pushButtonFromJson_clicked()
ARMARX_ERROR << "failed to switch mode or set angles";
}
}
+
+void KinematicUnitWidgetController::synchronizeRobotJointAngles()
+{
+ const NameValueMap currentJointAngles = kinematicUnitInterfacePrx->getJointAngles();
+ robot->setJointValues(currentJointAngles);
+}
diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h
index b84b6ccc6..d0a98a0a1 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h
@@ -268,9 +268,13 @@ namespace armarx
void highlightCriticalValues();
protected slots:
+
void showVisuLayers(bool show);
void copyToClipboard();
void on_pushButtonFromJson_clicked();
+
+ void synchronizeRobotJointAngles();
+
private:
std::recursive_mutex mutexNodeSet;
--
GitLab