diff --git a/source/RobotAPI/components/RobotNameService/RobotNameService.cpp b/source/RobotAPI/components/RobotNameService/RobotNameService.cpp
index 9ab8e07d7ca5234e40e167ae73201cae04105db5..397a638912d93ef767bcd23795e7e2e709b0f18a 100644
--- a/source/RobotAPI/components/RobotNameService/RobotNameService.cpp
+++ b/source/RobotAPI/components/RobotNameService/RobotNameService.cpp
@@ -54,8 +54,8 @@ namespace armarx
}
bool
- RobotNameService::registerRobot(const robot_name_service::dto::Robot& robot,
- const Ice::Current& current)
+ RobotNameService::registerRobotInfo(const robot_name_service::dto::RobotInfo& robot,
+ const Ice::Current& current)
{
std::scoped_lock l(robotsMutex);
ARMARX_INFO << "Register a new robot with name '" << robot.name << "' in RNS";
@@ -72,7 +72,7 @@ namespace armarx
}
void
- RobotNameService::unregisterRobot(const std::string& name, const Ice::Current& current)
+ RobotNameService::unregisterRobotInfo(const std::string& name, const Ice::Current& current)
{
std::scoped_lock l(robotsMutex);
@@ -82,8 +82,8 @@ namespace armarx
}
}
- IceUtil::Optional<robot_name_service::dto::Robot>
- RobotNameService::getRobot(const std::string& name, const Ice::Current& current)
+ IceUtil::Optional<robot_name_service::dto::RobotInfo>
+ RobotNameService::getRobotInfo(const std::string& name, const Ice::Current& current)
{
std::scoped_lock l(robotsMutex);
diff --git a/source/RobotAPI/components/RobotNameService/RobotNameService.h b/source/RobotAPI/components/RobotNameService/RobotNameService.h
index 7af787aef845a26c8ed47f7b9351c36cd6dce002..6cc056c194cdc73146551ae1da50e097b438c894 100644
--- a/source/RobotAPI/components/RobotNameService/RobotNameService.h
+++ b/source/RobotAPI/components/RobotNameService/RobotNameService.h
@@ -87,14 +87,14 @@ namespace armarx
// RobotNameServiceInterface interface
public:
- bool registerRobot(const robot_name_service::dto::Robot& robot,
- const Ice::Current& current) override;
- void unregisterRobot(const std::string& name, const Ice::Current& current) override;
- IceUtil::Optional<robot_name_service::dto::Robot>
- getRobot(const std::string& name, const Ice::Current& current) override;
+ bool registerRobotInfo(const robot_name_service::dto::RobotInfo& robot,
+ const Ice::Current& current) override;
+ void unregisterRobotInfo(const std::string& name, const Ice::Current& current) override;
+ IceUtil::Optional<robot_name_service::dto::RobotInfo>
+ getRobotInfo(const std::string& name, const Ice::Current& current) override;
private:
mutable std::mutex robotsMutex;
- std::map<std::string, robot_name_service::core::Robot> robots;
+ std::map<std::string, robot_name_service::core::RobotInfo> robots;
};
} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/CMakeLists.txt b/source/RobotAPI/gui-plugins/SkillManagerPlugin/CMakeLists.txt
index 113ccd8706d8f7c64cbd067f1002a5969b3f9507..5df70b65810beeead4599151d25b15960a26e615 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/CMakeLists.txt
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/CMakeLists.txt
@@ -27,6 +27,7 @@ set(SOURCES
aronTreeWidget/modal/AronTreeWidgetModal.cpp
ColorPalettes.cpp
SkillManagerMonitorWidgetController.cpp
+ PeriodicUpdateWidget.cpp
)
set(HEADERS
@@ -53,6 +54,7 @@ set(HEADERS
aronTreeWidget/modal/bool/AronTreeWidgetBoolInputModalController.h
ColorPalettes.h
SkillManagerMonitorWidgetController.h
+ PeriodicUpdateWidget.h
)
set(GUI_UIS
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/PeriodicUpdateWidget.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/PeriodicUpdateWidget.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e23920f1bb06788a1e6e4a8dc1d598349b4729a0
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/PeriodicUpdateWidget.cpp
@@ -0,0 +1,141 @@
+#include "PeriodicUpdateWidget.h"
+
+#include <cmath>
+
+#include <QCheckBox>
+#include <QDoubleSpinBox>
+#include <QHBoxLayout>
+#include <QPushButton>
+#include <QTimer>
+
+namespace armarx
+{
+ PeriodicUpdateWidget::PeriodicUpdateWidget(double frequency, double maxFrequency)
+ {
+ setSizePolicy(QSizePolicy::Policy::Minimum, QSizePolicy::Policy::Minimum);
+
+ QLayout* vlayout = new QVBoxLayout();
+ QLayout* hlayout = new QHBoxLayout();
+ this->setLayout(vlayout);
+
+ const int margin = 0;
+ hlayout->setContentsMargins(margin, margin, margin, margin);
+
+ _updateButton = new QPushButton("Update", this);
+ _autoCheckBox = new QCheckBox("Auto Update", this);
+ _frequencySpinBox = new QDoubleSpinBox(this);
+ _frequencySpinBox->setValue(frequency);
+ _frequencySpinBox->setMinimum(0);
+ _frequencySpinBox->setMaximum(maxFrequency);
+ _frequencySpinBox->setSingleStep(0.5);
+ _frequencySpinBox->setSuffix(" Hz");
+
+ hlayout->addWidget(_updateButton);
+ hlayout->addWidget(_autoCheckBox);
+ hlayout->addWidget(_frequencySpinBox);
+
+ vlayout->addItem(hlayout);
+
+
+ _timer = new QTimer(this);
+ _updateTimerFrequency();
+ _frequencySpinBox->setEnabled(_autoCheckBox->isChecked());
+
+
+ // Private connections.
+ connect(_autoCheckBox, &QCheckBox::toggled, this, &This::_toggleAutoUpdates);
+ connect(_frequencySpinBox,
+ &QDoubleSpinBox::editingFinished,
+ this,
+ &This::_updateTimerFrequency);
+
+ // Public connections.
+ connect(_updateButton, &QPushButton::pressed, this, &This::updateSingle);
+ connect(_timer, &QTimer::timeout, this, &This::updatePeriodic);
+
+ connect(this, &This::updateSingle, this, &This::update);
+ connect(this, &This::updatePeriodic, this, &This::update);
+
+ // See `startTimerIfEnabled` for the signal reasoning.
+ // qOverload is required because `QTimer::start()` is overloaded.
+ connect(this, &This::startTimerSignal, _timer, qOverload<>(&QTimer::start));
+ connect(this, &This::stopTimerSignal, _timer, &QTimer::stop);
+ }
+
+ QPushButton*
+ PeriodicUpdateWidget::updateButton()
+ {
+ return _updateButton;
+ }
+
+ int
+ PeriodicUpdateWidget::getUpdateIntervalMs() const
+ {
+ return static_cast<int>(std::round(1000 / _frequencySpinBox->value()));
+ }
+
+ void
+ PeriodicUpdateWidget::startTimerIfEnabled()
+ {
+ /* A QTimer can only be started and stopped within its own thread (the thread for which
+ * it has the greatest affinity). Since this method can be called from any thread, we
+ * need to take a detour through these signals, which can be emitted from any thread and
+ * will always be caught in this object's (and thus the timer's) native thread.
+ */
+ if (_autoCheckBox->isChecked())
+ {
+ emit startTimerSignal();
+ }
+ else
+ {
+ emit stopTimerSignal();
+ }
+ }
+
+ void
+ PeriodicUpdateWidget::stopTimer()
+ {
+ // See `startTimerIfEnabled` for the signal reasoning.
+ emit stopTimerSignal();
+ }
+
+ void
+ PeriodicUpdateWidget::_updateTimerFrequency()
+ {
+ _timer->setInterval(getUpdateIntervalMs());
+ }
+
+ void
+ PeriodicUpdateWidget::_toggleAutoUpdates(bool enabled)
+ {
+ // This method is already a slot, so it doesn't need to use the timer signals.
+ _frequencySpinBox->setEnabled(enabled);
+ if (enabled)
+ {
+ _timer->start();
+ }
+ else
+ {
+ _timer->stop();
+ }
+ }
+
+ QCheckBox*
+ PeriodicUpdateWidget::autoCheckBox()
+ {
+ return _autoCheckBox;
+ }
+
+ QDoubleSpinBox*
+ PeriodicUpdateWidget::frequencySpinBox()
+ {
+ return _frequencySpinBox;
+ }
+
+ QTimer*
+ PeriodicUpdateWidget::timer()
+ {
+ return _timer;
+ }
+
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/PeriodicUpdateWidget.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/PeriodicUpdateWidget.h
new file mode 100644
index 0000000000000000000000000000000000000000..9d199db39afc0a7042f5a5225b1f09c08d14bf46
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/PeriodicUpdateWidget.h
@@ -0,0 +1,66 @@
+#pragma once
+
+#include <QWidget>
+
+
+class QCheckBox;
+class QDoubleSpinBox;
+class QPushButton;
+class QTimer;
+
+namespace armarx
+{
+
+ class PeriodicUpdateWidget : public QWidget
+ {
+ Q_OBJECT
+ using This = PeriodicUpdateWidget;
+
+ public:
+ PeriodicUpdateWidget(double frequency = 2.0, double maxFrequency = 60);
+
+
+ QTimer* timer();
+
+ QCheckBox* autoCheckBox();
+ QDoubleSpinBox* frequencySpinBox();
+ QPushButton* updateButton();
+
+ bool isAutoEnabled() const;
+ double getUpdateFrequency() const;
+ int getUpdateIntervalMs() const;
+
+ void startTimerIfEnabled();
+ void stopTimer();
+
+
+ public slots:
+
+ signals:
+
+ void update();
+
+ void updateSingle();
+ void updatePeriodic();
+
+ private slots:
+
+ void _updateTimerFrequency();
+ void _toggleAutoUpdates(bool enabled);
+
+ signals:
+
+ void startTimerSignal();
+ void stopTimerSignal();
+
+ private:
+ QPushButton* _updateButton;
+ QCheckBox* _autoCheckBox;
+ QDoubleSpinBox* _frequencySpinBox;
+
+ QPushButton* _collapseAllButton;
+
+ QTimer* _timer;
+ };
+
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidget.ui b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidget.ui
index 83e522b9d6f895b6f492ba63d13529712a3f194d..82569fa85833ef3eb89476da914b7ba02606864b 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidget.ui
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidget.ui
@@ -6,12 +6,12 @@
<rect>
<x>0</x>
<y>0</y>
- <width>1060</width>
- <height>657</height>
+ <width>1126</width>
+ <height>729</height>
</rect>
</property>
<property name="sizePolicy">
- <sizepolicy hsizetype="Expanding" vsizetype="Expanding">
+ <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
@@ -19,41 +19,14 @@
<property name="windowTitle">
<string>SkillManagerMonitorWidget</string>
</property>
- <layout class="QGridLayout" name="gridLayout_3">
- <item row="3" column="2">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxUpdateFreq"/>
- </item>
- <item row="3" column="0">
- <widget class="QCheckBox" name="checkBoxAutoUpdate">
- <property name="text">
- <string>Auto Update</string>
- </property>
- </widget>
- </item>
- <item row="3" column="1">
- <widget class="QLabel" name="label">
- <property name="text">
- <string>Update Frequency:</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
- </property>
- </widget>
- </item>
- <item row="3" column="3">
- <widget class="QPushButton" name="pushButtonRefreshNow">
- <property name="text">
- <string>Refresh Now</string>
- </property>
- </widget>
- </item>
- <item row="4" column="0" colspan="4">
+ <layout class="QGridLayout" name="gridLayout_3" rowstretch="0,0,0,0">
+ <item row="3" column="0" colspan="2">
<widget class="QSplitter" name="splitter_2">
<property name="enabled">
<bool>true</bool>
</property>
<property name="sizePolicy">
- <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+ <sizepolicy hsizetype="Expanding" vsizetype="Expanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
@@ -75,11 +48,14 @@
<string>Executions</string>
</property>
<layout class="QGridLayout" name="gridLayout_4">
- <item row="1" column="0" colspan="3">
+ <item row="0" column="0" colspan="3">
<widget class="QTreeWidget" name="treeWidgetSkillExecutions">
+ <property name="contextMenuPolicy">
+ <enum>Qt::CustomContextMenu</enum>
+ </property>
<column>
<property name="text">
- <string>ExecutionID</string>
+ <string>Timestamp</string>
</property>
</column>
<column>
@@ -94,27 +70,7 @@
</column>
<column>
<property name="text">
- <string>IsConstructing</string>
- </property>
- </column>
- <column>
- <property name="text">
- <string>IsInitializing</string>
- </property>
- </column>
- <column>
- <property name="text">
- <string>IsPreparing</string>
- </property>
- </column>
- <column>
- <property name="text">
- <string>IsRunning</string>
- </property>
- </column>
- <column>
- <property name="text">
- <string>Finished</string>
+ <string>Status</string>
</property>
</column>
</widget>
@@ -171,6 +127,9 @@
<item row="4" column="0">
<widget class="QLineEdit" name="lineEditSearch">
<property name="text">
+ <string/>
+ </property>
+ <property name="placeholderText">
<string>Search...</string>
</property>
</widget>
@@ -209,65 +168,65 @@
<bool>false</bool>
</property>
<widget class="QWidget" name="groupBoxSkillDetailsTop" native="true">
- <layout class="QVBoxLayout" name="verticalLayout_2">
- <property name="leftMargin">
- <number>0</number>
- </property>
- <property name="topMargin">
- <number>0</number>
- </property>
- <property name="rightMargin">
- <number>0</number>
- </property>
- <property name="bottomMargin">
- <number>0</number>
- </property>
- <item>
- <layout class="QGridLayout" name="gridLayout_5">
- <item row="0" column="0">
- <widget class="QPushButton" name="pushButtonPaste">
- <property name="text">
- <string>Set args from clipboard</string>
- </property>
- </widget>
- </item>
- <item row="0" column="3">
- <widget class="QPushButton" name="pushButtonReset">
- <property name="text">
- <string>Reset args to profile</string>
- </property>
- </widget>
- </item>
- <item row="1" column="0" colspan="4">
- <widget class="QComboBox" name="comboBoxProfiles">
- <item>
+ <layout class="QVBoxLayout" name="verticalLayout_2">
+ <property name="leftMargin">
+ <number>0</number>
+ </property>
+ <property name="topMargin">
+ <number>0</number>
+ </property>
+ <property name="rightMargin">
+ <number>0</number>
+ </property>
+ <property name="bottomMargin">
+ <number>0</number>
+ </property>
+ <item>
+ <layout class="QGridLayout" name="gridLayout_5">
+ <item row="0" column="0">
+ <widget class="QPushButton" name="pushButtonPaste">
<property name="text">
- <string><No Profile selected. Using root></string>
+ <string>Set args from clipboard</string>
</property>
- </item>
- </widget>
- </item>
- <item row="0" column="1">
- <widget class="QPushButton" name="pushButtonCopy">
- <property name="text">
- <string>Copy args to clipboard</string>
- </property>
- </widget>
- </item>
- <item row="0" column="2">
- <widget class="QLabel" name="label_2">
- <property name="text">
- <string/>
- </property>
- </widget>
- </item>
- </layout>
- </item>
- <item>
- <widget class="QWidget" name="skillDescription" native="true"/>
- </item>
- </layout>
- </widget>
+ </widget>
+ </item>
+ <item row="0" column="3">
+ <widget class="QPushButton" name="pushButtonReset">
+ <property name="text">
+ <string>Reset args to profile</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="0" colspan="4">
+ <widget class="QComboBox" name="comboBoxProfiles">
+ <item>
+ <property name="text">
+ <string><No Profile selected. Using root></string>
+ </property>
+ </item>
+ </widget>
+ </item>
+ <item row="0" column="1">
+ <widget class="QPushButton" name="pushButtonCopy">
+ <property name="text">
+ <string>Copy args to clipboard</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="2">
+ <widget class="QLabel" name="label_2">
+ <property name="text">
+ <string/>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item>
+ <widget class="QWidget" name="skillDescription" native="true"/>
+ </item>
+ </layout>
+ </widget>
<widget class="QWidget" name="groupBoxSkillDetailsBottom" native="true">
<layout class="QVBoxLayout" name="verticalLayout_3">
<property name="leftMargin">
@@ -328,6 +287,50 @@
</widget>
</widget>
</item>
+ <item row="0" column="0" rowspan="3" colspan="2">
+ <layout class="QHBoxLayout" name="horizontalLayout">
+ <property name="sizeConstraint">
+ <enum>QLayout::SetDefaultConstraint</enum>
+ </property>
+ <item>
+ <layout class="QHBoxLayout" name="updateWidgetLayout">
+ <item>
+ <spacer name="horizontalSpacer_2">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeType">
+ <enum>QSizePolicy::Expanding</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>40</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ </layout>
+ </item>
+ <item>
+ <layout class="QHBoxLayout" name="stopAllLayout">
+ <item>
+ <spacer name="horizontalSpacer">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>40</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </item>
</layout>
</widget>
<resources/>
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp
index 8b88718f93dc71bae29b16094d9701c0ae7d896a..bbeaa5a4d9a889808db465419f732e5903031a31 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp
@@ -22,11 +22,27 @@
#include "SkillManagerMonitorWidgetController.h"
+#include <optional>
+#include <regex>
#include <string>
+#include <SimoxUtility/algorithm/string.h>
+
+#include <RobotAPI/libraries/skills/core/Skill.h>
+
+#include "aronTreeWidget/visitors/AronTreeWidgetConverter.h"
+#include "aronTreeWidget/visitors/AronTreeWidgetCreator.h"
+#include "aronTreeWidget/visitors/AronTreeWidgetModalCreator.h"
+
+// modals
+#include "aronTreeWidget/modal/text/AronTreeWidgetTextInputModalController.h"
+
+// debug
+#include <QAction>
#include <QClipboard>
#include <QDoubleSpinBox>
#include <QGridLayout>
+#include <QMenu>
#include <QTextBrowser>
#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
@@ -40,8 +56,12 @@
#include "aronTreeWidget/visitors/AronTreeWidgetModalCreator.h"
#include "aronTreeWidget/widgets/SkillDescriptionWidget.h"
+//configSk
namespace armarx
{
+ const skills::SkillID SkillManagerMonitorWidgetController::SelectedSkill::UNK_SKILL_ID =
+ skills::SkillID{.providerId = ::std::nullopt, .skillName = skills::SkillID::UNKNOWN};
+
QPointer<QDialog>
SkillManagerMonitorWidgetController::getConfigDialog(QWidget* parent)
{
@@ -76,6 +96,38 @@ namespace armarx
// Others
namespace armarx
{
+ void
+ SkillManagerMonitorWidgetController::prepareAndRunMenu(const QPoint& pos)
+ {
+ QMenu* menu = new QMenu();
+
+ // Stop skill
+ QAction* stopSkillAction = new QAction("Stop execution", this);
+ skills::SkillStatus currentStatus =
+ skillStatusUpdates.at(selectedSkill.skillExecutionId).status;
+ stopSkillAction->setDisabled(currentStatus == skills::SkillStatus::Aborted ||
+ currentStatus == skills::SkillStatus::Failed ||
+ currentStatus == skills::SkillStatus::Succeeded);
+
+ QAction* rerunSkillAction = new QAction("Re-run with similar params", this);
+ menu->addAction(stopSkillAction);
+ menu->addAction(rerunSkillAction);
+ connect(stopSkillAction,
+ &QAction::triggered,
+ this,
+ &SkillManagerMonitorWidgetController::stopSkill);
+ connect(rerunSkillAction,
+ &QAction::triggered,
+ this,
+ &SkillManagerMonitorWidgetController::rerunSkillWithSimilarParams);
+
+ // Temporarily disable rerun-skill-Action
+ rerunSkillAction->setDisabled(true);
+
+ // open menu
+ menu->popup(widget.treeWidgetSkillExecutions->viewport()->mapToGlobal(pos));
+ }
+
SkillExecutionInfoTreeWidgetItem*
SkillExecutionInfoTreeWidgetItem::SearchRecursiveForMatch(
SkillExecutionInfoTreeWidgetItem* haystack,
@@ -101,13 +153,12 @@ namespace armarx
SkillManagerMonitorWidgetController::SkillManagerMonitorWidgetController()
{
widget.setupUi(getWidget());
+ this->updateWidget = new PeriodicUpdateWidget(2.0, 60);
+ widget.updateWidgetLayout->insertWidget(0, updateWidget);
+
+ this->stopAllButton = new QPushButton("Stop all executions");
+ widget.stopAllLayout->addWidget(stopAllButton);
- double default_hz = 2;
- widget.doubleSpinBoxUpdateFreq->setValue(default_hz);
- widget.doubleSpinBoxUpdateFreq->setMinimum(0.5);
- widget.doubleSpinBoxUpdateFreq->setMaximum(20);
- widget.doubleSpinBoxUpdateFreq->setSingleStep(0.5);
- widget.doubleSpinBoxUpdateFreq->setSuffix(" Hz");
skillDescriptionWidget = new SkillDescriptionWidget();
widget.skillDescription->parentWidget()->layout()->replaceWidget(widget.skillDescription,
@@ -115,16 +166,17 @@ namespace armarx
widget.skillDescription = skillDescriptionWidget;
- refreshSkillsResultTimer = new QTimer(this);
- updateTimerFrequency();
- refreshSkillsResultTimer->start();
-
- connect(widget.doubleSpinBoxUpdateFreq,
- &QDoubleSpinBox::editingFinished,
+ connect(this->stopAllButton,
+ &QPushButton::clicked,
this,
- &SkillManagerMonitorWidgetController::updateTimerFrequency);
- connect(refreshSkillsResultTimer,
- &QTimer::timeout,
+ &SkillManagerMonitorWidgetController::stopAllExecutions);
+ connect(widget.treeWidgetSkillExecutions,
+ &QTreeWidget::customContextMenuRequested,
+ this,
+ &SkillManagerMonitorWidgetController::prepareAndRunMenu);
+
+ connect(updateWidget,
+ &armarx::PeriodicUpdateWidget::update,
this,
&SkillManagerMonitorWidgetController::refreshSkillsAndExecutions);
@@ -140,21 +192,31 @@ namespace armarx
connect(widget.pushButtonExecuteSkill,
&QPushButton::clicked,
this,
- &SkillManagerMonitorWidgetController::executeSkill);
- // connect(widget.pushButtonStopSkill,
- // &QPushButton::clicked,
- // this,
- // &SkillManagerMonitorWidgetController::stopSkill);
+ &SkillManagerMonitorWidgetController::executeSelectedSkill);
connect(widget.treeWidgetSkills,
&QTreeWidget::currentItemChanged,
this,
&SkillManagerMonitorWidgetController::skillSelectionChanged);
- connect(widget.pushButtonRefreshNow,
+ connect(widget.treeWidgetSkillExecutions,
+ &QTreeWidget::currentItemChanged,
+ this,
+ &SkillManagerMonitorWidgetController::skillExecutionSelectionChanged);
+
+ /*connect(widget.pushButtonRefreshNow,
&QPushButton::clicked,
this,
- &SkillManagerMonitorWidgetController::refreshSkills);
+ &SkillManagerMonitorWidgetController::refreshSkills);*/
+ connect(widget.pushButtonSearch,
+ &QPushButton::clicked,
+ this,
+ &SkillManagerMonitorWidgetController::searchSkills);
+ // alternatively run search when pressing enter
+ connect(widget.lineEditSearch,
+ &QLineEdit::returnPressed,
+ this,
+ &SkillManagerMonitorWidgetController::searchSkills);
}
SkillManagerMonitorWidgetController::~SkillManagerMonitorWidgetController()
@@ -176,6 +238,7 @@ namespace armarx
widget.treeWidgetSkillDetails->setColumnHidden(3, true);
getProxy(memory, observerName, 1000);
+ this->updateWidget->startTimerIfEnabled();
connected = true;
}
@@ -192,23 +255,51 @@ namespace armarx
skillsArgumentsTreeWidgetItem = nullptr;
selectedSkill.skillId.providerId->providerName = "";
selectedSkill.skillId.skillName = "";
+ this->updateWidget->stopTimer();
}
void
- SkillManagerMonitorWidgetController::updateTimerFrequency()
+ SkillManagerMonitorWidgetController::searchSkills()
{
- int f = static_cast<int>(std::round(1000 / widget.doubleSpinBoxUpdateFreq->value()));
- refreshSkillsResultTimer->setInterval(f);
+ this->currentSkillSearch = widget.lineEditSearch->text();
}
void
SkillManagerMonitorWidgetController::refreshSkillsAndExecutions()
{
- if (widget.checkBoxAutoUpdate->isChecked())
+ refreshSkills();
+ refreshExecutions();
+ }
+
+ void
+ SkillManagerMonitorWidgetController::matchSkillUpdateToSearch(
+ std::map<skills::manager::dto::SkillID, skills::manager::dto::SkillDescription>& update)
+ {
+ /*
+ if (this->currentSkillSearch.isEmpty())
{
- refreshSkills();
- refreshExecutions();
+ return;
}
+
+ // TODO: whitespace to wildcard
+
+ std::string search = this->currentSkillSearch.toLower().toStdString();
+ std::transform(search.begin(), search.end(), search.begin(),
+ [](unsigned char c){ return std::tolower(c); });
+
+ for (auto it = update.begin(); it != update.end();)
+ {
+ if (simox::alg::to_lower(skills::SkillID::FromIce(it->first).skillName)
+ .find(this->currentSkillSearch.toLower().toStdString()))
+ {
+ it = update.erase(it);
+ }
+ else
+ {
+ ++it;
+ }
+ }
+*/
}
void
@@ -227,6 +318,7 @@ namespace armarx
std::scoped_lock l(updateMutex);
auto managerSkills = memory->getSkillDescriptions();
+ this->matchSkillUpdateToSearch(managerSkills);
// completely recreate internal skills map
skills.clear();
@@ -341,16 +433,16 @@ namespace armarx
SkillManagerMonitorWidgetController::refreshExecutions()
{
static std::map<skills::SkillStatus, std::string> ExecutionStatus2String = {
+ // Main states
+ {skills::SkillStatus::Constructing, "Constructing"},
+ {skills::SkillStatus::Initializing, "Initializing"},
+ {skills::SkillStatus::Preparing, "Preparing"},
+ {skills::SkillStatus::Running, "Running"},
+
// Terminating
{skills::SkillStatus::Aborted, "Aborted"},
{skills::SkillStatus::Failed, "Failed"},
- {skills::SkillStatus::Succeeded, "Succeeded"},
-
- // Others
- {skills::SkillStatus::Constructing, "Under construction"},
- {skills::SkillStatus::Running, "Running"},
- {skills::SkillStatus::Initializing, "Initializing"},
- {skills::SkillStatus::Preparing, "Preparing"}};
+ {skills::SkillStatus::Succeeded, "Succeeded"}};
if (!memory)
{
@@ -362,15 +454,18 @@ namespace armarx
{
std::scoped_lock l(updateMutex);
- auto currentManagerStatuses =
- memory
- ->getSkillExecutionStatuses(); // we assume that there are no more than 100 new skills..
+ auto currentManagerStatuses = memory->getSkillExecutionStatuses();
for (const auto& [k, v] : currentManagerStatuses)
{
auto executionId = skills::SkillExecutionID::FromIce(k);
auto statusUpdate = skills::SkillStatusUpdate::FromIce(v);
+ // update maps
+ skillStatusUpdates[executionId] = statusUpdate;
+ //skillExecutionParams.insert_or_assign(executionId,
+ // statusUpdate.usedParameterization);
+
SkillExecutionInfoTreeWidgetItem* found = nullptr;
for (int i = 0; i < widget.treeWidgetSkillExecutions->topLevelItemCount(); ++i)
{
@@ -382,22 +477,14 @@ namespace armarx
if (found)
{
- // update values
- found->setText(3,
- QString::fromStdString(
- statusUpdate.hasBeenConstructed() ? " yes " : " no "));
- found->setText(4,
- QString::fromStdString(
- statusUpdate.hasBeenInitialized() ? " yes " : " no "));
- found->setText(5,
- QString::fromStdString(
- statusUpdate.hasBeenPrepared() ? " yes " : " no "));
- found->setText(6,
- QString::fromStdString(
- statusUpdate.hasBeenRunning() ? " yes " : " no "));
- found->setText(7,
- QString::fromStdString(
- statusUpdate.hasBeenTerminated() ? " yes " : " no "));
+ for (std::pair<skills::SkillStatus, std::string> i : ExecutionStatus2String)
+ {
+ if (i.first == statusUpdate.status)
+ {
+ found->setText(3, QString::fromStdString(i.second));
+ }
+ }
+
break;
}
}
@@ -405,29 +492,22 @@ namespace armarx
if (!found)
{
// TODO: Sort to executor!
- auto item = new SkillExecutionInfoTreeWidgetItem(
- executionId, widget.treeWidgetSkillExecutions);
+ auto item = new SkillExecutionInfoTreeWidgetItem(executionId);
item->setText(0,
QString::fromStdString(
executionId.executionStartedTime.toDateTimeString()));
item->setText(1, QString::fromStdString(executionId.executorName));
item->setText(2, QString::fromStdString(executionId.skillId.toString()));
- item->setText(3,
- QString::fromStdString(
- statusUpdate.hasBeenConstructed() ? " yes " : " no "));
- item->setText(4,
- QString::fromStdString(
- statusUpdate.hasBeenInitialized() ? " yes " : " no "));
- item->setText(
- 5,
- QString::fromStdString(statusUpdate.hasBeenPrepared() ? " yes " : " no "));
- item->setText(
- 6,
- QString::fromStdString(statusUpdate.hasBeenRunning() ? " yes " : " no "));
- item->setText(7,
- QString::fromStdString(
- statusUpdate.hasBeenTerminated() ? " yes " : " no "));
+ for (std::pair<skills::SkillStatus, std::string> i : ExecutionStatus2String)
+ {
+ if (i.first == statusUpdate.status)
+ {
+ item->setText(3, QString::fromStdString(i.second));
+ }
+ }
+
+ widget.treeWidgetSkillExecutions->insertTopLevelItem(0, item);
}
}
}
@@ -438,31 +518,48 @@ namespace armarx
}
void
- SkillManagerMonitorWidgetController::executeSkill()
+ SkillManagerMonitorWidgetController::executeSelectedSkill()
{
- if (not selectedSkill.skillId.isFullySpecified())
- {
- return;
- }
+ auto data = getConfigAsAron();
+ executeSkillWithParams(selectedSkill.skillId, data);
+ }
+
+ void
+ SkillManagerMonitorWidgetController::rerunSkillWithSimilarParams()
+ {
+ // TODO: disabled until skillparameterization is replaced
+ //skills::SkillParameterization selectedExecutionParams =
+ //skillExecutionParams.at(selectedSkill.skillExecutionId);
+ //executeSkillWithParams(selectedSkill.skillExecutionId.skillId,
+ // selectedExecutionParams.parameterization);
+ executeSelectedSkill();
+ }
+
+ void
+ SkillManagerMonitorWidgetController::executeSkillWithParams(skills::SkillID skillId,
+ aron::data::DictPtr params)
+ {
std::scoped_lock l(updateMutex);
- auto providerId = *selectedSkill.skillId.providerId;
- const auto& skillDescriptions = skills.at(providerId);
- if (skillDescriptions.find(selectedSkill.skillId) == skillDescriptions.end())
+ auto providerId = skillId.providerId;
+ if (!providerId.has_value())
+ {
+ return;
+ }
+ const auto& skillDescriptions = skills.at(providerId.value());
+ if (skillDescriptions.find(skillId) == skillDescriptions.end())
{
return;
}
-
- auto data = getConfigAsAron();
char hostname[HOST_NAME_MAX];
gethostname(hostname, HOST_NAME_MAX);
- skills::SkillExecutionRequest req{selectedSkill.skillId,
- "Skills.Manager GUI (hostname: " + std::string(hostname) +
- ")",
- data};
+ skills::SkillExecutionRequest req{
+ .skillId = selectedSkill.skillId,
+ .executorName = "Skills.Manager GUI (hostname: " + std::string(hostname) + ")",
+ .parameters = params};
ARMARX_CHECK(selectedSkill.skillId.isFullySpecified()); // sanity check
ARMARX_IMPORTANT << "Executing skill from GUI: " << selectedSkill.skillId << ".";
@@ -470,24 +567,112 @@ namespace armarx
memory->begin_executeSkill(req.toManagerIce());
}
+ void
+ SkillManagerMonitorWidgetController::stopAllExecutions()
+ {
+ QTreeWidget const* tree = widget.treeWidgetSkillExecutions;
+
+ int const max_retries = 3;
+ int left_retries = max_retries;
+ bool retry = false;
+
+ do
+ {
+ retry = false;
+
+ for (ssize_t i = 0; i < tree->topLevelItemCount(); ++i)
+ {
+ SkillExecutionInfoTreeWidgetItem const* item =
+ dynamic_cast<SkillExecutionInfoTreeWidgetItem*>(
+ widget.treeWidgetSkillExecutions->topLevelItem(i));
+
+ if (not item)
+ {
+ // At this point, dynamic-casting failed and we don't know anything about the
+ // type.
+ ARMARX_ERROR << "Cannot stop unknown skill.";
+ retry = true;
+ continue;
+ }
+
+ try
+ {
+ ARMARX_INFO << "Aborting skill '" << item->executionId.skillId.skillName
+ << "'...";
+ memory->abortSkillAsync(item->executionId.toManagerIce());
+ }
+ catch (Ice::Exception const& e)
+ {
+ retry = true;
+ ARMARX_ERROR << "Unhandeled Ice exception while aborting skill '"
+ << item->executionId.skillId.skillName << "'.";
+ }
+ catch (...)
+ {
+ retry = true;
+ ARMARX_ERROR << "Unhandled error while aborting skill '"
+ << item->executionId.skillId.skillName << "'.";
+ }
+ }
+
+ if (retry)
+ {
+ left_retries -= 1;
+
+ if (left_retries > 0)
+ {
+ ARMARX_WARNING << "There where errors aborting skills. Retrying...";
+ }
+ else
+ {
+ ARMARX_ERROR << "Couldn't abort all skills after " << max_retries
+ << " tries. Giving up.";
+ retry = false;
+ }
+ }
+ } while (retry);
+ }
+
void
SkillManagerMonitorWidgetController::stopSkill()
{
- // std::scoped_lock l(updateMutex);
- // if (selectedSkill.skillId.isFullySpecified())
- // {
- // return;
- // }
+ std::scoped_lock l(updateMutex);
+
+ /*
+ if (selectedSkill.skillExecutionId.skillId.fullySpecified())
+ {
+ ARMARX_INFO << "The user requested to stop a skill, which was not fully specified!";
+ return;
+ }
+
+ const auto& skillDescriptions = skills.at(*selectedSkill.skillId.providerId);
+ if (!skillDescriptions.count(selectedSkill.skillId.skillName))
+ {
+ return;
+ }
+*/
+
+ ARMARX_INFO << "Stopping skill from GUI: " << selectedSkill.skillExecutionId.toString();
+
+ memory->abortSkillAsync(selectedSkill.skillExecutionId.toManagerIce());
+ }
- // const auto& skillDescriptions = skills.at(*selectedSkill.skillId.providerId);
- // if (!skillDescriptions.count(selectedSkill.skillId.skillName))
- // {
- // return;
- // }
+ void
+ SkillManagerMonitorWidgetController::skillExecutionSelectionChanged(QTreeWidgetItem* current,
+ QTreeWidgetItem*)
+ {
+ std::scoped_lock l(updateMutex);
+ widget.groupBoxSkillExecutions->setEnabled(false);
- // ARMARX_INFO << "Stopping skill from GUI: " << selectedSkill.skillId;
+ if (!current)
+ {
+ // gui has died?
+ return;
+ }
- // manager->abortSkill(selectedSkill.skillId);
+ auto c = static_cast<SkillExecutionInfoTreeWidgetItem*>(current);
+ selectedSkill.skillExecutionId = c->executionId;
+ widget.groupBoxSkillExecutions->setEnabled(true);
}
void
@@ -506,14 +691,14 @@ namespace armarx
if (!current->parent())
{
// no parent available. Perhaps provider clicked? Reset selected skill.
- selectedSkill.skillId.providerId->providerName = "";
- selectedSkill.skillId.skillName = "";
+ selectedSkill.skillId = SelectedSkill::UNK_SKILL_ID;
return;
}
auto c = static_cast<SkillInfoTreeWidgetItem*>(current);
auto skillDescription = c->skillDescription;
+
if (selectedSkill.skillId == skillDescription.skillId)
{
// no change
@@ -537,7 +722,28 @@ namespace armarx
ARMARX_CHECK(skills.at(*selectedSkill.skillId.providerId).count(selectedSkill.skillId) > 0);
auto skillDesc = skills.at(*selectedSkill.skillId.providerId).at(selectedSkill.skillId);
- skillDescriptionWidget->setSkillDescription(skillDesc);
+ {
+ auto it = new QTreeWidgetItem(widget.treeWidgetSkillDetails,
+ {QString::fromStdString("Name"),
+ QString::fromStdString(skillDesc.skillId.skillName)});
+ widget.treeWidgetSkillDetails->addTopLevelItem(it);
+ }
+
+ {
+ auto it = new QTreeWidgetItem(widget.treeWidgetSkillDetails,
+ {QString::fromStdString("Description"),
+ QString::fromStdString(skillDesc.description)});
+ widget.treeWidgetSkillDetails->addTopLevelItem(it);
+ }
+
+ {
+ auto it = new QTreeWidgetItem(
+ widget.treeWidgetSkillDetails,
+ {QString::fromStdString("Timeout"),
+ QString::fromStdString(std::to_string(skillDesc.timeout.toMilliSeconds())) +
+ " ms"});
+ widget.treeWidgetSkillDetails->addTopLevelItem(it);
+ }
// select root profile
widget.comboBoxProfiles->setCurrentIndex(0);
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.h
index 9f251bf9e1e5890d9059836fa12017fca9264925..786f13d869aee2c6c80d7de9058e840ef0305db1 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.h
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.h
@@ -33,6 +33,7 @@
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
+#include "RobotAPI/gui-plugins/SkillManagerPlugin/PeriodicUpdateWidget.h"
#include <RobotAPI/gui-plugins/SkillManagerPlugin/ui_SkillManagerMonitorWidget.h>
#include <RobotAPI/interface/skills/SkillMemoryInterface.h>
#include <RobotAPI/libraries/aron/core/data/variant/All.h>
@@ -66,6 +67,7 @@ namespace armarx
class SkillExecutionInfoTreeWidgetItem : public QTreeWidgetItem
{
+ //Q_OBJECT
public:
SkillExecutionInfoTreeWidgetItem(const skills::SkillExecutionID& id,
QTreeWidgetItem* parent) :
@@ -73,6 +75,12 @@ namespace armarx
{
}
+ // After constructing an item, it must be manually inserted into the tree!
+ SkillExecutionInfoTreeWidgetItem(const skills::SkillExecutionID& id) : executionId(id)
+ {
+ }
+
+ // When using this constructor, the new item will be appended to the bottom of the tree!
SkillExecutionInfoTreeWidgetItem(const skills::SkillExecutionID& id, QTreeWidget* parent) :
QTreeWidgetItem(parent), executionId(id)
{
@@ -113,15 +121,16 @@ namespace armarx
void onInitComponent() override;
void onConnectComponent() override;
- void onDisconnectComponent() override;
+ void onDisconnectComponent();
private slots:
void skillSelectionChanged(QTreeWidgetItem* current, QTreeWidgetItem* previous);
+ void skillExecutionSelectionChanged(QTreeWidgetItem* current, QTreeWidgetItem* previous);
- void stopSkill();
- void executeSkill();
+ void stopAllExecutions();
+ void stopSkill(); // TODO: rename
+ void executeSelectedSkill();
- void updateTimerFrequency();
void refreshSkills();
void refreshExecutions();
void refreshSkillsAndExecutions();
@@ -130,6 +139,11 @@ namespace armarx
void pasteCurrentConfig();
void resetCurrentConfig();
+ void prepareAndRunMenu(const QPoint& pos);
+ void rerunSkillWithSimilarParams();
+
+ void searchSkills();
+
private:
aron::data::DictPtr getConfigAsAron() const;
@@ -140,6 +154,7 @@ namespace armarx
*/
Ui::SkillManagerMonitorWidget widget;
QPointer<SimpleConfigDialog> dialog;
+ QString currentSkillSearch;
std::string observerName = "SkillManager";
skills::dti::SkillMemoryInterfacePrx memory = nullptr;
@@ -150,24 +165,41 @@ namespace armarx
{};
std::map<skills::SkillExecutionID, skills::SkillStatusUpdate> skillStatusUpdates = {};
+ // store copies (!) of skill descriptions
+ //std::map<skills::SkillExecutionID, skills::SkillParameterization> skillExecutionParams = {};
+
// User Input
struct SelectedSkill
{
+ static const skills::SkillID UNK_SKILL_ID;
+
skills::SkillID skillId;
+ skills::SkillExecutionID skillExecutionId;
// make default constructable
SelectedSkill() :
- skillId{.providerId = skills::ProviderID{.providerName = ""}, .skillName = ""}
+ skillId(UNK_SKILL_ID),
+ skillExecutionId{.skillId = UNK_SKILL_ID,
+ .executorName = skills::SkillExecutionID::UNKNOWN,
+ .executionStartedTime = armarx::core::time::DateTime::Invalid()}
{
}
- } selectedSkill;
+ }
+
+ selectedSkill;
+
+ void executeSkillWithParams(skills::SkillID skillId, aron::data::DictPtr params);
+ void matchSkillUpdateToSearch(std::map<skills::manager::dto::SkillID,
+ skills::manager::dto::SkillDescription>& update);
+
+
+ PeriodicUpdateWidget* updateWidget =
+ nullptr; // TODO: this should not be held by the controller!
+ QPushButton* stopAllButton = nullptr; // also this. Oh well...
// Helper to get the treeWidgetItem easily
QTreeWidgetItem* skillsArgumentsTreeWidgetItem = nullptr;
AronTreeWidgetControllerPtr aronTreeWidgetController = nullptr;
-
- // others
- QTimer* refreshSkillsResultTimer;
SkillDescriptionWidget* skillDescriptionWidget = nullptr;
// connected flag
diff --git a/source/RobotAPI/interface/robot_name_service/RobotNameServiceInterface.ice b/source/RobotAPI/interface/robot_name_service/RobotNameServiceInterface.ice
index 7891ed5dbfd4583eb592ebf8913d9c12ca3e9a4c..5fb248cd5f0824e78020c9279f72864b59688ead 100644
--- a/source/RobotAPI/interface/robot_name_service/RobotNameServiceInterface.ice
+++ b/source/RobotAPI/interface/robot_name_service/RobotNameServiceInterface.ice
@@ -27,10 +27,12 @@
#include <RobotAPI/interface/armem/mns.ice>
#include <RobotAPI/interface/skills/SkillManagerInterface.ice>
+#include <RobotAPI/interface/units/ForceTorqueUnit.ice>
#include <RobotAPI/interface/units/HandUnitInterface.ice>
#include <RobotAPI/interface/units/KinematicUnitInterface.ice>
#include <RobotAPI/interface/units/LocalizationUnitInterface.ice>
#include <RobotAPI/interface/units/PlatformUnitInterface.ice>
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice>
module armarx
{
@@ -39,24 +41,38 @@ module armarx
module dto
{
- struct Hand
+ module location
+ {
+ enum Side
+ {
+ LEFT,
+ RIGHT,
+ UNKNOWN
+ };
+ };
+
+ struct TCPInfo
{
string name;
string ft_name;
+ location::Side side;
HandUnitInterface* handUnitInterface;
};
- dictionary<string, Hand> Hands;
+ // maps tcp name to tcp
+ dictionary<string, TCPInfo> TCPInfos;
- struct Arm
+ struct ArmInfo
{
string kinematicChainName;
- Hand hand;
+ location::Side side;
+ TCPInfo tcpInfo;
};
- dictionary<string, Arm> Arms;
+ // maps kinematic chain name to arm
+ dictionary<string, ArmInfo> ArmInfos;
- struct Robot
+ struct RobotInfo
{
string name;
armarx::data::PackagePath xmlPackagePath;
@@ -66,12 +82,10 @@ module armarx
armarx::skills::manager::dti::SkillManagerInterface* skillManager;
// kinematic stuff. MAY BE EMPTY
- Arms arms;
+ ArmInfos armInfos;
- // units. MAY BE NULL
- armarx::KinematicUnitInterface* kinematicUnitInterface;
- armarx::PlatformUnitInterface* platformUnitInterface;
- armarx::LocalizationUnitInterface* localizationUnitInterface;
+ // RobotUnit
+ armarx::RobotUnitInterface* robotUnit;
// TODO: Feel free to extend!
};
@@ -81,9 +95,9 @@ module armarx
{
interface RobotNameServiceInterface
{
- bool registerRobot(dto::Robot robot);
- void unregisterRobot(string name);
- optional(1) dto::Robot getRobot(string name);
+ bool registerRobotInfo(dto::RobotInfo robot);
+ void unregisterRobotInfo(string name);
+ optional(1) dto::RobotInfo getRobotInfo(string name);
};
};
};
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/LocationLoader.cpp b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/LocationLoader.cpp
index cfeb43db57231ff85188667c60773c6d0e30390c..9a667a933b40e6da8f24810862a79dd934205e6c 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/LocationLoader.cpp
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/LocationLoader.cpp
@@ -6,6 +6,8 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
+#include <RobotAPI/libraries/core/json_conversions.h>
+
namespace armarx::priorknowledge::util
{
FramedLocationPtr
@@ -53,8 +55,18 @@ namespace armarx::priorknowledge::util
pose,
framedPose.at("pose").get<std::vector<std::vector<float>>>()); // load the 4x4 matrix
- FramedLocationPtr loc(new FramedLocation(
- LocationId(source, locationName), LocationType::FRAMED_LOCATION, frame, agent, pose));
+ std::optional<Names> names;
+ if (auto it = j.find("names"); it != j.end())
+ {
+ it->get_to(names.emplace());
+ }
+
+ FramedLocationPtr loc(new FramedLocation(LocationId(source, locationName),
+ LocationType::FRAMED_LOCATION,
+ frame,
+ agent,
+ pose,
+ names));
return loc;
}
@@ -111,12 +123,19 @@ namespace armarx::priorknowledge::util
extents,
framedOrientedBox.at("extents").get<std::vector<float>>()); // load the 4x4 matrix
+ std::optional<Names> names;
+ if (auto it = j.find("names"); it != j.end())
+ {
+ it->get_to(names.emplace());
+ }
+
FramedBoxedLocationPtr loc(new FramedBoxedLocation(LocationId(source, locationName),
LocationType::FRAMED_BOXED_LOCATION,
frame,
agent,
pose,
- extents));
+ extents,
+ names));
return loc;
}
@@ -133,7 +152,7 @@ namespace armarx::priorknowledge::util
}
for (const auto& [locationName, j] :
- js["locations"].get<std::map<std::string, nlohmann::json>>())
+ js.at("locations").get<std::map<std::string, nlohmann::json>>())
{
if (j.find("framedPose") != j.end())
{
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/datatypes/Location.h b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/datatypes/Location.h
index 2a1eccb4cbe72f30ac0da1582c294eb6508ef0f2..f4a05204d706c1598751b34459c7cbdae1a5ca91 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/datatypes/Location.h
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/datatypes/Location.h
@@ -1,11 +1,13 @@
#pragma once
#include <memory>
+#include <optional>
#include <string>
#include <SimoxUtility/shapes/OrientedBox.h>
#include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/libraries/core/Names.h>
namespace armarx::priorknowledge::util
{
@@ -31,8 +33,12 @@ namespace armarx::priorknowledge::util
{
LocationId id;
LocationType type;
+ std::optional<Names> names;
- Location(const LocationId& i, const LocationType t) : id(i), type(t)
+ Location(const LocationId& i,
+ const LocationType t,
+ const std::optional<Names>& names = std::nullopt) :
+ id(i), type(t), names(names)
{
}
@@ -45,12 +51,13 @@ namespace armarx::priorknowledge::util
std::string agent;
Eigen::Matrix4f pose;
- FramedLocation(const LocationId& i,
- const LocationType t,
- const std::string& f,
- const std::string& a,
- const Eigen::Matrix4f& p) :
- Location(i, t), frame(f), agent(a), pose(p)
+ FramedLocation(const LocationId& id,
+ const LocationType type,
+ const std::string& frame,
+ const std::string& agent,
+ const Eigen::Matrix4f& pose,
+ const std::optional<Names>& names = std::nullopt) :
+ Location(id, type, names), frame(frame), agent(agent), pose(pose)
{
}
@@ -63,13 +70,14 @@ namespace armarx::priorknowledge::util
{
Eigen::Vector3f extents;
- FramedBoxedLocation(const LocationId& i,
- const LocationType t,
- const std::string& f,
- const std::string& a,
- const Eigen::Matrix4f& p,
- const Eigen::Vector3f& e) :
- FramedLocation(i, t, f, a, p), extents(e)
+ FramedBoxedLocation(const LocationId& id,
+ const LocationType type,
+ const std::string& frame,
+ const std::string& agent,
+ const Eigen::Matrix4f& pose,
+ const Eigen::Vector3f& extents,
+ const std::optional<Names>& names = std::nullopt) :
+ FramedLocation(id, type, frame, agent, pose, names), extents(extents)
{
}
diff --git a/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.cpp b/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.cpp
index e5d468c552d96d70d3ef4ae92196cdb714f39163..3d8907a348bad7051e3fc88239f3b816899acd5e 100644
--- a/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.cpp
+++ b/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.cpp
@@ -44,12 +44,6 @@ namespace armarx::armem::client::util
}
}
- std::mutex&
- SimpleWriterBase::memoryWriterMutex()
- {
- return memoryMutex;
- }
-
armem::client::Writer&
SimpleWriterBase::memoryWriter()
{
diff --git a/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.h b/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.h
index 13a54cc5b213a90e1949f73c1b11de716c0593cc..895376b0a2f3ff1f30fdd070c2e50c62b9aa2968 100644
--- a/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.h
+++ b/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.h
@@ -58,7 +58,6 @@ namespace armarx::armem::client::util
virtual std::string propertyPrefix() const = 0;
virtual Properties defaultProperties() const = 0;
- std::mutex& memoryWriterMutex();
armem::client::Writer& memoryWriter();
@@ -66,7 +65,6 @@ namespace armarx::armem::client::util
Properties props;
armem::client::Writer memoryWriterClient;
- std::mutex memoryMutex;
};
} // namespace armarx::armem::client::util
diff --git a/source/RobotAPI/libraries/armem_locations/aron/Location.xml b/source/RobotAPI/libraries/armem_locations/aron/Location.xml
index d53f51c2c8f75bacae2868ae9bc155aad717379e..6b5bebc692a9898e63f508ae07939afd02085e22 100644
--- a/source/RobotAPI/libraries/armem_locations/aron/Location.xml
+++ b/source/RobotAPI/libraries/armem_locations/aron/Location.xml
@@ -1,14 +1,27 @@
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
+ <AronIncludes>
+ <Include include="RobotAPI/libraries/aron/common/aron/Names.xml" />
+ </AronIncludes>
<GenerateTypes>
+ <!--
+ ToDo: Model regions. Ideas:
+ - Polygon (convex, non-convex)
+ -
+ -->
+
<Object name='armarx::navigation::location::arondto::Location'>
<ObjectChild key='framedPose'>
<FramedPose />
</ObjectChild>
+ <ObjectChild key='names'>
+ <armarx::arondto::Names optional="true" />
+ </ObjectChild>
+
</Object>
</GenerateTypes>
diff --git a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
index 63da8444d40d90e6c4e73a1a4fd755082f4914f9..bbb52cc2b5f473d71cbf1d7c73e39002368f71ad 100644
--- a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
@@ -36,6 +36,8 @@ armarx_add_library(
server/attachments/Segment.h
+ client/class/ClassReader.h
+ client/class/ClassWriter.h
client/articulated_object/Reader.h
client/articulated_object/Writer.h
client/articulated_object/ArticulatedObjectReader.h
@@ -55,6 +57,8 @@ armarx_add_library(
types.cpp
utils.cpp
+ client/class/ClassReader.cpp
+ client/class/ClassWriter.cpp
client/articulated_object/Reader.cpp
client/articulated_object/Writer.cpp
client/articulated_object/ArticulatedObjectReader.cpp
diff --git a/source/RobotAPI/libraries/armem_objects/aron/ObjectClass.xml b/source/RobotAPI/libraries/armem_objects/aron/ObjectClass.xml
index e2d695429d3c4193ad97df106821f5bb912a0028..019987f5054e844478066d377ad0bf1e948b6db7 100644
--- a/source/RobotAPI/libraries/armem_objects/aron/ObjectClass.xml
+++ b/source/RobotAPI/libraries/armem_objects/aron/ObjectClass.xml
@@ -13,6 +13,30 @@ Core segment type of Object/Class.
</AronIncludes>
<GenerateTypes>
+ <Object name="armarx::armem::arondto::Feature">
+ <ObjectChild key="angle">
+ <float32 />
+ </ObjectChild>
+
+ <ObjectChild key="scale">
+ <float32 />
+ </ObjectChild>
+
+ <ObjectChild key="point2d">
+ <vector2f />
+ </ObjectChild>
+
+ <ObjectChild key="point3d">
+ <vector3f />
+ </ObjectChild>
+
+ <ObjectChild key="feature">
+ <List>
+ <float32 />
+ </List>
+ </ObjectChild>
+ </Object>
+
<Object name="armarx::armem::arondto::ObjectClass">
<ObjectChild key="id">
@@ -62,6 +86,12 @@ Core segment type of Object/Class.
<armarx::arondto::ObjectNames />
</ObjectChild>
+ <ObjectChild key="ivtFeatures">
+ <List>
+ <armarx::armem::arondto::Feature />
+ </List>
+ </ObjectChild>
+
</Object>
</GenerateTypes>
diff --git a/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp b/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp
index 6b3f3c184edb32070f6d7f28c853f340bcd78d32..da32c2771a2149f58e5ba3a1a01db93ecbe71028 100644
--- a/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp
@@ -111,6 +111,71 @@ namespace armarx::armem
} // namespace armarx::armem
+namespace armarx::armem::clazz
+{
+
+ void
+ fromAron(const arondto::Feature& dto, Feature& bo)
+ {
+ bo.angle = dto.angle;
+ bo.scale = dto.scale;
+ bo.feature = dto.feature;
+ bo.point2d = dto.point2d;
+ bo.point3d = dto.point3d;
+ }
+
+ void
+ toAron(arondto::Feature& dto, const Feature& bo)
+ {
+ dto.angle = bo.angle;
+ dto.scale = bo.scale;
+ dto.feature = bo.feature;
+ dto.point2d = bo.point2d;
+ dto.point3d = bo.point3d;
+ }
+
+ void
+ fromAron(const arondto::ObjectClass& dto, ObjectClass& bo)
+ {
+ armarx::fromAron(dto.id, bo.id);
+ armarx::fromAron(dto.simoxXmlPath, bo.simoxXmlPath);
+ armarx::fromAron(dto.articulatedSimoxXmlPath, bo.articulatedSimoxXmlPath);
+ armarx::fromAron(dto.urdfPath, bo.urdfPath);
+ armarx::fromAron(dto.articulatedUrdfPath, bo.articulatedUrdfPath);
+ armarx::fromAron(dto.sdfPath, bo.sdfPath);
+ armarx::fromAron(dto.meshWrlPath, bo.meshWrlPath);
+ armarx::fromAron(dto.meshObjPath, bo.meshObjPath);
+ armarx::fromAron(dto.aabb, bo.aabb);
+ armarx::fromAron(dto.oobb, bo.oobb);
+ bo.ivtFeatures.clear();
+ for (const auto& i : dto.ivtFeatures)
+ {
+ fromAron(i, bo.ivtFeatures.emplace_back());
+ }
+ }
+
+ void
+ toAron(arondto::ObjectClass& dto, const ObjectClass& bo)
+ {
+ armarx::toAron(dto.id, bo.id);
+ armarx::toAron(dto.simoxXmlPath, bo.simoxXmlPath);
+ armarx::toAron(dto.articulatedSimoxXmlPath, bo.articulatedSimoxXmlPath);
+ armarx::toAron(dto.urdfPath, bo.urdfPath);
+ armarx::toAron(dto.articulatedUrdfPath, bo.articulatedUrdfPath);
+ armarx::toAron(dto.sdfPath, bo.sdfPath);
+ armarx::toAron(dto.meshWrlPath, bo.meshWrlPath);
+ armarx::toAron(dto.meshObjPath, bo.meshObjPath);
+ armarx::toAron(dto.aabb, bo.aabb);
+ armarx::toAron(dto.oobb, bo.oobb);
+ dto.ivtFeatures.clear();
+ for (const auto& i : bo.ivtFeatures)
+ {
+ toAron(dto.ivtFeatures.emplace_back(), i);
+ }
+ }
+
+} // namespace armarx::armem::clazz
+
armarx::armem::MemoryID
armarx::armem::obj::makeObjectInstanceMemoryID(const objpose::ObjectPose& objectPose)
{
diff --git a/source/RobotAPI/libraries/armem_objects/aron_conversions.h b/source/RobotAPI/libraries/armem_objects/aron_conversions.h
index e27ed16c4cede302e2eecf2c013440d240162550..b61164a5d6a7de07bd8ba3761117e263ac714949 100644
--- a/source/RobotAPI/libraries/armem_objects/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_objects/aron_conversions.h
@@ -1,11 +1,10 @@
#pragma once
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
-
-#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron/Attachment.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron/Marker.aron.generated.h>
-
+#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/types.h>
namespace armarx::armem
@@ -18,19 +17,32 @@ namespace armarx::armem
/* Attachments */
- void fromAron(const arondto::attachment::AgentDescription& dto, attachment::AgentDescription& bo);
+ void fromAron(const arondto::attachment::AgentDescription& dto,
+ attachment::AgentDescription& bo);
void toAron(arondto::attachment::AgentDescription& dto, const attachment::AgentDescription& bo);
- void fromAron(const arondto::attachment::ObjectAttachment& dto, attachment::ObjectAttachment& bo);
+ void fromAron(const arondto::attachment::ObjectAttachment& dto,
+ attachment::ObjectAttachment& bo);
void toAron(arondto::attachment::ObjectAttachment& dto, const attachment::ObjectAttachment& bo);
- void fromAron(const arondto::attachment::ArticulatedObjectAttachment& dto, attachment::ArticulatedObjectAttachment& bo);
- void toAron(arondto::attachment::ArticulatedObjectAttachment& dto, const attachment::ArticulatedObjectAttachment& bo);
+ void fromAron(const arondto::attachment::ArticulatedObjectAttachment& dto,
+ attachment::ArticulatedObjectAttachment& bo);
+ void toAron(arondto::attachment::ArticulatedObjectAttachment& dto,
+ const attachment::ArticulatedObjectAttachment& bo);
+
+ void fromAron(const arondto::Marker& dto, marker::Marker& bo);
+ void toAron(arondto::Marker& dto, const marker::Marker& bo);
+} // namespace armarx::armem
+
+namespace armarx::armem::clazz
+{
+ void fromAron(const armarx::armem::arondto::Feature& dto, Feature& bo);
+ void toAron(armarx::armem::arondto::Feature& dto, const Feature& bo);
- void fromAron(const arondto::Marker& dto, marker::Marker&bo);
- void toAron(arondto::Marker& dto, const marker::Marker&bo);
-} // namespace armarx::armem
+ void fromAron(const armarx::armem::arondto::ObjectClass& dto, ObjectClass& bo);
+ void toAron(armarx::armem::arondto::ObjectClass& dto, const ObjectClass& bo);
+} // namespace armarx::armem::clazz
#include <RobotAPI/libraries/armem/core/MemoryID.h>
@@ -38,4 +50,4 @@ namespace armarx::armem::obj
{
/// Make a Memory ID for the object instance snapshot representing this pose.
MemoryID makeObjectInstanceMemoryID(const objpose::ObjectPose& objectPose);
-}
+} // namespace armarx::armem::obj
diff --git a/source/RobotAPI/libraries/armem_objects/client/class/ClassReader.cpp b/source/RobotAPI/libraries/armem_objects/client/class/ClassReader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bceba7d960ffbe41a620c36d47a5d8fc0897c157
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/client/class/ClassReader.cpp
@@ -0,0 +1,45 @@
+#include "ClassReader.h"
+
+namespace armarx::armem::obj::clazz
+{
+ std::optional<armem::clazz::ObjectClass>
+ ClassReader::getObjectClass(const std::string& providerName, const ObjectID& id)
+ {
+ auto mid = armarx::armem::MemoryID();
+ mid.memoryName = properties().memoryName;
+ mid.coreSegmentName = properties().coreSegmentName;
+ mid.providerSegmentName = providerName;
+ mid.entityName = id.str();
+
+ auto i = this->memoryReader().getLatestSnapshotIn(mid);
+
+ if (i.has_value() && i->size() == 1)
+ {
+ auto& instance = i->getInstance(0);
+ auto aron = instance.dataAs<armarx::armem::arondto::ObjectClass>();
+
+ armem::clazz::ObjectClass objClass;
+ armarx::armem::clazz::fromAron(aron, objClass);
+
+ return objClass;
+ }
+
+ return std::nullopt;
+ }
+
+ std::string
+ ClassReader::propertyPrefix() const
+ {
+ return "classReader.";
+ }
+
+ client::util::SimpleReaderBase::Properties
+ ClassReader::defaultProperties() const
+ {
+ client::util::SimpleReaderBase::Properties p;
+ p.memoryName = "Object";
+ p.coreSegmentName = "Class";
+ return p;
+ }
+
+} // namespace armarx::armem::obj::clazz
diff --git a/source/RobotAPI/libraries/armem_objects/client/class/ClassReader.h b/source/RobotAPI/libraries/armem_objects/client/class/ClassReader.h
new file mode 100644
index 0000000000000000000000000000000000000000..4d2805b4f03e81bb6b58b65d73997eeae56755ab
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/client/class/ClassReader.h
@@ -0,0 +1,49 @@
+/*
+ * 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
+
+#include <mutex>
+#include <optional>
+
+#include <RobotAPI/libraries/armem/client/util/SimpleReaderBase.h>
+#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+#include <RobotAPI/libraries/armem_objects/types.h>
+
+namespace armarx::armem::obj::clazz
+{
+ class ClassReader : public armem::client::util::SimpleReaderBase
+ {
+ public:
+ ClassReader() = default;
+
+ std::optional<armem::clazz::ObjectClass> getObjectClass(const std::string& providerName,
+ const armarx::ObjectID& id);
+
+ protected:
+ std::string propertyPrefix() const final;
+ Properties defaultProperties() const final;
+
+ private:
+ };
+
+
+} // namespace armarx::armem::obj::clazz
diff --git a/source/RobotAPI/libraries/armem_objects/client/class/ClassWriter.cpp b/source/RobotAPI/libraries/armem_objects/client/class/ClassWriter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6c1d36c77eb7360567b11665dd7d530cdbbd191c
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/client/class/ClassWriter.cpp
@@ -0,0 +1,59 @@
+#include "ClassWriter.h"
+
+namespace armarx::armem::obj::clazz
+{
+ ClassWriter::ClassWriter(const std::string& p) :
+ providerName(p)
+ {
+
+ }
+
+ void ClassWriter::setProviderName(const std::string& pName)
+ {
+ this->providerName = pName;
+ }
+
+ bool
+ ClassWriter::commitObjectClass(const armarx::armem::clazz::ObjectClass& c,
+ const armarx::core::time::DateTime& referenceTime)
+ {
+ armarx::armem::arondto::ObjectClass objClassAron;
+ armarx::armem::clazz::toAron(objClassAron, c);
+
+ armarx::armem::MemoryID mid;
+ mid.memoryName = properties().memoryName;
+ mid.coreSegmentName = properties().coreSegmentName;
+ mid.providerSegmentName = properties().providerName;
+ mid.entityName = c.id.str();
+
+ auto commit = armarx::armem::Commit();
+ auto& update = commit.add();
+
+ update.confidence = 1.0f;
+ update.referencedTime = referenceTime;
+ update.sentTime = armarx::core::time::DateTime::Now();
+ update.entityID = mid;
+ update.instancesData = {objClassAron.toAron()};
+
+ auto res = this->memoryWriter().commit(commit);
+
+ return res.allSuccess();
+ }
+
+ std::string
+ ClassWriter::propertyPrefix() const
+ {
+ return "classWriter.";
+ }
+
+ client::util::SimpleWriterBase::Properties
+ ClassWriter::defaultProperties() const
+ {
+ client::util::SimpleWriterBase::Properties p;
+ p.memoryName = "Object";
+ p.coreSegmentName = "Class";
+ p.providerName = providerName;
+ return p;
+ }
+
+} // namespace armarx::armem::obj::clazz
diff --git a/source/RobotAPI/libraries/armem_objects/client/class/ClassWriter.h b/source/RobotAPI/libraries/armem_objects/client/class/ClassWriter.h
new file mode 100644
index 0000000000000000000000000000000000000000..e4a3ecc99441c8e8106c0319442ae0c456012f1c
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/client/class/ClassWriter.h
@@ -0,0 +1,53 @@
+/*
+ * 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
+
+#include <mutex>
+#include <optional>
+
+#include <RobotAPI/libraries/armem/client/util/SimpleWriterBase.h>
+#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+#include <RobotAPI/libraries/armem_objects/types.h>
+
+namespace armarx::armem::obj::clazz
+{
+ class ClassWriter : public armem::client::util::SimpleWriterBase
+ {
+ public:
+ ClassWriter() = default;
+ ClassWriter(const std::string& p);
+
+ void setProviderName(const std::string& pName);
+
+ bool commitObjectClass(const armarx::armem::clazz::ObjectClass& c,
+ const armarx::core::time::DateTime& referenceTime);
+
+ protected:
+ std::string propertyPrefix() const final;
+ Properties defaultProperties() const final;
+
+ private:
+ std::string providerName;
+ };
+
+
+} // namespace armarx::armem::obj::clazz
diff --git a/source/RobotAPI/libraries/armem_objects/types.h b/source/RobotAPI/libraries/armem_objects/types.h
index 79fe9ac956f65869ceb03b35db8545155c4c963c..598c78ca9c6bc5f4fc007059352ee94fc28b2fec 100644
--- a/source/RobotAPI/libraries/armem_objects/types.h
+++ b/source/RobotAPI/libraries/armem_objects/types.h
@@ -23,6 +23,8 @@
#include <Eigen/Geometry>
+#include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
+
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/core/Time.h>
@@ -31,7 +33,6 @@
#include "aron_forward_declarations.h"
-
namespace armarx::armem
{
struct ObjectInstance
@@ -40,7 +41,36 @@ namespace armarx::armem
MemoryID classID;
MemoryID sourceID;
};
-}
+} // namespace armarx::armem
+
+namespace armarx::armem::clazz
+{
+ struct Feature
+ {
+ float angle;
+ float scale;
+ Eigen::Vector2f point2d;
+ Eigen::Vector3f point3d;
+ std::vector<float> feature;
+ };
+
+ struct ObjectClass
+ {
+ armarx::ObjectID id;
+ armarx::PackagePath simoxXmlPath;
+ armarx::PackagePath articulatedSimoxXmlPath;
+ armarx::PackagePath urdfPath;
+ armarx::PackagePath articulatedUrdfPath;
+ armarx::PackagePath sdfPath;
+ armarx::PackagePath articulatedSdfPath;
+ armarx::PackagePath meshWrlPath;
+ armarx::PackagePath meshObjPath;
+ simox::AxisAlignedBoundingBox aabb;
+ simox::OrientedBoxf oobb;
+ // TODO NAMES
+ std::vector<Feature> ivtFeatures;
+ };
+} // namespace armarx::armem::clazz
namespace armarx::armem::attachment
{
@@ -110,7 +140,6 @@ namespace armarx::armem::articulated_object
using ArticulatedObjects = armarx::armem::robot::Robots;
} // namespace armarx::armem::articulated_object
-
namespace armarx::armem::marker
{
class Marker
diff --git a/source/RobotAPI/libraries/armem_robot/aron/Robot.xml b/source/RobotAPI/libraries/armem_robot/aron/Robot.xml
index 7303d707d40e0f742a7faa2c9d1b8e4a9b561714..b7192c366e7ae685c176578b03a44d9f533f1e75 100644
--- a/source/RobotAPI/libraries/armem_robot/aron/Robot.xml
+++ b/source/RobotAPI/libraries/armem_robot/aron/Robot.xml
@@ -1,4 +1,3 @@
-<!--This class contains the data structure for ObjectPose -->
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
<AronIncludes>
diff --git a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.cpp b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.cpp
index a682162553005783d3ad74e42d78bffefbc09ae2..ca80477ebbf264624dd5f722ce2dee86853e8586 100644
--- a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.cpp
+++ b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.cpp
@@ -1,19 +1,19 @@
#include "Writer.h"
-#include <RobotAPI/libraries/armem_vision/aron_conversions.h>
#include <RobotAPI/libraries/armem_vision/aron/OccupancyGrid.aron.generated.h>
-
+#include <RobotAPI/libraries/armem_vision/aron_conversions.h>
namespace armarx::armem::vision::occupancy_grid::client
{
Writer::~Writer() = default;
- bool Writer::store(const OccupancyGrid& grid,
- const std::string& frame,
- const std::string& providerName,
- const std::int64_t& timestamp)
+ bool
+ Writer::store(const OccupancyGrid& grid,
+ const std::string& frame,
+ const std::string& providerName,
+ const std::int64_t& timestamp)
{
- std::lock_guard g{memoryWriterMutex()};
+ std::lock_guard g{writeMutex};
const auto result = memoryWriter().addSegment(properties().coreSegmentName, providerName);
@@ -28,7 +28,7 @@ namespace armarx::armem::vision::occupancy_grid::client
const auto iceTimestamp = Time(Duration::MicroSeconds(timestamp));
const auto providerId = armem::MemoryID(result.segmentID);
- const auto entityID = providerId.withEntityName(frame).withTimestamp(iceTimestamp);
+ const auto entityID = providerId.withEntityName(frame).withTimestamp(iceTimestamp);
armem::EntityUpdate update;
update.entityID = entityID;
@@ -41,7 +41,7 @@ namespace armarx::armem::vision::occupancy_grid::client
dict->addElement("grid", toAron(grid.grid));
update.instancesData = {dict};
- update.referencedTime = iceTimestamp;
+ update.referencedTime = iceTimestamp;
ARMARX_DEBUG << "Committing " << update << " at time " << iceTimestamp;
armem::EntityUpdateResult updateResult = memoryWriter().commit(update);
@@ -56,7 +56,8 @@ namespace armarx::armem::vision::occupancy_grid::client
return updateResult.success;
}
- std::string Writer::propertyPrefix() const
+ std::string
+ Writer::propertyPrefix() const
{
return "mem.vision.occupancy_grid.";
}
@@ -65,7 +66,7 @@ namespace armarx::armem::vision::occupancy_grid::client
Writer::defaultProperties() const
{
- return SimpleWriterBase::Properties{.memoryName = "Vision",
+ return SimpleWriterBase::Properties{.memoryName = "Vision",
.coreSegmentName = "OccupancyGrid"};
}
} // namespace armarx::armem::vision::occupancy_grid::client
diff --git a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.h b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.h
index df5e02ceaa5b2034e0028e7625c616924af8a2bf..7f950eab82ea37520855a07daf1560b9f30aee53 100644
--- a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.h
+++ b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.h
@@ -55,8 +55,10 @@ namespace armarx::armem::vision::occupancy_grid::client
protected:
std::string propertyPrefix() const override;
Properties defaultProperties() const override;
- };
+ private:
+ std::mutex writeMutex;
+ };
} // namespace armarx::armem::vision::occupancy_grid::client
diff --git a/source/RobotAPI/libraries/aron/common/CMakeLists.txt b/source/RobotAPI/libraries/aron/common/CMakeLists.txt
index 36cbeb1f837f96ea05b131a843654481b5195bb9..011f3d196f33eb1caba3640c6dce3688bb3b7483 100644
--- a/source/RobotAPI/libraries/aron/common/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/common/CMakeLists.txt
@@ -18,14 +18,15 @@ armarx_add_library(
forward_declarations.h
json_conversions.h
+ aron_conversions/armarx.h
aron_conversions/core.h
- aron_conversions/stl.h
- aron_conversions/packagepath.h
+ aron_conversions/eigen.h
aron_conversions/framed.h
- aron_conversions/time.h
- aron_conversions/armarx.h
+ aron_conversions/names.h
+ aron_conversions/packagepath.h
aron_conversions/simox.h
- aron_conversions/eigen.h
+ aron_conversions/stl.h
+ aron_conversions/time.h
json_conversions/armarx.h
json_conversions/framed.h
@@ -37,15 +38,15 @@ armarx_add_library(
util/object_finders.h
SOURCES
-
+ aron_conversions/armarx.cpp
aron_conversions/core.cpp
- aron_conversions/stl.cpp
- aron_conversions/packagepath.cpp
+ aron_conversions/eigen.cpp
aron_conversions/framed.cpp
- aron_conversions/time.cpp
- aron_conversions/armarx.cpp
+ aron_conversions/names.cpp
+ aron_conversions/packagepath.cpp
aron_conversions/simox.cpp
- aron_conversions/eigen.cpp
+ aron_conversions/stl.cpp
+ aron_conversions/time.cpp
json_conversions/armarx.cpp
json_conversions/framed.cpp
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions.h b/source/RobotAPI/libraries/aron/common/aron_conversions.h
index 95177ce57e2b6ab67e0e0e5e4f761646d2775302..c753c3c964ec032276b7a0492cf380ad356329fb 100644
--- a/source/RobotAPI/libraries/aron/common/aron_conversions.h
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions.h
@@ -1,6 +1,11 @@
#pragma once
+#include <RobotAPI/libraries/aron/common/aron_conversions/armarx.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/eigen.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/framed.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/names.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/packagepath.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/simox.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/stl.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/time.h>
#include <RobotAPI/libraries/aron/core/aron_conversions.h>
-#include "aron_conversions/armarx.h"
-#include "aron_conversions/simox.h"
-#include "aron_conversions/eigen.h"
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/names.cpp b/source/RobotAPI/libraries/aron/common/aron_conversions/names.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0871f1f68a89bea5142f7d17634068e9ac02026d
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/names.cpp
@@ -0,0 +1,18 @@
+#include "names.h"
+
+#include <RobotAPI/libraries/aron/common/aron/Names.aron.generated.h>
+#include <RobotAPI/libraries/core/Names.h>
+
+void
+armarx::fromAron(const arondto::Names& dto, Names& bo)
+{
+ bo.recognized = dto.recognized;
+ bo.spoken = dto.spoken;
+}
+
+void
+armarx::toAron(arondto::Names& dto, const Names& bo)
+{
+ dto.recognized = bo.recognized;
+ dto.spoken = bo.spoken;
+}
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/names.h b/source/RobotAPI/libraries/aron/common/aron_conversions/names.h
new file mode 100644
index 0000000000000000000000000000000000000000..237e2b20c1e076d55270cd08b756303dfb49d374
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/names.h
@@ -0,0 +1,14 @@
+#pragma once
+
+#include <SimoxUtility/json/json.h>
+
+#include <RobotAPI/libraries/aron/common/forward_declarations.h>
+#include <RobotAPI/libraries/core/forward_declarations.h>
+
+namespace armarx
+{
+ void fromAron(const arondto::Names& dto, armarx::Names& bo);
+ void toAron(arondto::Names& dto, const armarx::Names& bo);
+
+
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/aron/common/forward_declarations.h b/source/RobotAPI/libraries/aron/common/forward_declarations.h
index d3c7df9f82d251032ed5f1e49180f74f30cf223f..b6a0cf94f11c3fa43352d8c4cca3d29237ce3c51 100644
--- a/source/RobotAPI/libraries/aron/common/forward_declarations.h
+++ b/source/RobotAPI/libraries/aron/common/forward_declarations.h
@@ -5,10 +5,12 @@ namespace simox::arondto
class AxisAlignedBoundingBox;
class Color;
class OrientedBox;
+
} // namespace simox::arondto
namespace armarx::arondto
{
class Names;
class FrameID;
+
} // namespace armarx::arondto
diff --git a/source/RobotAPI/libraries/core/CMakeLists.txt b/source/RobotAPI/libraries/core/CMakeLists.txt
index 1e4b08820998f187015b03173f5e38c6fee21a14..7756a2cd5be8edad7ee10e8fdcdc25b4501cb008 100644
--- a/source/RobotAPI/libraries/core/CMakeLists.txt
+++ b/source/RobotAPI/libraries/core/CMakeLists.txt
@@ -20,6 +20,8 @@ set(LIBS RobotAPIInterfaces ArmarXCoreObservers ArmarXCoreStatechart ArmarXCoreE
)
set(LIB_FILES
+ json_conversions.cpp
+
PIDController.cpp
Trajectory.cpp
TrajectoryController.cpp
@@ -51,6 +53,7 @@ set(LIB_FILES
CartesianVelocityControllerWithRamp.cpp
CartesianNaturalPositionController.cpp
#CartesianNaturalVelocityController.cpp
+ Names.cpp
visualization/DebugDrawerTopic.cpp
visualization/GlasbeyLUT.cpp
@@ -60,6 +63,9 @@ set(LIB_FILES
)
set(LIB_HEADERS
+ forward_declarations.h
+ json_conversions.h
+
PIDController.h
MultiDimPIDController.h
Trajectory.h
@@ -103,14 +109,13 @@ set(LIB_HEADERS
CartesianNaturalPositionController.h
#CartesianNaturalVelocityController.h
EigenHelpers.h
+ Names.h
visualization/DebugDrawerTopic.h
visualization/GlasbeyLUT.h
#diffik/NaturalDiffIK.h
#diffik/SimpleDiffIK.h
-
- json_conversions.h
)
add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/core/Names.cpp b/source/RobotAPI/libraries/core/Names.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7e30ec9a3fb6abedb2bd7fb6f12de6e2aba967d1
--- /dev/null
+++ b/source/RobotAPI/libraries/core/Names.cpp
@@ -0,0 +1,7 @@
+#include "Names.h"
+
+namespace armarx
+{
+
+
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/Names.h b/source/RobotAPI/libraries/core/Names.h
new file mode 100644
index 0000000000000000000000000000000000000000..6f3ec69588d538c284bcee016db37af5808daed3
--- /dev/null
+++ b/source/RobotAPI/libraries/core/Names.h
@@ -0,0 +1,15 @@
+#pragma once
+
+#include <string>
+#include <vector>
+
+namespace armarx
+{
+
+ struct Names
+ {
+ std::vector<std::string> recognized;
+ std::vector<std::string> spoken;
+ };
+
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/forward_declarations.h b/source/RobotAPI/libraries/core/forward_declarations.h
new file mode 100644
index 0000000000000000000000000000000000000000..b3030f5921b332a881d7dffdb715ee076c1979e0
--- /dev/null
+++ b/source/RobotAPI/libraries/core/forward_declarations.h
@@ -0,0 +1,7 @@
+#pragma once
+
+namespace armarx
+{
+ class Names;
+ class FramedPose;
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/json_conversions.cpp b/source/RobotAPI/libraries/core/json_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..be883c3db8bb45f624ed5b87e489a73666c28b95
--- /dev/null
+++ b/source/RobotAPI/libraries/core/json_conversions.cpp
@@ -0,0 +1,46 @@
+#include "json_conversions.h"
+
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/libraries/core/Names.h>
+
+void
+armarx::to_json(nlohmann::json& j, const FramedPose& fp)
+{
+ j = nlohmann::json{{"agent", fp.agent},
+ {"frame", fp.frame},
+ {"qw", fp.orientation->qw},
+ {"qx", fp.orientation->qx},
+ {"qy", fp.orientation->qy},
+ {"qz", fp.orientation->qz},
+ {"x", fp.position->x},
+ {"y", fp.position->y},
+ {"z", fp.position->z}};
+}
+
+void
+armarx::from_json(const nlohmann::json& j, FramedPose& fp)
+{
+ j.at("agent").get_to(fp.agent);
+ j.at("frame").get_to(fp.frame);
+ j.at("qw").get_to(fp.orientation->qw);
+ j.at("qx").get_to(fp.orientation->qx);
+ j.at("qy").get_to(fp.orientation->qy);
+ j.at("qz").get_to(fp.orientation->qz);
+ j.at("x").get_to(fp.position->x);
+ j.at("y").get_to(fp.position->y);
+ j.at("z").get_to(fp.position->z);
+}
+
+void
+armarx::to_json(simox::json::json& j, const Names& value)
+{
+ j["recognized"] = value.recognized;
+ j["spoken"] = value.spoken;
+}
+
+void
+armarx::from_json(const simox::json::json& j, Names& value)
+{
+ j.at("recognized").get_to(value.recognized);
+ j.at("spoken").get_to(value.spoken);
+}
diff --git a/source/RobotAPI/libraries/core/json_conversions.h b/source/RobotAPI/libraries/core/json_conversions.h
index bccd85c6884cb75930932cec64a4480742191b20..0d45d0f05ead70ca1ea58c877b3ec0f65f8293f5 100644
--- a/source/RobotAPI/libraries/core/json_conversions.h
+++ b/source/RobotAPI/libraries/core/json_conversions.h
@@ -20,45 +20,18 @@
* GNU General Public License
*/
-
#pragma once
-
-// Simox
#include <SimoxUtility/json.h>
-// RobotAPI
-#include <RobotAPI/libraries/core/FramedPose.h>
-
+#include <RobotAPI/libraries/core/forward_declarations.h>
namespace armarx
{
- void to_json(nlohmann::json& j, const FramedPose& fp)
- {
- j = nlohmann::json
- {
- {"agent", fp.agent},
- {"frame", fp.frame},
- {"qw", fp.orientation->qw},
- {"qx", fp.orientation->qx},
- {"qy", fp.orientation->qy},
- {"qz", fp.orientation->qz},
- {"x", fp.position->x},
- {"y", fp.position->y},
- {"z", fp.position->z}
- };
- }
+ void to_json(nlohmann::json& j, const FramedPose& fp);
+ void from_json(const nlohmann::json& j, FramedPose& fp);
+
+ void to_json(simox::json::json& j, const Names& value);
+ void from_json(const simox::json::json& j, Names& value);
- void from_json(const nlohmann::json& j, FramedPose& fp)
- {
- j.at("agent").get_to(fp.agent);
- j.at("frame").get_to(fp.frame);
- j.at("qw").get_to(fp.orientation->qw);
- j.at("qx").get_to(fp.orientation->qx);
- j.at("qy").get_to(fp.orientation->qy);
- j.at("qz").get_to(fp.orientation->qz);
- j.at("x").get_to(fp.position->x);
- j.at("y").get_to(fp.position->y);
- j.at("z").get_to(fp.position->z);
- }
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/robot_name_service/client/Plugin.cpp b/source/RobotAPI/libraries/robot_name_service/client/Plugin.cpp
index 89f973a6e3f4588096a977091cdcbbeb4742bc9a..0f944891c8bfa557ba7eeb72c6e4ca15b732466f 100644
--- a/source/RobotAPI/libraries/robot_name_service/client/Plugin.cpp
+++ b/source/RobotAPI/libraries/robot_name_service/client/Plugin.cpp
@@ -19,6 +19,8 @@ namespace armarx::plugins
{
using namespace armarx::robot_name_service::client::constants;
+ ARMARX_INFO << "Got as config: " << properties.configJson;
+
nlohmann::json config = nlohmann::json::parse(properties.configJson);
if (config.contains(CONFIG_ROBOT_NAME))
@@ -53,7 +55,7 @@ namespace armarx::plugins
{
for (const auto& [k, config_arm] : config[CONFIG_ARMS].items())
{
- auto& arm = robot.arms[k];
+ auto& arm = robot.armInfos[k];
if (config_arm.contains(CONFIG_ARM_KINEMATIC_CHAIN_NAME))
{
@@ -109,7 +111,7 @@ namespace armarx::plugins
{
for (const auto& [k, config_arm] : config[CONFIG_ARMS].items())
{
- auto& arm = robot.arms[k];
+ auto& arm = robot.armInfos[k];
if (config_arm.contains(CONFIG_ARM_HAND))
{
@@ -126,7 +128,7 @@ namespace armarx::plugins
}
// finally commit robot
- properties.rns->registerRobot(robot.toIce());
+ properties.rns->registerRobotInfo(robot.toIce());
}
void
@@ -139,7 +141,7 @@ namespace armarx::plugins
void
RobotNameServiceComponentPlugin::preOnDisconnectComponent()
{
- properties.rns->unregisterRobot(robot.name);
+ properties.rns->unregisterRobotInfo(robot.name);
}
} // namespace armarx::plugins
diff --git a/source/RobotAPI/libraries/robot_name_service/client/Plugin.h b/source/RobotAPI/libraries/robot_name_service/client/Plugin.h
index 813003f98554196e32fa9cc2c27eb650cea5f914..5d59cd16f3755ab02d566b4bef1a5955cfe2be92 100644
--- a/source/RobotAPI/libraries/robot_name_service/client/Plugin.h
+++ b/source/RobotAPI/libraries/robot_name_service/client/Plugin.h
@@ -60,7 +60,7 @@ namespace armarx::plugins
armarx::robot_name_service::dti::RobotNameServiceInterfacePrx rns;
} properties;
- armarx::robot_name_service::core::Robot robot;
+ armarx::robot_name_service::core::RobotInfo robot;
friend class armarx::RobotNameServiceComponentPluginUser;
};
diff --git a/source/RobotAPI/libraries/robot_name_service/core/CMakeLists.txt b/source/RobotAPI/libraries/robot_name_service/core/CMakeLists.txt
index 6c702dd1e210e92e3e14c4d02a4e6d870a731c44..19ac42cfedb1959c36c25d6961ba589ebd91ad31 100644
--- a/source/RobotAPI/libraries/robot_name_service/core/CMakeLists.txt
+++ b/source/RobotAPI/libraries/robot_name_service/core/CMakeLists.txt
@@ -17,8 +17,18 @@ armarx_add_library(
SOURCES
Robot.cpp
+ aron_conversions.cpp
HEADERS
Robot.h
+ aron_conversions.h
+)
+
+armarx_enable_aron_file_generation_for_target(
+ TARGET_NAME
+ "${LIB_NAME}"
+ ARON_FILES
+ aron/Robot.xml
)
add_library(RobotAPI::robot_name_service_core ALIAS robot_name_service_core)
+
diff --git a/source/RobotAPI/libraries/robot_name_service/core/Robot.cpp b/source/RobotAPI/libraries/robot_name_service/core/Robot.cpp
index bfa5fbfe5d529388f9b545d93adae68d5f853f57..11978184a40e334b4b3b29099debf83976df04e9 100644
--- a/source/RobotAPI/libraries/robot_name_service/core/Robot.cpp
+++ b/source/RobotAPI/libraries/robot_name_service/core/Robot.cpp
@@ -4,17 +4,17 @@ namespace armarx::robot_name_service::core
{
void
- Hand::fromIce(const dto::Hand& r)
+ TCPInfo::fromIce(const dto::TCPInfo& r)
{
name = r.name;
ft_name = r.ft_name;
handUnitInterface = r.handUnitInterface;
}
- dto::Hand
- Hand::toIce() const
+ dto::TCPInfo
+ TCPInfo::toIce() const
{
- dto::Hand r;
+ dto::TCPInfo r;
r.name = name;
r.ft_name = ft_name;
r.handUnitInterface = handUnitInterface;
@@ -22,57 +22,53 @@ namespace armarx::robot_name_service::core
}
void
- Arm::fromIce(const dto::Arm& r)
+ ArmInfo::fromIce(const dto::ArmInfo& r)
{
kinematicChainName = r.kinematicChainName;
- hand.fromIce(r.hand);
+ hand.fromIce(r.tcpInfo);
}
- dto::Arm
- Arm::toIce() const
+ dto::ArmInfo
+ ArmInfo::toIce() const
{
- dto::Arm r;
+ dto::ArmInfo r;
r.kinematicChainName = kinematicChainName;
- r.hand = hand.toIce();
+ r.tcpInfo = hand.toIce();
return r;
}
void
- Robot::fromIce(const dto::Robot& r)
+ RobotInfo::fromIce(const dto::RobotInfo& r)
{
name = r.name;
xmlPackagePath = r.xmlPackagePath;
memoryNameSystem = armarx::armem::client::MemoryNameSystem(r.memoryNameSystem);
skillManager = r.skillManager;
- arms.clear();
- for (const auto& [k, a] : r.arms)
+ armInfos.clear();
+ for (const auto& [k, a] : r.armInfos)
{
- arms[k].fromIce(a);
+ armInfos[k].fromIce(a);
}
- kinematicUnitInterface = r.kinematicUnitInterface;
- platformUnitInterface = r.platformUnitInterface;
- localizationUnitInterface = r.localizationUnitInterface;
+ robotUnit = r.robotUnit;
}
- dto::Robot
- Robot::toIce() const
+ dto::RobotInfo
+ RobotInfo::toIce() const
{
- dto::Robot r;
+ dto::RobotInfo r;
r.name = name;
r.xmlPackagePath = xmlPackagePath;
r.memoryNameSystem = memoryNameSystem.getMemoryNameSystem();
r.skillManager = skillManager;
- for (const auto& [k, a] : arms)
+ for (const auto& [k, a] : armInfos)
{
- r.arms[k] = a.toIce();
+ r.armInfos[k] = a.toIce();
}
- r.kinematicUnitInterface = kinematicUnitInterface;
- r.platformUnitInterface = platformUnitInterface;
- r.localizationUnitInterface = localizationUnitInterface;
+ r.robotUnit = robotUnit;
return r;
}
diff --git a/source/RobotAPI/libraries/robot_name_service/core/Robot.h b/source/RobotAPI/libraries/robot_name_service/core/Robot.h
index 243d94c1b2a7f72d8f050ee770d06611da5a83cf..4dedb60a99ccb82519bf810363457fb5bb723e52 100644
--- a/source/RobotAPI/libraries/robot_name_service/core/Robot.h
+++ b/source/RobotAPI/libraries/robot_name_service/core/Robot.h
@@ -10,11 +10,11 @@
namespace armarx::robot_name_service::core
{
- class Hand
+ class TCPInfo
{
public:
- void fromIce(const armarx::robot_name_service::dto::Hand& r);
- armarx::robot_name_service::dto::Hand toIce() const;
+ void fromIce(const armarx::robot_name_service::dto::TCPInfo& r);
+ armarx::robot_name_service::dto::TCPInfo toIce() const;
public:
std::string name;
@@ -22,22 +22,22 @@ namespace armarx::robot_name_service::core
armarx::HandUnitInterfacePrx handUnitInterface;
};
- class Arm
+ class ArmInfo
{
public:
- void fromIce(const armarx::robot_name_service::dto::Arm& r);
- armarx::robot_name_service::dto::Arm toIce() const;
+ void fromIce(const armarx::robot_name_service::dto::ArmInfo& r);
+ armarx::robot_name_service::dto::ArmInfo toIce() const;
public:
std::string kinematicChainName;
- Hand hand;
+ TCPInfo hand;
};
- class Robot
+ class RobotInfo
{
public:
- void fromIce(const armarx::robot_name_service::dto::Robot& r);
- armarx::robot_name_service::dto::Robot toIce() const;
+ void fromIce(const armarx::robot_name_service::dto::RobotInfo& r);
+ armarx::robot_name_service::dto::RobotInfo toIce() const;
public:
// header
@@ -49,11 +49,9 @@ namespace armarx::robot_name_service::core
armarx::skills::manager::dti::SkillManagerInterfacePrx skillManager;
// kinematic stuff
- std::map<std::string, Arm> arms;
+ std::map<std::string, ArmInfo> armInfos;
// units
- armarx::KinematicUnitInterfacePrx kinematicUnitInterface;
- armarx::PlatformUnitInterfacePrx platformUnitInterface;
- armarx::LocalizationUnitInterfacePrx localizationUnitInterface;
+ armarx::RobotUnitInterfacePrx robotUnit;
};
} // namespace armarx::robot_name_service::core
diff --git a/source/RobotAPI/libraries/robot_name_service/core/aron/Robot.xml b/source/RobotAPI/libraries/robot_name_service/core/aron/Robot.xml
new file mode 100644
index 0000000000000000000000000000000000000000..5b29a3d3c5bfb61b1ae6112a97972b2a4066aa58
--- /dev/null
+++ b/source/RobotAPI/libraries/robot_name_service/core/aron/Robot.xml
@@ -0,0 +1,40 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <GenerateTypes>
+
+ <Object name='armarx::robot_name_service::arondto::TCPInfo'>
+ <ObjectChild key='name'>
+ <string />
+ </ObjectChild>
+
+ </Object>
+
+ <Object name='armarx::robot_name_service::arondto::TCPInfo'>
+ <ObjectChild key='name'>
+ <string />
+ </ObjectChild>
+
+ <ObjectChild key='ft_name'>
+ <string />
+ </ObjectChild>
+
+ </Object>
+
+ <Object name='armarx::robot_name_service::arondto::ArmInfo'>
+ <ObjectChild key='kinematicChainName'>
+ <string />
+ </ObjectChild>
+
+ </Object>
+
+
+ <Object name='armarx::robot_name_service::arondto::RobotInfo'>
+ <ObjectChild key='name'>
+ <string />
+ </ObjectChild>
+
+ </Object>
+
+
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/robot_name_service/core/aron_conversions.cpp b/source/RobotAPI/libraries/robot_name_service/core/aron_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..57d53663682f641335c663b5483e626388d01d12
--- /dev/null
+++ b/source/RobotAPI/libraries/robot_name_service/core/aron_conversions.cpp
@@ -0,0 +1,6 @@
+#include "aron_conversions.h"
+
+namespace armarx::robot_name_service::core
+{
+
+} // namespace armarx::robot_name_service::core
diff --git a/source/RobotAPI/libraries/robot_name_service/core/aron_conversions.h b/source/RobotAPI/libraries/robot_name_service/core/aron_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..4019e276eac94db6072c1338b74c3176d12734e8
--- /dev/null
+++ b/source/RobotAPI/libraries/robot_name_service/core/aron_conversions.h
@@ -0,0 +1,14 @@
+#pragma once
+
+#include <map>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <RobotAPI/interface/robot_name_service/RobotNameServiceInterface.h>
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+
+namespace armarx::robot_name_service::core
+{
+
+} // namespace armarx::robot_name_service::core
diff --git a/source/RobotAPI/libraries/robot_name_service/server/CMakeLists.txt b/source/RobotAPI/libraries/robot_name_service/server/CMakeLists.txt
index d0c268aa439c501d38505d3fab455bf6b4a9b876..862934ffff91b608fdf651a6b57c4371b4749238 100644
--- a/source/RobotAPI/libraries/robot_name_service/server/CMakeLists.txt
+++ b/source/RobotAPI/libraries/robot_name_service/server/CMakeLists.txt
@@ -18,8 +18,10 @@ armarx_add_library(
SOURCES
plugins.cpp
+ segments/RobotSegment.cpp
HEADERS
plugins.h
+ segments/RobotSegment.h
)
add_library(RobotAPI::robot_name_service_server ALIAS robot_name_service_server)
diff --git a/source/RobotAPI/libraries/robot_name_service/server/segments/RobotSegment.cpp b/source/RobotAPI/libraries/robot_name_service/server/segments/RobotSegment.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9e67bbb2526b9876818b342c0d55991ad7e473b6
--- /dev/null
+++ b/source/RobotAPI/libraries/robot_name_service/server/segments/RobotSegment.cpp
@@ -0,0 +1,6 @@
+#include "RobotSegment.h"
+
+namespace armarx::robot_name_service::server::segment
+{
+
+} // namespace armarx::robot_name_service::server::segment
diff --git a/source/RobotAPI/libraries/robot_name_service/server/segments/RobotSegment.h b/source/RobotAPI/libraries/robot_name_service/server/segments/RobotSegment.h
new file mode 100644
index 0000000000000000000000000000000000000000..7e9d006f6b7ca1d736af0486ed09daa1fe98f3c4
--- /dev/null
+++ b/source/RobotAPI/libraries/robot_name_service/server/segments/RobotSegment.h
@@ -0,0 +1,17 @@
+#pragma once
+
+#include <map>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+#include <RobotAPI/libraries/armem/server/segment/SpecializedCoreSegment.h>
+
+namespace armarx::robot_name_service::server::segment
+{
+ class RobotInfoCoreSegment : public armarx::armem::server::segment::SpecializedCoreSegment
+ {
+ public:
+ };
+} // namespace armarx::robot_name_service::server::segment
diff --git a/source/RobotAPI/libraries/skills/core/SkillExecutionID.cpp b/source/RobotAPI/libraries/skills/core/SkillExecutionID.cpp
index d2f305c1a5e42b5dc28da3285ef46dcbe154b3e6..2bf5f3235ba19f60f4475e9fe3d126fbcfec1db9 100644
--- a/source/RobotAPI/libraries/skills/core/SkillExecutionID.cpp
+++ b/source/RobotAPI/libraries/skills/core/SkillExecutionID.cpp
@@ -4,6 +4,7 @@ namespace armarx
{
namespace skills
{
+
skills::manager::dto::SkillExecutionID
SkillExecutionID::toManagerIce() const
{
@@ -29,7 +30,9 @@ namespace armarx
{
armarx::core::time::DateTime t;
armarx::core::time::fromIce(i.executionStartedTime, t);
- return {skills::SkillID::FromIce(i.skillId), i.executorName, t};
+ return {.skillId = skills::SkillID::FromIce(i.skillId),
+ .executorName = i.executorName,
+ .executionStartedTime = t};
}
SkillExecutionID
@@ -40,5 +43,13 @@ namespace armarx
armarx::core::time::fromIce(i.executionStartedTime, t);
return {skills::SkillID::FromIce(i.skillId, providerName), i.executorName, t};
}
+
+ std::string
+ SkillExecutionID::toString() const
+ {
+ return skillId.toString() + ENTER_SEPARATOR + executorName + SEPARATOR +
+ executionStartedTime.toDateTimeString() + EXIT_SEPARATOR;
+ }
+
} // namespace skills
} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/SkillExecutionID.h b/source/RobotAPI/libraries/skills/core/SkillExecutionID.h
index 6c4975388e716c5b0491518a356add0b15cbbf5e..53fc20a1013f5924c77159580899ae824bd1ad86 100644
--- a/source/RobotAPI/libraries/skills/core/SkillExecutionID.h
+++ b/source/RobotAPI/libraries/skills/core/SkillExecutionID.h
@@ -18,6 +18,15 @@ namespace armarx
{
struct SkillExecutionID
{
+ SkillID skillId;
+ std::string executorName;
+ armarx::core::time::DateTime executionStartedTime;
+
+ static const constexpr char* UNKNOWN = "UNKNOWN";
+ static const constexpr char* ENTER_SEPARATOR = "[";
+ static const constexpr char* EXIT_SEPARATOR = "]";
+ static const constexpr char* SEPARATOR = "@";
+
bool
operator==(const SkillExecutionID& other) const
{
@@ -36,13 +45,6 @@ namespace armarx
return this->toString() <= other.toString();
}
- std::string
- toString() const
- {
- return skillId.toString() + " requested by " + executorName + " at " +
- executionStartedTime.toDateTimeString();
- }
-
skills::manager::dto::SkillExecutionID toManagerIce() const;
skills::provider::dto::SkillExecutionID toProviderIce() const;
@@ -52,10 +54,7 @@ namespace armarx
static SkillExecutionID FromIce(const skills::provider::dto::SkillExecutionID&,
const std::optional<skills::ProviderID>& providerName);
-
- SkillID skillId;
- std::string executorName;
- armarx::core::time::DateTime executionStartedTime;
+ std::string toString() const;
};
} // namespace skills
diff --git a/source/RobotAPI/libraries/skills/core/SkillID.h b/source/RobotAPI/libraries/skills/core/SkillID.h
index e611a82828442ad378dde77d4d90edf477b5e5f1..91494c78c331272e85babe5052ccd2369e29833e 100644
--- a/source/RobotAPI/libraries/skills/core/SkillID.h
+++ b/source/RobotAPI/libraries/skills/core/SkillID.h
@@ -18,6 +18,7 @@ namespace armarx
{
public:
static const constexpr char* NAME_SEPARATOR = "/";
+ static const constexpr char* UNKNOWN = "UNKNOWN";
bool operator==(const SkillID& other) const;
bool operator!=(const SkillID& other) const;
diff --git a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
index 60db0fa913274594078d59c3f7a930b86e1423be..55f9d80c01a78154aa95780e2cc6f549be94f843 100644
--- a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
+++ b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
@@ -137,7 +137,8 @@ namespace armarx
auto terminated = TerminatedSkillStatusUpdate{
{.executionId = statusUpdate.executionId,
.parameters = statusUpdate.parameters,
- .callbackInterface = statusUpdate.callbackInterface}};
+ .callbackInterface = statusUpdate.callbackInterface,
+ .result = statusUpdate.result}};
terminated.status = TerminatedSkillStatus::Aborted;
return terminated;
};
@@ -151,7 +152,8 @@ namespace armarx
auto terminated = TerminatedSkillStatusUpdate{
{.executionId = statusUpdate.executionId,
.parameters = statusUpdate.parameters,
- .callbackInterface = statusUpdate.callbackInterface}};
+ .callbackInterface = statusUpdate.callbackInterface,
+ .result = statusUpdate.result}};
terminated.status = TerminatedSkillStatus::Failed;
return terminated;
};
@@ -273,12 +275,18 @@ namespace armarx
try
{
mainRet = skill->mainOfSkill();
- if (mainRet.status != TerminatedSkillStatus::Succeeded)
+ if (mainRet.status == TerminatedSkillStatus::Failed)
{
- std::string message = "SkillError 501: The main method of skill '" + skillName +
- "' did not succeed.";
+ std::string message =
+ "SkillError 501: The main method of skill '" + skillName + "' did fail.";
return exitAndMakeFailedResult(message);
}
+ else if (mainRet.status == TerminatedSkillStatus::Aborted)
+ {
+ std::string message =
+ "SkillError 501: The main method of skill '" + skillName + "' got aborted.";
+ return exitAndMakeAbortedResult(message);
+ }
}
catch (const error::SkillAbortedException& ex)
{