diff --git a/scenarios/deprecated/MotionControlTest/CMakeLists.txt b/scenarios/deprecated/MotionControlTest/CMakeLists.txt
index 7fb1259c8a86a3f286ca088308a014a3f5cc5eda..c7040306f02cd266a24a7ef0d92fb3ae989199b4 100644
--- a/scenarios/deprecated/MotionControlTest/CMakeLists.txt
+++ b/scenarios/deprecated/MotionControlTest/CMakeLists.txt
@@ -11,7 +11,7 @@ set(SCENARIO_COMPONENTS
)
# optional 3rd parameter: "path/to/global/config.cfg"
-armarx_scenario("MotionControlTest" "${SCENARIO_COMPONENTS}" "./configs/Global.cfg")
+armarx_scenario("MotionControlTest" "${SCENARIO_COMPONENTS}" "./config/Global.cfg")
#set(SCENARIO_CONFIGS
# config/ComponentName.optionalString.cfg
diff --git a/source/RobotAPI/gui_plugins/KinematicUnitPlugin/CMakeLists.txt b/source/RobotAPI/gui_plugins/KinematicUnitPlugin/CMakeLists.txt
index e85a9c83c06b4eba4164d778a56441705243146c..f0374fc8801a9572e04daf1ff44089def04651ae 100644
--- a/source/RobotAPI/gui_plugins/KinematicUnitPlugin/CMakeLists.txt
+++ b/source/RobotAPI/gui_plugins/KinematicUnitPlugin/CMakeLists.txt
@@ -36,7 +36,7 @@ set(GUI_UIS
KinematicUnitConfigDialog.ui
)
-set(COMPONENT_LIBS RobotAPIUnits ${Simox_LIBRARIES})
+set(COMPONENT_LIBS RobotAPIUnits DebugDrawer ${Simox_LIBRARIES})
set(LIB_VERSION 0.1.0)
set(LIB_SOVERSION 0)
diff --git a/source/RobotAPI/gui_plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp b/source/RobotAPI/gui_plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
index edab6f1513f911d4aeb5ed8ea732c58455f73251..b3dff5a115dd63003a61fa9253ae69eed6dda64a 100644
--- a/source/RobotAPI/gui_plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui_plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
@@ -3,6 +3,7 @@
#include <RobotAPI/gui_plugins/KinematicUnitPlugin/ui_KinematicUnitConfigDialog.h>
#include <Core/core/system/ArmarXDataPath.h>
#include <Core/core/ArmarXObjectScheduler.h>
+#include <Core/core/ArmarXManager.h>
// Qt headers
#include <Qt>
@@ -48,8 +49,10 @@ KinematicUnitWidgetController::KinematicUnitWidgetController() :
kinematicUnitNode(nullptr),
selectedControlMode(ePositionControl)
{
- // init gui
+ rootVisu = NULL;
+ debugLayerVisu = NULL;
+ // init gui
ui.setupUi(getWidget());
getWidget()->setEnabled(false);
}
@@ -96,12 +99,34 @@ void KinematicUnitWidgetController::onInitComponent()
std::cout << "returning" << std::endl;
return;
}
+
+ rootVisu = new SoSeparator();
+ rootVisu->ref();
+
kinematicUnitVisualization = getCoinVisualization(robot);
kinematicUnitNode = kinematicUnitVisualization->getCoinVisualization();
- kinematicUnitNode->ref();
+ rootVisu->addChild(kinematicUnitNode);
robotNodeSet = getRobotNodeSet(robot, robotNodeSetName);
+ // create the debugdrawer component
+ std::string debugDrawerComponentName = "KinemticUnitGUIDebugDrawer_" + getName();
+ ARMARX_INFO << "Creating component " << debugDrawerComponentName;
+ debugDrawer = Component::create<DebugDrawerComponent>(properties,debugDrawerComponentName);
+ if (mutex3D)
+ debugDrawer->setMutex(mutex3D);
+ else
+ ARMARX_ERROR << " No 3d mutex available...";
+ ArmarXManagerPtr m = getArmarXManager();
+ m->addObject(debugDrawer);
+
+ {
+ boost::recursive_mutex::scoped_lock lock(*mutex3D);
+ debugLayerVisu = new SoSeparator();
+ debugLayerVisu->ref();
+ debugLayerVisu->addChild(debugDrawer->getVisualization());
+ rootVisu->addChild(debugLayerVisu);
+ }
string widgetLabel = "KinematicUnit: " + kinematicUnitName;
@@ -109,6 +134,7 @@ void KinematicUnitWidgetController::onInitComponent()
initGUIComboBox(robotNodeSet); // init the pull down menu (QT: ComboBox)
initGUIJointListTable(robotNodeSet);
+ /*
connect(this, SIGNAL(jointAnglesReported()), this, SLOT(updateJointAnglesTable()), Qt::QueuedConnection);
connect(this, SIGNAL(jointVelocitiesReported()), this, SLOT(updateJointVelocitiesTable()), Qt::QueuedConnection);
connect(this, SIGNAL(jointTorquesReported()), this, SLOT(updateJointTorquesTable()), Qt::QueuedConnection);
@@ -116,7 +142,7 @@ void KinematicUnitWidgetController::onInitComponent()
connect(this, SIGNAL(jointControlModesReported()), this, SLOT(updateControlModesTable()), Qt::QueuedConnection);
connect(this, SIGNAL(jointMotorTemperaturesReported()), this, SLOT(updateJointMotorTemperaturesTable()), Qt::QueuedConnection);
connect(this, SIGNAL(jointStatusesReported()), this, SLOT(updateJointStatusesTable()), Qt::QueuedConnection);
-
+*/
usingTopic(topicName);
usingProxy(kinematicUnitName);
@@ -141,8 +167,32 @@ void KinematicUnitWidgetController::onExitComponent()
{
enableMainWidgetAsync(false);
- if (kinematicUnitNode)
- kinematicUnitNode->unref();
+ {
+ boost::recursive_mutex::scoped_lock lock(*mutex3D);
+
+ if (debugLayerVisu)
+ {
+ debugLayerVisu->removeAllChildren();
+ debugLayerVisu->unref();
+ debugLayerVisu = NULL;
+ }
+
+ if (rootVisu)
+ {
+ rootVisu->removeAllChildren();
+ rootVisu->unref();
+ rootVisu = NULL;
+ }
+ }
+
+
+ if (debugDrawer)
+ {
+ ARMARX_INFO << "Removing DebugDrawer component...";
+ debugDrawer->getObjectScheduler()->terminate();
+ ARMARX_INFO << "Removing DebugDrawer component...done";
+ }
+
}
QPointer<QDialog> KinematicUnitWidgetController::getConfigDialog(QWidget* parent)
@@ -192,6 +242,20 @@ void KinematicUnitWidgetController::saveSettings(QSettings *settings)
}
+void KinematicUnitWidgetController::showVisuLayers(bool show)
+{
+ if (debugDrawer)
+ {
+ if(show)
+ {
+ debugDrawer->enableAllLayers();
+ }
+ else
+ {
+ debugDrawer->disableAllLayers();
+ }
+ }
+}
void KinematicUnitWidgetController::updateGuiElements()
{
@@ -201,49 +265,13 @@ void KinematicUnitWidgetController::updateGuiElements()
void KinematicUnitWidgetController::modelUpdateCB()
{
-
- /*
- if (data->getDirtyFlag())
- {
- FloatVector angles;
- FloatVector velocities;
- Armar3ControlMode m;
- Armar3ComponentState s;
-
- if (data->getDirtyFlagAngles())
- {
- data->getDataAngles(angles);
- reportKinematicUnitJointAnglesDegree(angles);
- }
- if (data->getDirtyFlagComponentState())
- {
- data->getDataComponentState(s);
- reportKinematicUnitStateChanged(s);
- }
- if (data->getDirtyFlagControlMode())
- {
- data->getDataControlMode(m);if (!currentNode)
- return;
- float ticks = (float)(ui.horizontalSliderKinematicUnitPos->maximum() - ui.horizontalSliderKinematicUnitPos->minimum()+1);
- float value = (float)ui.horizontalSliderKinematicUnitPos->value();
- float perc = (value - (float)ui.horizontalSliderKinematicUnitPos->minimum()) / ticks;
- float lo = currentNode->getJointLimitLo();
- float hi = currentNode->getJointLimitHi();
- float result = lo + (hi-lo)*perc;
-
- if (KinematicUnitPrx)
- KinematicUnitPrx->begin_setJoint(currentNodeType,result*180.0f/(float)M_PI);
- reportKinematicUnitControlModeChanged(m);
- }
- }
-*/
}
SoNode *KinematicUnitWidgetController::getScene()
{
- if(kinematicUnitNode)
- std::cout << "returning scene=" << kinematicUnitNode->getName() << std::endl;
- return kinematicUnitNode;
+ //if(kinematicUnitNode)
+ // std::cout << "returning scene=" << kinematicUnitNode->getName() << std::endl;
+ return rootVisu;
}
@@ -262,12 +290,22 @@ void KinematicUnitWidgetController::connectSlots()
connect(ui.radioButtonVelocityControl, SIGNAL(clicked(bool)), this, SLOT(setControlModeVelocity()));
connect(ui.radioButtonTorqueControl, SIGNAL(clicked(bool)), this, SLOT(setControlModeTorque()));
+ connect(ui.showDebugLayer, SIGNAL(toggled(bool)), this, SLOT(showVisuLayers(bool)), Qt::QueuedConnection);
+ /* connect(this, SIGNAL(jointAnglesReported()), this, SLOT(updateJointAnglesTable()), Qt::QueuedConnection);
+ connect(this, SIGNAL(jointVelocitiesReported()), this, SLOT(updateJointVelocitiesTable()), Qt::QueuedConnection);
+ connect(this, SIGNAL(jointTorquesReported()), this, SLOT(updateJointTorquesTable()), Qt::QueuedConnection);
+ connect(this, SIGNAL(jointControlModesReported()), this, SLOT(updateControlModesTable()), Qt::QueuedConnection);
+ connect(this, SIGNAL(jointMotorTemperaturesReported()), this, SLOT(updateJointMotorTemperaturesTable()), Qt::QueuedConnection);
+ */
connect(this, SIGNAL(jointAnglesReported()), this, SLOT(updateJointAnglesTable()), Qt::QueuedConnection);
connect(this, SIGNAL(jointVelocitiesReported()), this, SLOT(updateJointVelocitiesTable()), Qt::QueuedConnection);
connect(this, SIGNAL(jointTorquesReported()), this, SLOT(updateJointTorquesTable()), Qt::QueuedConnection);
+ connect(this, SIGNAL(jointCurrentsReported()), this, SLOT(updateJointCurrentsTable()), Qt::QueuedConnection);
connect(this, SIGNAL(jointControlModesReported()), this, SLOT(updateControlModesTable()), Qt::QueuedConnection);
connect(this, SIGNAL(jointMotorTemperaturesReported()), this, SLOT(updateJointMotorTemperaturesTable()), Qt::QueuedConnection);
+ connect(this, SIGNAL(jointStatusesReported()), this, SLOT(updateJointStatusesTable()), Qt::QueuedConnection);
+
connect(ui.tableJointList, SIGNAL(cellDoubleClicked(int,int)), this, SLOT(selectJointFromTableWidget(int,int)), Qt::QueuedConnection);
diff --git a/source/RobotAPI/gui_plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h b/source/RobotAPI/gui_plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h
index 97e19ecf147297bec099ed8d04dc0177e988ae13..6a3f4f24a682879aa996731c1e8651933272fc9f 100644
--- a/source/RobotAPI/gui_plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h
+++ b/source/RobotAPI/gui_plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h
@@ -28,6 +28,7 @@
/* ArmarX headers */
#include <RobotAPI/gui_plugins/KinematicUnitPlugin/ui_kinematicunitguiplugin.h>
#include <RobotAPI/interface/units/KinematicUnitInterface.h>
+#include <RobotAPI/components/DebugDrawer/DebugDrawerComponent.h>
#include <Core/core/Component.h>
#include <Gui/ArmarXGuiLib/ArmarXGuiPlugin.h>
#include <Gui/ArmarXGuiLib/ArmarXComponentWidgetController.h>
@@ -185,6 +186,10 @@ namespace armarx
CoinVisualizationPtr kinematicUnitVisualization;
SoNode* kinematicUnitNode;
+ SoSeparator* rootVisu;
+
+ SoSeparator* debugLayerVisu;
+ armarx::DebugDrawerComponentPtr debugDrawer;
// interface implementation
void reportControlModeChanged(const NameControlModeMap& jointModes, bool aValueChanged, const Ice::Current& c);
@@ -199,6 +204,8 @@ namespace armarx
void updateModel();
+ protected slots:
+ void showVisuLayers(bool show);
private:
boost::recursive_mutex mutexNodeSet;
diff --git a/source/RobotAPI/gui_plugins/KinematicUnitPlugin/kinematicunitguiplugin.ui b/source/RobotAPI/gui_plugins/KinematicUnitPlugin/kinematicunitguiplugin.ui
index 64e2c360bb14990f9f695709ddf31ccb134adee6..af40fc2bb6f91ced77b921c54ba572fc2593b5ee 100644
--- a/source/RobotAPI/gui_plugins/KinematicUnitPlugin/kinematicunitguiplugin.ui
+++ b/source/RobotAPI/gui_plugins/KinematicUnitPlugin/kinematicunitguiplugin.ui
@@ -6,7 +6,7 @@
<rect>
<x>0</x>
<y>0</y>
- <width>810</width>
+ <width>900</width>
<height>547</height>
</rect>
</property>
@@ -60,25 +60,6 @@
</property>
</widget>
</item>
- <item row="4" column="0">
- <widget class="QLCDNumber" name="lcdNumberKinematicUnitJointValue">
- <property name="enabled">
- <bool>true</bool>
- </property>
- <property name="maximumSize">
- <size>
- <width>80</width>
- <height>100</height>
- </size>
- </property>
- <property name="toolTip">
- <string>Value in percent between low and high limit of joint</string>
- </property>
- <property name="smallDecimalPoint">
- <bool>false</bool>
- </property>
- </widget>
- </item>
<item row="2" column="1" colspan="2">
<widget class="QComboBox" name="nodeListComboBox">
<property name="enabled">
@@ -92,28 +73,6 @@
</property>
</widget>
</item>
- <item row="4" column="1" colspan="2">
- <widget class="QSlider" name="horizontalSliderKinematicUnitPos">
- <property name="enabled">
- <bool>true</bool>
- </property>
- <property name="minimum">
- <number>-100</number>
- </property>
- <property name="maximum">
- <number>100</number>
- </property>
- <property name="orientation">
- <enum>Qt::Horizontal</enum>
- </property>
- <property name="tickPosition">
- <enum>QSlider::TicksBelow</enum>
- </property>
- <property name="tickInterval">
- <number>0</number>
- </property>
- </widget>
- </item>
<item row="3" column="1" colspan="2">
<layout class="QHBoxLayout" name="horizontalLayout">
<item>
@@ -212,6 +171,60 @@
</item>
</layout>
</item>
+ <item row="4" column="1">
+ <layout class="QHBoxLayout" name="horizontalLayout_4"/>
+ </item>
+ <item row="5" column="2">
+ <widget class="QCheckBox" name="showDebugLayer">
+ <property name="text">
+ <string>debug layer</string>
+ </property>
+ <property name="checked">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="1">
+ <widget class="QSlider" name="horizontalSliderKinematicUnitPos">
+ <property name="enabled">
+ <bool>true</bool>
+ </property>
+ <property name="minimum">
+ <number>-100</number>
+ </property>
+ <property name="maximum">
+ <number>100</number>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="tickPosition">
+ <enum>QSlider::TicksBelow</enum>
+ </property>
+ <property name="tickInterval">
+ <number>0</number>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="0">
+ <widget class="QLCDNumber" name="lcdNumberKinematicUnitJointValue">
+ <property name="enabled">
+ <bool>true</bool>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>80</width>
+ <height>100</height>
+ </size>
+ </property>
+ <property name="toolTip">
+ <string>Value in percent between low and high limit of joint</string>
+ </property>
+ <property name="smallDecimalPoint">
+ <bool>false</bool>
+ </property>
+ </widget>
+ </item>
</layout>
</item>
<item>