From 22b5eabefa9aa21a3bcff89a7e1fa20b4d7f9b58 Mon Sep 17 00:00:00 2001
From: Nikolaus Vahrenkamp <vahrenkamp@kit.edu>
Date: Wed, 17 Feb 2016 13:17:26 +0100
Subject: [PATCH] Improvement of KinemticUnitGui: Now, the robot file and the
topic name is automatically queried from the kinemtic unit proxy. Some
re-connect fixes.
---
.../RobotState/RobotStateComponent.cpp | 14 +-
.../RobotState/RobotStateComponent.h | 10 +-
.../components/units/KinematicUnit.cpp | 54 ++++
.../RobotAPI/components/units/KinematicUnit.h | 29 ++
.../KinematicUnitConfigDialog.cpp | 59 ++--
.../KinematicUnitConfigDialog.h | 9 +-
.../KinematicUnitConfigDialog.ui | 70 ++---
.../KinematicUnitGuiPlugin.cpp | 272 +++++++++++-------
.../KinematicUnitGuiPlugin.h | 7 +
source/RobotAPI/interface/core/RobotState.ice | 21 +-
.../units/KinematicUnitInterface.ice | 37 ++-
11 files changed, 405 insertions(+), 177 deletions(-)
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
index 103191364..4d618fa97 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
@@ -77,7 +77,7 @@ namespace armarx
ARMARX_VERBOSE << "Failed loading robot from file " << robotFile;
}
- string robotNodeSetName = getProperty<std::string>("RobotNodeSetName").getValue();
+ robotNodeSetName = getProperty<std::string>("RobotNodeSetName").getValue();
if (robotNodeSetName == "")
@@ -204,7 +204,7 @@ namespace armarx
auto packages = armarx::Application::GetProjectDependencies();
packages.push_back(Application::GetProjectName());
- for (const std::string& projectName : packages)
+ for (const std::string & projectName : packages)
{
if (projectName.empty())
{
@@ -282,6 +282,16 @@ namespace armarx
}
}
+
+ string RobotStateComponent::getRobotNodeSetName(const Current&) const
+ {
+ if (robotNodeSetName.empty())
+ {
+ throw NotInitializedException("RobotNodeSetName is empty", "getRobotNodeSetName");
+ }
+ return robotNodeSetName;
+ }
+
}
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.h b/source/RobotAPI/components/RobotState/RobotStateComponent.h
index 617d938e6..f5c1d1434 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.h
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.h
@@ -58,7 +58,7 @@ namespace armarx
/**
* \defgroup Component-RobotStateComponent RobotStateComponent
* \ingroup RobotAPI-Components
- * \brief Maintains a robot representation based on VirtualRobot (see [Simox](http://simox.sourceforge.net/)).
+ * \brief Maintains a robot representation based on VirtualRobot (see [Simox](http://gitlab.com/Simox/simox)).
*
* The robot can be loaded from a Simox robot XML file.
* Upon request, an Ice proxy to a shared instance of this internal robot
@@ -110,6 +110,12 @@ namespace armarx
*/
virtual std::string getRobotName(const Ice::Current&) const;
+ /**
+ *
+ * \return The name of this robot instance.
+ */
+ virtual std::string getRobotNodeSetName(const Ice::Current&) const;
+
/**
* Create an instance of RobotStatePropertyDefinitions.
*/
@@ -154,6 +160,8 @@ namespace armarx
std::string relativeRobotFile;
RobotStateObserverPtr robotStateObs;
+ std::string robotNodeSetName;
+
};
diff --git a/source/RobotAPI/components/units/KinematicUnit.cpp b/source/RobotAPI/components/units/KinematicUnit.cpp
index d5c08b7a9..a0650298d 100644
--- a/source/RobotAPI/components/units/KinematicUnit.cpp
+++ b/source/RobotAPI/components/units/KinematicUnit.cpp
@@ -29,9 +29,11 @@
#include <VirtualRobot/VirtualRobotException.h>
#include <VirtualRobot/RuntimeEnvironment.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
using namespace armarx;
+using namespace std;
void KinematicUnit::onInitComponent()
@@ -39,6 +41,7 @@ void KinematicUnit::onInitComponent()
// read names of kinematic chain elements belonging to this unit from XML and setup a map of all joints
// the kinematic chain elements belonging to this unit are defined in a RobotNodeSet
std::string robotFile = getProperty<std::string>("RobotFileName").getValue();
+ relativeRobotFile = robotFile;
robotNodeSetName = getProperty<std::string>("RobotNodeSetName").getValue();
std::string project = getProperty<std::string>("RobotFileNameProject").getValue();
@@ -124,6 +127,57 @@ void KinematicUnit::releaseKinematicUnit(const StringSequence& nodes, const Ice:
SensorActorUnit::release(c);
}
+
+std::string KinematicUnit::getRobotFilename(const Ice::Current&) const
+{
+ return relativeRobotFile;
+}
+
+std::vector<std::string> KinematicUnit::getArmarXPackages(const Ice::Current&) const
+{
+ std::vector<string> result;
+ auto packages = armarx::Application::GetProjectDependencies();
+ packages.push_back(Application::GetProjectName());
+
+ for (const std::string & projectName : packages)
+ {
+ if (projectName.empty())
+ {
+ continue;
+ }
+
+ result.push_back(projectName);
+ }
+
+ return result;
+}
+
+std::string KinematicUnit::getRobotName(const Ice::Current&) const
+{
+ if (robot)
+ {
+ return robot->getName();
+ }
+ else
+ {
+ throw NotInitializedException("Robot Ptr is NULL", "getName");
+ }
+}
+
+std::string KinematicUnit::getRobotNodeSetName(const Ice::Current&) const
+{
+ if (robotNodeSetName.empty())
+ {
+ throw NotInitializedException("RobotNodeSetName is empty", "getRobotNodeSetName");
+ }
+ return robotNodeSetName;
+}
+
+std::string KinematicUnit::getReportTopicName(const Ice::Current&) const
+{
+ return listenerName;
+}
+
PropertyDefinitionsPtr KinematicUnit::createPropertyDefinitions()
{
return PropertyDefinitionsPtr(new KinematicUnitPropertyDefinitions(
diff --git a/source/RobotAPI/components/units/KinematicUnit.h b/source/RobotAPI/components/units/KinematicUnit.h
index c3dd87e08..5a2119816 100644
--- a/source/RobotAPI/components/units/KinematicUnit.h
+++ b/source/RobotAPI/components/units/KinematicUnit.h
@@ -85,6 +85,34 @@ namespace armarx
virtual void onConnectComponent();
virtual void onExitComponent();
+ /**
+ * \return the robot xml filename as specified in the configuration
+ */
+ virtual std::string getRobotFilename(const Ice::Current&) const;
+
+ /*!
+ * \brief getArmarXPackages
+ * \return All dependent packages, which might contain a robot file.
+ */
+ virtual std::vector< std::string > getArmarXPackages(const Ice::Current&) const;
+
+ /**
+ *
+ * \return The name of this robot instance.
+ */
+ virtual std::string getRobotName(const Ice::Current&) const;
+ /**
+ *
+ * \return The name of this robot instance.
+ */
+ virtual std::string getRobotNodeSetName(const Ice::Current&) const;
+
+ /**
+ *
+ * \return The name of the report topic
+ */
+ virtual std::string getReportTopicName(const Ice::Current&) const;
+
virtual void onInitKinematicUnit() = 0;
virtual void onStartKinematicUnit() = 0;
virtual void onExitKinematicUnit() = 0;
@@ -105,6 +133,7 @@ namespace armarx
protected:
KinematicUnitListenerPrx listenerPrx;
+ std::string relativeRobotFile;
VirtualRobot::RobotPtr robot;
std::string robotNodeSetName;
std::string listenerName;
diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.cpp b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.cpp
index 80817194c..01596e7e5 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.cpp
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.cpp
@@ -35,6 +35,8 @@
#include <ArmarXGui/libraries/ArmarXGuiBase/widgets/IceProxyFinder.h>
+#include <RobotAPI/interface/core/RobotState.h>
+
using namespace armarx;
KinematicUnitConfigDialog::KinematicUnitConfigDialog(QWidget* parent) :
@@ -43,24 +45,14 @@ KinematicUnitConfigDialog::KinematicUnitConfigDialog(QWidget* parent) :
uuid(IceUtil::generateUUID())
{
ui->setupUi(this);
- fileDialog = new QFileDialog(parent);
- fileDialog->setModal(true);
- fileDialog->setFileMode(QFileDialog::ExistingFiles);
- QStringList fileTypes;
- fileTypes << tr("XML (*.xml)")
- << tr("All Files (*.*)");
- fileDialog->setNameFilters(fileTypes);
- connect(ui->btnSelectModelFile, SIGNAL(clicked()), fileDialog, SLOT(show()));
- connect(fileDialog, SIGNAL(accepted()), this, SLOT(modelFileSelected()));
+
connect(this->ui->buttonBox, SIGNAL(accepted()), this, SLOT(verifyConfig()));
ui->buttonBox->button(QDialogButtonBox::Ok)->setDefault(true);
proxyFinder = new IceProxyFinder<KinematicUnitInterfacePrx>(this);
proxyFinder->setSearchMask("*");
- ui->gridLayout->addWidget(proxyFinder, 2, 1, 1, 2);
- topicFinder = new IceTopicFinder(this);
- topicFinder->setSearchMask("*RobotState");
- ui->gridLayout->addWidget(topicFinder, 3, 1, 1, 2);
+ ui->gridLayout->addWidget(proxyFinder, 0, 1, 1, 2);
+ connect(proxyFinder->getUi()->cbProxyName, SIGNAL(currentIndexChanged(int)), this, SLOT(selectionChanged(int)));
}
KinematicUnitConfigDialog::~KinematicUnitConfigDialog()
@@ -71,7 +63,6 @@ KinematicUnitConfigDialog::~KinematicUnitConfigDialog()
void KinematicUnitConfigDialog::onInitComponent()
{
proxyFinder->setIceManager(getIceManager());
- topicFinder->setIceManager(getIceManager());
}
void KinematicUnitConfigDialog::onConnectComponent()
@@ -85,28 +76,46 @@ void KinematicUnitConfigDialog::onExitComponent()
}
-
-
-void KinematicUnitConfigDialog::modelFileSelected()
-{
- ui->editRobotFilepath->setText(fileDialog->selectedFiles()[0]);
-}
-
void KinematicUnitConfigDialog::verifyConfig()
{
if (proxyFinder->getSelectedProxyName().trimmed().length() == 0)
{
QMessageBox::critical(this, "Invalid Configuration", "The proxy name must not be empty");
}
- else if (topicFinder->getSelectedProxyName().trimmed().length() == 0)
- {
- QMessageBox::critical(this, "Invalid Configuration", "The topic name must not be empty");
- }
else
{
this->accept();
}
}
+void KinematicUnitConfigDialog::selectionChanged(int nr)
+{
+ ARMARX_LOG << "Selected entry:" << nr;
+ ui->labelTopic->setText("");
+ ui->labelRobotModel->setText("");
+ ui->labelRNS->setText("");
+ if (nr < 0)
+ {
+ return;
+ }
+ std::string kinematicUnitName = proxyFinder->getUi()->cbProxyName->currentText().toStdString();
+ try
+ {
+ ARMARX_INFO << "Connecting to KinematicUnitProxy " << kinematicUnitName;
+ KinematicUnitInterfacePrx kinematicUnitInterfacePrx = getProxy<KinematicUnitInterfacePrx>(kinematicUnitName);
+ std::string topicName = kinematicUnitInterfacePrx->getReportTopicName();
+ std::string robotNodeSetName = kinematicUnitInterfacePrx->getRobotNodeSetName();
+ std::string rfile = kinematicUnitInterfacePrx->getRobotFilename();
+ ui->labelTopic->setText(QString(topicName.c_str()));
+ ui->labelRobotModel->setText(QString(rfile.c_str()));
+ ui->labelRNS->setText(QString(robotNodeSetName.c_str()));
+ }
+ catch (...)
+ {
+ ARMARX_INFO << "Could not connect to KinematicUnitProxy " << kinematicUnitName;
+ }
+
+}
+
diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.h b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.h
index a6990bacf..f1366c34e 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.h
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.h
@@ -61,14 +61,17 @@ namespace armarx
signals:
public slots:
- void modelFileSelected();
+ //void modelFileSelected();
void verifyConfig();
+
+ void selectionChanged(int nr);
private:
IceProxyFinderBase* proxyFinder;
- IceProxyFinderBase* topicFinder;
+ //IceProxyFinderBase* proxyFinderRobotState;
+ //IceProxyFinderBase* topicFinder;
Ui::KinematicUnitConfigDialog* ui;
- QFileDialog* fileDialog;
+ //QFileDialog* fileDialog;
std::string uuid;
friend class KinematicUnitWidgetController;
};
diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.ui b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.ui
index c5a20a981..499209e79 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.ui
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.ui
@@ -6,8 +6,8 @@
<rect>
<x>0</x>
<y>0</y>
- <width>527</width>
- <height>133</height>
+ <width>464</width>
+ <height>190</height>
</rect>
</property>
<property name="sizePolicy">
@@ -35,65 +35,69 @@
</item>
<item row="0" column="0">
<layout class="QGridLayout" name="gridLayout">
- <item row="2" column="0">
- <widget class="QLabel" name="labelKinematicUnitName">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Minimum" vsizetype="Preferred">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
+ <property name="spacing">
+ <number>5</number>
+ </property>
+ <item row="1" column="0">
+ <widget class="QLabel" name="labelRobotFilepath">
<property name="text">
- <string>Kinematic unit name</string>
+ <string>Robot Model</string>
</property>
</widget>
</item>
- <item row="1" column="1" colspan="2">
- <widget class="QLineEdit" name="ediRobotNodeSetName"/>
- </item>
- <item row="0" column="2">
- <widget class="QToolButton" name="btnSelectModelFile">
+ <item row="4" column="1">
+ <widget class="QLabel" name="labelTopic">
<property name="text">
- <string>...</string>
- </property>
- <property name="icon">
- <iconset>
- <normaloff>:/icons/document-open-folder.png</normaloff>:/icons/document-open-folder.png</iconset>
+ <string><not set></string>
</property>
</widget>
</item>
- <item row="0" column="1">
- <widget class="QLineEdit" name="editRobotFilepath"/>
+ <item row="2" column="0">
+ <widget class="QLabel" name="labelRobotNodeSetName">
+ <property name="text">
+ <string>Robot Node Set</string>
+ </property>
+ </widget>
</item>
- <item row="0" column="0">
- <widget class="QLabel" name="labelRobotFilepath">
+ <item row="2" column="1">
+ <widget class="QLabel" name="labelRNS">
<property name="text">
- <string>Robot model filepath</string>
+ <string><not set></string>
</property>
</widget>
</item>
- <item row="1" column="0">
- <widget class="QLabel" name="labelRobotNodeSetName">
+ <item row="1" column="1">
+ <widget class="QLabel" name="labelRobotModel">
<property name="text">
- <string>Robot nodeset name</string>
+ <string><not set></string>
</property>
</widget>
</item>
- <item row="3" column="0">
+ <item row="4" column="0">
<widget class="QLabel" name="label">
<property name="text">
<string>RobotState Topic Name</string>
</property>
</widget>
</item>
+ <item row="0" column="0">
+ <widget class="QLabel" name="labelKinematicUnitName">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Minimum" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>Kinematic unit name</string>
+ </property>
+ </widget>
+ </item>
</layout>
</item>
</layout>
</widget>
<tabstops>
- <tabstop>editRobotFilepath</tabstop>
- <tabstop>btnSelectModelFile</tabstop>
- <tabstop>ediRobotNodeSetName</tabstop>
<tabstop>buttonBox</tabstop>
</tabstops>
<resources/>
diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
index 636b567c2..fc779ad5e 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
@@ -4,6 +4,7 @@
#include <RobotAPI/gui-plugins/KinematicUnitPlugin/ui_KinematicUnitConfigDialog.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
#include <ArmarXCore/core/ArmarXManager.h>
@@ -34,10 +35,10 @@
#include <boost/filesystem.hpp>
-#define KINEMATIC_UNIT_FILE_DEFAULT std::string("RobotAPI/robots/Armar3/ArmarIII.xml")
-#define KINEMATIC_UNIT_FILE_DEFAULT_PACKAGE std::string("RobotAPI")
+//#define KINEMATIC_UNIT_FILE_DEFAULT std::string("RobotAPI/robots/Armar3/ArmarIII.xml")
+//#define KINEMATIC_UNIT_FILE_DEFAULT_PACKAGE std::string("RobotAPI")
#define KINEMATIC_UNIT_NAME_DEFAULT "Robot"
-#define TOPIC_NAME_DEFAULT "RobotState"
+//#define TOPIC_NAME_DEFAULT "RobotState"
using namespace armarx;
using namespace VirtualRobot;
@@ -69,46 +70,12 @@ void KinematicUnitWidgetController::onInitComponent()
ARMARX_INFO << flush;
verbose = true;
- // // parsing properties from ice config
- // kinematicUnitFile = getProperty<std::string>("RobotFileName").getValue();
- // robotNodeSetName = getProperty<std::string>("RobotNodeSetName").getValue();
- // kinematicUnitName = getProperty<std::string>("KinematicUnitName").getValue();
- ARMARX_INFO << "RobotFileName: " << kinematicUnitFile << flush;
- ARMARX_INFO << "RobotNodeSetName: " << robotNodeSetName << flush;
- ARMARX_INFO << "KinematicUnitName: " << kinematicUnitName << flush;
-
- // VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robotFile, VirtualRobot::RobotIO::eStructure);
-
- // init
- robot = loadRobotFile(kinematicUnitFile);
-
- if (!robot)
- {
- getObjectScheduler()->terminate();
-
- if (getWidget()->parentWidget())
- {
- getWidget()->parentWidget()->close();
- }
-
- std::cout << "returning" << std::endl;
- return;
- }
-
- rootVisu = new SoSeparator();
+ rootVisu = new SoSeparator;
rootVisu->ref();
-
- kinematicUnitVisualization = getCoinVisualization(robot);
- kinematicUnitNode = kinematicUnitVisualization->getCoinVisualization();
- rootVisu->addChild(kinematicUnitNode);
-
- robotNodeSet = getRobotNodeSet(robot, robotNodeSetName);
-
- if (!robotNodeSet)
- {
- ARMARX_ERROR << "Failed to obtain RobotNodeSet '" << robotNodeSetName << "'";
- }
+ robotVisu = new SoSeparator;
+ robotVisu->ref();
+ rootVisu->addChild(robotVisu);
// create the debugdrawer component
std::string debugDrawerComponentName = "KinemticUnitGUIDebugDrawer_" + getName();
@@ -128,6 +95,7 @@ void KinematicUnitWidgetController::onInitComponent()
ArmarXManagerPtr m = getArmarXManager();
m->addObject(debugDrawer, false);
+
{
boost::recursive_mutex::scoped_lock lock(*mutex3D);
debugLayerVisu = new SoSeparator();
@@ -136,40 +104,133 @@ void KinematicUnitWidgetController::onInitComponent()
rootVisu->addChild(debugLayerVisu);
}
-
string widgetLabel = "KinematicUnit: " + kinematicUnitName;
ui.labelKinematicUnitName->setText(QString(widgetLabel.c_str()));
- initGUIComboBox(robotNodeSet); // init the pull down menu (QT: ComboBox)
- initGUIJointListTable(robotNodeSet);
connectSlots();
- usingTopic(topicName);
-
usingProxy(kinematicUnitName);
}
void KinematicUnitWidgetController::onConnectComponent()
{
- if (!robot)
+ kinematicUnitInterfacePrx = getProxy<KinematicUnitInterfacePrx>(kinematicUnitName);
+ topicName = kinematicUnitInterfacePrx->getReportTopicName();
+
+ robotVisu->removeAllChildren();
+
+ robot.reset();
+
+ std::string rfile;
+ StringList includePaths;
+
+ // get robot filename
+ try
+ {
+
+ StringList packages = kinematicUnitInterfacePrx->getArmarXPackages();
+ packages.push_back(Application::GetProjectName());
+ ARMARX_VERBOSE << "ArmarX packages " << packages;
+
+ for (const std::string & projectName : packages)
+ {
+ if (projectName.empty())
+ {
+ continue;
+ }
+
+ CMakePackageFinder project(projectName);
+ StringList projectIncludePaths;
+ auto pathsString = project.getDataDir();
+ ARMARX_VERBOSE << "Data paths of ArmarX package " << projectName << ": " << pathsString;
+ boost::split(projectIncludePaths,
+ pathsString,
+ boost::is_any_of(";,"),
+ boost::token_compress_on);
+ ARMARX_VERBOSE << "Result: Data paths of ArmarX package " << projectName << ": " << projectIncludePaths;
+ includePaths.insert(includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
+ }
+
+ rfile = kinematicUnitInterfacePrx->getRobotFilename();
+ ARMARX_VERBOSE << "Relative robot file " << rfile;
+ ArmarXDataPath::getAbsolutePath(rfile, rfile, includePaths);
+ ARMARX_VERBOSE << "Absolute robot file " << rfile;
+
+ robotNodeSetName = kinematicUnitInterfacePrx->getRobotNodeSetName();
+ }
+ catch (...)
+ {
+ ARMARX_ERROR << "Unable to retrieve robot filename";
+ }
+
+ try
{
+ ARMARX_INFO << "Loading robot from file " << rfile;
+ robot = loadRobotFile(rfile);
+ }
+ catch (...)
+ {
+ ARMARX_ERROR << "Failed to init robot";
+ }
+
+ if (!robot || !robot->hasRobotNodeSet(robotNodeSetName))
+ {
+ getObjectScheduler()->terminate();
+
+ if (getWidget()->parentWidget())
+ {
+ getWidget()->parentWidget()->close();
+ }
+
+ std::cout << "returning" << std::endl;
return;
}
- ARMARX_INFO << flush;
+ kinematicUnitFile = rfile;
+ robotNodeSet = robot->getRobotNodeSet(robotNodeSetName);
+
- // get proxy to send commands to the kinematic unit
- std::string kinematicUnitInstructionChannel = kinematicUnitName;
- // NOW: kinematicUnitInstructionChannel -> HeadKinematicUnit
- kinematicUnitInterfacePrx = getProxy<KinematicUnitInterfacePrx>(kinematicUnitInstructionChannel);
+ initGUIComboBox(robotNodeSet); // init the pull down menu (QT: ComboBox)
+ initGUIJointListTable(robotNodeSet);
+
+ kinematicUnitVisualization = getCoinVisualization(robot);
+ kinematicUnitNode = kinematicUnitVisualization->getCoinVisualization();
+ robotVisu->addChild(kinematicUnitNode);
// init control mode map
- reportedJointControlModes = kinematicUnitInterfacePrx->getControlModes();
+ try
+ {
+ reportedJointControlModes = kinematicUnitInterfacePrx->getControlModes();
+ }
+ catch (...)
+ {
+ }
initializeUi();
+
+ usingTopic(topicName);
+
enableMainWidgetAsync(true);
}
+void KinematicUnitWidgetController::onDisconnectComponent()
+{
+ enableMainWidgetAsync(false);
+
+ {
+ boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
+ robot.reset();
+ robotNodeSet.reset();
+ currentNode.reset();
+ }
+
+ {
+ boost::recursive_mutex::scoped_lock lock(*mutex3D);
+ robotVisu->removeAllChildren();
+ debugLayerVisu->removeAllChildren();
+ }
+}
+
void KinematicUnitWidgetController::onExitComponent()
{
enableMainWidgetAsync(false);
@@ -177,6 +238,13 @@ void KinematicUnitWidgetController::onExitComponent()
{
boost::recursive_mutex::scoped_lock lock(*mutex3D);
+ if (robotVisu)
+ {
+ robotVisu->removeAllChildren();
+ robotVisu->unref();
+ robotVisu = NULL;
+ }
+
if (debugLayerVisu)
{
debugLayerVisu->removeAllChildren();
@@ -208,29 +276,6 @@ QPointer<QDialog> KinematicUnitWidgetController::getConfigDialog(QWidget* parent
{
dialog = new KinematicUnitConfigDialog(parent);
dialog->setName(dialog->getDefaultName());
-
- armarx::CMakePackageFinder finder(KINEMATIC_UNIT_FILE_DEFAULT_PACKAGE);
-
- if (!finder.packageFound())
- {
- ARMARX_WARNING << "ArmarX Package " << KINEMATIC_UNIT_FILE_DEFAULT_PACKAGE << " has not been found!";
- }
- else
- {
- ARMARX_INFO << "Adding to datapaths: " << finder.getDataDir();
- armarx::ArmarXDataPath::addDataPaths(finder.getDataDir());
- }
-
- std::string filename = KINEMATIC_UNIT_FILE_DEFAULT;
- ArmarXDataPath::getAbsolutePath(filename, filename);
-
- boost::filesystem::path dir(filename);
-
-
- dialog->fileDialog->setDirectory(QString::fromStdString(dir.remove_filename().string()));
- dialog->ui->editRobotFilepath->setText(QString::fromStdString(filename));
- dialog->ui->ediRobotNodeSetName->setText(KINEMATIC_UNIT_NAME_DEFAULT);
-
}
return qobject_cast<KinematicUnitConfigDialog*>(dialog);
@@ -239,38 +284,17 @@ QPointer<QDialog> KinematicUnitWidgetController::getConfigDialog(QWidget* parent
void KinematicUnitWidgetController::configured()
{
ARMARX_VERBOSE << "KinematicUnitWidget::configured()";
- kinematicUnitFile = dialog->ui->editRobotFilepath->text().toStdString();
- robotNodeSetName = dialog->ui->ediRobotNodeSetName->text().toStdString();
kinematicUnitName = dialog->proxyFinder->getSelectedProxyName().toStdString();
- topicName = dialog->topicFinder->getSelectedProxyName().toStdString();
}
void KinematicUnitWidgetController::loadSettings(QSettings* settings)
{
- kinematicUnitFile = settings->value("kinematicUnitFile", QString::fromStdString(KINEMATIC_UNIT_FILE_DEFAULT)).toString().toStdString();
- ArmarXDataPath::getAbsolutePath(kinematicUnitFile, kinematicUnitFile);
kinematicUnitName = settings->value("kinematicUnitName", KINEMATIC_UNIT_NAME_DEFAULT).toString().toStdString();
- robotNodeSetName = settings->value("robotNodeSetName", KINEMATIC_UNIT_NAME_DEFAULT).toString().toStdString();
- topicName = settings->value("topicName", TOPIC_NAME_DEFAULT).toString().toStdString();
}
void KinematicUnitWidgetController::saveSettings(QSettings* settings)
{
- std::string robFile = kinematicUnitFile;
-
- try
- {
- robFile = ArmarXDataPath::getRelativeArmarXPath(robFile);
- }
- catch (...)
- {
- ARMARX_WARNING << "Could not get relative filename, using full filename:" << robFile;
- }
-
settings->setValue("kinematicUnitName", QString::fromStdString(kinematicUnitName));
- settings->setValue("robotNodeSetName", QString::fromStdString(robotNodeSetName));
- settings->setValue("kinematicUnitFile", QString::fromStdString(robFile));
- settings->setValue("topicName", QString::fromStdString(topicName));
}
@@ -355,8 +379,14 @@ void KinematicUnitWidgetController::kinematicUnitZeroPosition()
jointAngles[rn[i]->getName()] = 0.0f;
}
- kinematicUnitInterfacePrx->switchControlMode(jointModes);
- kinematicUnitInterfacePrx->setJointAngles(jointAngles);
+ try
+ {
+ kinematicUnitInterfacePrx->switchControlMode(jointModes);
+ kinematicUnitInterfacePrx->setJointAngles(jointAngles);
+ }
+ catch (...)
+ {
+ }
//set slider to 0 if position control mode is used
if (selectedControlMode == ePositionControl)
@@ -422,9 +452,16 @@ void KinematicUnitWidgetController::setControlModeVelocity()
ui.horizontalSliderKinematicUnitPos->setMaximum(90);
ui.horizontalSliderKinematicUnitPos->setMinimum(-90);
- if (kinematicUnitInterfacePrx)
+ try
{
- kinematicUnitInterfacePrx->switchControlMode(jointModes);
+ if (kinematicUnitInterfacePrx)
+ {
+ kinematicUnitInterfacePrx->switchControlMode(jointModes);
+ }
+ }
+ catch (...)
+ {
+
}
selectedControlMode = eVelocityControl;
@@ -446,7 +483,14 @@ void KinematicUnitWidgetController::setControlModeTorque()
if (kinematicUnitInterfacePrx)
{
- kinematicUnitInterfacePrx->switchControlMode(jointModes);
+ try
+ {
+ kinematicUnitInterfacePrx->switchControlMode(jointModes);
+ }
+ catch (...)
+ {
+
+ }
}
}
@@ -693,7 +737,13 @@ void KinematicUnitWidgetController::sliderValueChanged(int pos)
if (kinematicUnitInterfacePrx)
{
- kinematicUnitInterfacePrx->setJointAngles(jointAngles);
+ try
+ {
+ kinematicUnitInterfacePrx->setJointAngles(jointAngles);
+ }
+ catch (...)
+ {
+ }
updateModel();
}
}
@@ -704,7 +754,13 @@ void KinematicUnitWidgetController::sliderValueChanged(int pos)
if (kinematicUnitInterfacePrx)
{
- kinematicUnitInterfacePrx->setJointVelocities(jointVelocities);
+ try
+ {
+ kinematicUnitInterfacePrx->setJointVelocities(jointVelocities);
+ }
+ catch (...)
+ {
+ }
updateModel();
}
}
@@ -715,7 +771,13 @@ void KinematicUnitWidgetController::sliderValueChanged(int pos)
if (kinematicUnitInterfacePrx)
{
- kinematicUnitInterfacePrx->setJointTorques(jointTorques);
+ try
+ {
+ kinematicUnitInterfacePrx->setJointTorques(jointTorques);
+ }
+ catch (...)
+ {
+ }
updateModel();
}
}
diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h
index 6c248994f..656ba4d10 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h
@@ -28,6 +28,7 @@
#include <RobotAPI/gui-plugins/KinematicUnitPlugin/ui_kinematicunitguiplugin.h>
#include <RobotAPI/interface/units/KinematicUnitInterface.h>
#include <RobotAPI/components/DebugDrawer/DebugDrawerComponent.h>
+#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
@@ -143,6 +144,7 @@ namespace armarx
// inherited from Component
virtual void onInitComponent();
virtual void onConnectComponent();
+ virtual void onDisconnectComponent();
virtual void onExitComponent();
// inherited of ArmarXWidget
@@ -222,11 +224,15 @@ namespace armarx
KinematicUnitInterfacePrx kinematicUnitInterfacePrx;// send commands to kinematic unit
KinematicUnitListenerPrx kinematicUnitListenerPrx; // receive reports from kinematic unit
+
+ //RobotStateComponentInterfacePrx robotStateComponentPrx;
+
bool verbose;
std::string kinematicUnitFile;
std::string kinematicUnitFileDefault;
std::string kinematicUnitName;
+ //std::string robotStateComponentName;
std::string topicName;
std::string robotNodeSetName;
@@ -237,6 +243,7 @@ namespace armarx
CoinVisualizationPtr kinematicUnitVisualization;
SoNode* kinematicUnitNode;
SoSeparator* rootVisu;
+ SoSeparator* robotVisu;
SoSeparator* debugLayerVisu;
armarx::DebugDrawerComponentPtr debugDrawer;
diff --git a/source/RobotAPI/interface/core/RobotState.ice b/source/RobotAPI/interface/core/RobotState.ice
index d37226e2d..c65d1ee36 100644
--- a/source/RobotAPI/interface/core/RobotState.ice
+++ b/source/RobotAPI/interface/core/RobotState.ice
@@ -149,13 +149,20 @@ module armarx
idempotent
StringList getArmarXPackages();
- /**
- * @return The name of the robot represented by this component. Same as
- * getSynchronizedRobot()->getName()
- *
- */
- ["cpp:const"]
- idempotent string getRobotName() throws NotInitializedException;
+ /**
+ * @return The name of the robot represented by this component. Same as
+ * getSynchronizedRobot()->getName()
+ *
+ */
+ ["cpp:const"]
+ idempotent string getRobotName() throws NotInitializedException;
+
+ /**
+ * @return The name of the robot node set that is represented by this component.
+ *
+ */
+ ["cpp:const"]
+ idempotent string getRobotNodeSetName() throws NotInitializedException;
};
};
diff --git a/source/RobotAPI/interface/units/KinematicUnitInterface.ice b/source/RobotAPI/interface/units/KinematicUnitInterface.ice
index ff0f43253..9b52ec9da 100644
--- a/source/RobotAPI/interface/units/KinematicUnitInterface.ice
+++ b/source/RobotAPI/interface/units/KinematicUnitInterface.ice
@@ -176,7 +176,42 @@ module armarx
*/
NameControlModeMap getControlModes();
- /*
+ /**
+ * @return the robot xml filename as specified in the configuration
+ */
+ ["cpp:const"]
+ idempotent
+ string getRobotFilename();
+
+ /**
+ * @return All dependent packages, which might contain a robot file.
+ */
+ ["cpp:const"]
+ idempotent
+ StringList getArmarXPackages();
+
+ /**
+ * @return The name of the robot used by this component.
+ *
+ */
+ ["cpp:const"]
+ idempotent string getRobotName() throws NotInitializedException;
+
+ /**
+ * @return The name of the robot node set that is used by this component.
+ *
+ */
+ ["cpp:const"]
+ idempotent string getRobotNodeSetName() throws NotInitializedException;
+
+ /**
+ * @return The name of the report topic that is offered by this unit.
+ *
+ */
+ ["cpp:const"]
+ idempotent string getReportTopicName() throws NotInitializedException;
+
+ /*
* NYI
*/
//void set Trajectory(...);
--
GitLab