diff --git a/source/RobotAPI/components/ArViz/Client/Elements.h b/source/RobotAPI/components/ArViz/Client/Elements.h
index af08e951c9c20b0bd05baad85b14679627cc2e7e..7583bff7d56435ce6daa6616715ddb6e76e9ea03 100644
--- a/source/RobotAPI/components/ArViz/Client/Elements.h
+++ b/source/RobotAPI/components/ArViz/Client/Elements.h
@@ -8,6 +8,7 @@
#include "elements/Color.h"
#include "elements/ElementOps.h"
#include "elements/Mesh.h"
+#include "elements/Path.h"
#include "elements/PointCloud.h"
#include "elements/Robot.h"
//#include "elements/RobotHand.h" // Not included by default (exposes additional headers).
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationPolygon.h b/source/RobotAPI/components/ArViz/Coin/VisualizationPolygon.h
index 3ace79284308975308125cfc945cc1b2059d994d..7ea618b063125dce6507245847688ffe29f69e3d 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationPolygon.h
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationPolygon.h
@@ -4,11 +4,14 @@
#include <RobotAPI/interface/ArViz/Elements.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
#include <Inventor/nodes/SoCoordinate3.h>
#include <Inventor/nodes/SoDrawStyle.h>
#include <Inventor/nodes/SoFaceSet.h>
#include <Inventor/nodes/SoLineSet.h>
+
namespace armarx::viz::coin
{
struct VisualizationPolygon : TypedElementVisualization<SoSeparator>
@@ -41,6 +44,16 @@ namespace armarx::viz::coin
bool update(ElementType const& element)
{
+ if (element.points.size() < 3)
+ {
+ // A polygon with < 3 points is invalid.
+ ARMARX_WARNING << deactivateSpam(1000)
+ << "Ignoring polygon '" << element.id << "' "
+ << "with " << element.points.size() << " points "
+ << "(a polygon requires >= 3 points).";
+ return true;
+ }
+
int pointSize = (int)element.points.size();
int i = 0;
diff --git a/source/RobotAPI/components/DebugDrawerToArViz/DebugDrawerToArViz.cpp b/source/RobotAPI/components/DebugDrawerToArViz/DebugDrawerToArViz.cpp
index f3417d30644acd9540aa8c1f888a77c49290a225..08b6827e5c9ce6d334d7cadeb6ad9a074a15a02b 100644
--- a/source/RobotAPI/components/DebugDrawerToArViz/DebugDrawerToArViz.cpp
+++ b/source/RobotAPI/components/DebugDrawerToArViz/DebugDrawerToArViz.cpp
@@ -144,8 +144,8 @@ namespace armarx
{
return;
}
- setAndCommit(layer, viz::Polygon(name).addPoint(toEigen(globalPosition1)).addPoint(toEigen(globalPosition2))
- .lineColor(toViz(color)).lineWidth(lineWidth).color(simox::Color::black(0)));
+ setAndCommit(layer, viz::Path(name).addPoint(toEigen(globalPosition1)).addPoint(toEigen(globalPosition2))
+ .color(toViz(color)).width(lineWidth).color(simox::Color::black(0)));
}
void DebugDrawerToArViz::setLineSetVisu(const std::string& layerName, const std::string& name, const DebugDrawerLineSet& lineSet, const Ice::Current&)
diff --git a/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.cpp b/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.cpp
index b8ab1db644a6228974453e648271c5b7975283fe..1db9be387b047ca5f97078338d01a68cb53f72c9 100644
--- a/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.cpp
+++ b/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.cpp
@@ -57,8 +57,8 @@ namespace armarx::armem
std::lock_guard l(updateMutex);
if (updateChanged)
{
- // if the update is not changed it has probably been committed already
- ARMARX_WARNING << deactivateSpam() << "Try to send data to robotStateMemory but data has not changed.";
+ // If the update is not changed it has probably been committed already.
+ ARMARX_INFO << deactivateSpam() << "Try to send data to robotStateMemory but data has not changed.";
return;
}
diff --git a/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.cpp
index 68a21112e18da9b40ea282a899b5802d003279eb..132f0b639df824fe91a3b6bd9f25427f50950d7d 100644
--- a/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.cpp
@@ -47,6 +47,7 @@ PlatformUnitGuiPlugin::PlatformUnitGuiPlugin()
addWidget<PlatformUnitWidget>();
}
+
PlatformUnitWidget::PlatformUnitWidget() :
platformUnitProxyName("PlatformUnit"), // overwritten in loadSettings() anyway?
platformName("Platform"),
@@ -79,7 +80,6 @@ PlatformUnitWidget::PlatformUnitWidget() :
connect(getWidget().data(), SIGNAL(commandKeyPressed(int)), this, SLOT(controlPlatformWithKeyboard(int)));
connect(getWidget().data(), SIGNAL(commandKeyReleased(int)), this, SLOT(stopPlatformWithKeyboard(int)));
-
}
@@ -91,23 +91,25 @@ void PlatformUnitWidget::onInitComponent()
}
+
void PlatformUnitWidget::onConnectComponent()
{
platformUnitProxy = getProxy<PlatformUnitInterfacePrx>(platformUnitProxyName);
connectSlots();
-
}
+
void PlatformUnitWidget::onDisconnectComponent()
{
stopControlTimer();
-
}
+
void PlatformUnitWidget::onExitComponent()
{
}
+
QPointer<QDialog> PlatformUnitWidget::getConfigDialog(QWidget* parent)
{
if (!dialog)
@@ -119,6 +121,7 @@ QPointer<QDialog> PlatformUnitWidget::getConfigDialog(QWidget* parent)
return qobject_cast<PlatformUnitConfigDialog*>(dialog);
}
+
void PlatformUnitWidget::configured()
{
platformUnitProxyName = dialog->finder->getSelectedProxyName().toStdString();
@@ -132,6 +135,7 @@ void PlatformUnitWidget::loadSettings(QSettings* settings)
platformName = settings->value("platformName", QString::fromStdString(platformName)).toString().toStdString();
}
+
void PlatformUnitWidget::saveSettings(QSettings* settings)
{
settings->setValue("platformUnitProxyName", QString::fromStdString(platformUnitProxyName));
@@ -153,11 +157,9 @@ void PlatformUnitWidget::connectSlots()
connect(rotaCtrl, SIGNAL(released()), this, SLOT(stopPlatform()), Qt::UniqueConnection);
connect(rotaCtrl, SIGNAL(released()), this, SLOT(stopControlTimer()), Qt::UniqueConnection);
connect(ui.buttonStopPlatform, SIGNAL(pressed()), this, SLOT(stopPlatform()), Qt::UniqueConnection);
-
-
-
}
+
void PlatformUnitWidget::moveTo()
{
ARMARX_LOG << "Moving Platform";
@@ -169,6 +171,7 @@ void PlatformUnitWidget::moveTo()
platformUnitProxy->moveTo(positionX, positionY, rotation, posAcc, rotAcc);
}
+
void PlatformUnitWidget::setNewPlatformPoseLabels(float x, float y, float alpha)
{
ui.labelCurrentPositionX->setText(QString::number(x));
@@ -177,6 +180,7 @@ void PlatformUnitWidget::setNewPlatformPoseLabels(float x, float y, float alpha)
}
+
void PlatformUnitWidget::setNewTargetPoseLabels(float x, float y, float alpha)
{
ui.editTargetPositionX->setText(QString::number(x));
@@ -184,13 +188,13 @@ void PlatformUnitWidget::setNewTargetPoseLabels(float x, float y, float alpha)
ui.editTargetRotation->setText(QString::number(alpha));
}
+
void PlatformUnitWidget::startControlTimer()
{
-
- ctrlEvaluationTimer.start(CONTROL_TICK_RATE);//tickrate in ms
-
+ ctrlEvaluationTimer.start(CONTROL_TICK_RATE); //tickrate in ms
}
+
void PlatformUnitWidget::stopControlTimer()
{
ctrlEvaluationTimer.stop();
@@ -198,6 +202,7 @@ void PlatformUnitWidget::stopControlTimer()
rotaCtrl->setNibble({0, 0});
}
+
void PlatformUnitWidget::reportPlatformPose(PlatformPose const& currentPose, const Ice::Current& c)
{
// moved to qt thread for thread safety
@@ -205,6 +210,7 @@ void PlatformUnitWidget::reportPlatformPose(PlatformPose const& currentPose, con
platformRotation = currentPose.rotationAroundZ;
}
+
void PlatformUnitWidget::reportNewTargetPose(::Ice::Float newPlatformPositionX, ::Ice::Float newPlatformPositionY, ::Ice::Float newPlatformRotation, const Ice::Current& c)
{
// moved to qt thread for thread safety
@@ -219,14 +225,15 @@ void PlatformUnitWidget::reportPlatformVelocity(::Ice::Float currentPlatformVelo
}
+
void PlatformUnitWidget::stopPlatform()
{
platformUnitProxy->stopPlatform();
}
+
void PlatformUnitWidget::controlPlatformWithKeyboard(int key)
{
-
pressedKeys.insert(key);
if (!ctrlEvaluationTimer.isActive())
{
@@ -237,13 +244,11 @@ void PlatformUnitWidget::controlPlatformWithKeyboard(int key)
keyboardVelocityControl();
keyboardVelocityTimer.start();
}
-
}
+
void PlatformUnitWidget::stopPlatformWithKeyboard(int key)
{
-
-
pressedKeys.remove(key);
if (!keyboardVelocityTimer.isActive())
@@ -251,9 +256,9 @@ void PlatformUnitWidget::stopPlatformWithKeyboard(int key)
keyboardVelocityControl();
keyboardVelocityTimer.start();
}
-
}
+
void PlatformUnitWidget::keyboardVelocityControl()
{
if (!pressedKeys.contains(Qt::Key_A) && !pressedKeys.contains(Qt::Key_D))
@@ -307,6 +312,7 @@ void PlatformUnitWidget::keyboardVelocityControl()
break;
}
}
+
currentKeyboardVelocityAlpha = std::max(-1.f, currentKeyboardVelocityAlpha);
currentKeyboardVelocityAlpha = std::min(1.f, currentKeyboardVelocityAlpha);
currentKeyboardVelocityX = std::max(-1.f, currentKeyboardVelocityX);
@@ -317,13 +323,9 @@ void PlatformUnitWidget::keyboardVelocityControl()
float y = sin(acos(currentKeyboardVelocityAlpha));
speedCtrl->setNibble(QPointF(currentKeyboardVelocityX, currentKeyboardVelocityY));
rotaCtrl->setNibble(QPointF(currentKeyboardVelocityAlpha, -y));
-
-
-
}
-
QPointer<QWidget> PlatformUnitWidget::getWidget()
{
if (!__widget)
@@ -335,27 +337,29 @@ QPointer<QWidget> PlatformUnitWidget::getWidget()
}
-
void PlatformUnitWidget::controlTimerTick()
{
float translationFactor = ui.maxTranslationSpeed->value();
float rotationFactor = ui.maxRotationSpeed->value() * -1;
float rotationVel = rotaCtrl->getRotation() / M_PI_2 * rotationFactor;
- ARMARX_INFO << deactivateSpam(0.5) << speedCtrl->getPosition().x()*translationFactor << ", " << speedCtrl->getPosition().y()*translationFactor;
- ARMARX_INFO << deactivateSpam(0.5) << "Rotation Speed: " << (rotationVel);
- platformUnitProxy->move(speedCtrl->getPosition().x()*translationFactor, -1 * speedCtrl->getPosition().y()*translationFactor, rotationVel);
+ ARMARX_INFO << deactivateSpam(0.5)
+ << "Translation speed: (" << speedCtrl->getPosition().x() * translationFactor
+ << ", " << speedCtrl->getPosition().y() * translationFactor << ")"
+ << ", \t rotation speed: " << (rotationVel);
+
+ platformUnitProxy->move(speedCtrl->getPosition().x() * translationFactor,
+ -1 * speedCtrl->getPosition().y() * translationFactor,
+ rotationVel);
if (speedCtrl->getPosition().x() == 0
- && speedCtrl->getPosition().y() == 0
- && rotaCtrl->getRotation() == 0)
+ && speedCtrl->getPosition().y() == 0
+ && rotaCtrl->getRotation() == 0)
{
stopControlTimer();
}
-
}
-
void KeyboardPlatformHookWidget::keyPressEvent(QKeyEvent* event)
{
switch (event->key())
@@ -374,6 +378,7 @@ void KeyboardPlatformHookWidget::keyPressEvent(QKeyEvent* event)
QWidget::keyPressEvent(event);
}
+
void KeyboardPlatformHookWidget::keyReleaseEvent(QKeyEvent* event)
{
switch (event->key())
@@ -392,6 +397,7 @@ void KeyboardPlatformHookWidget::keyReleaseEvent(QKeyEvent* event)
QWidget::keyReleaseEvent(event);
}
+
void armarx::PlatformUnitWidget::reportPlatformOdometryPose(Ice::Float, Ice::Float, Ice::Float, const Ice::Current&)
{
// ignore for now
diff --git a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
index 8e2246c3dd4dc4175c23e0bb01455b380d94e86c..35b4f9668406d586316b00a01b781eabdfd1bcf9 100644
--- a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
@@ -149,7 +149,7 @@ namespace armarx::armem::common::robot_state::localization
}
else
{
- ARMARX_WARNING << deactivateSpam(60) << "Cannot perform tf lookup '" << parentFrame << " -> " << frame << "'";
+ ARMARX_WARNING << deactivateSpam(60) << "Cannot perform tf lookup '" << parentFrame << "' -> '" << frame << "'.";
}
};