Skip to content
Snippets Groups Projects
Commit 47d86206 authored by Mirko Wächter's avatar Mirko Wächter
Browse files

Merge branch 'master' of https://gitlab.com/ArmarX/RobotAPI 

* 'master' of https://gitlab.com/ArmarX/RobotAPI:
  string fields can now be plotted as vertical markers
  kinematic unit gui uses now deg instead of rad
  Added mutex to filters
  fixed plotter deadlock
  switched highlighting of current==0 situation (now checking jointStatus.operation!=eOffline)
parents 2e0114ff 915d4373
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 222 additions and 88 deletions
source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
+ 29
− 10
View file @ 47d86206
......@@ -356,6 +356,8 @@ void KinematicUnitWidgetController::updateGuiElements()
// modelUpdateCB();
}
void KinematicUnitWidgetController::updateKinematicUnitListInDialog() {}
void KinematicUnitWidgetController::modelUpdateCB()
{
}
......@@ -650,8 +652,8 @@ bool KinematicUnitWidgetController::initGUIJointListTable(VirtualRobot::RobotNod
QStringList s;
s << "Joint Name"
<< "Control Mode"
<< "Angle [rad]"
<< "Velocity [rad/s]"
<< "Angle [deg]"
<< "Velocity [deg/s]"
<< "Torque [Nm]"
<< "Current [A]"
<< "Operation"
......@@ -1008,9 +1010,9 @@ void KinematicUnitWidgetController::updateJointAnglesTable()
QModelIndex index = ui.tableJointList->model()->index(i, eTabelColumnAngleProgressbar);
ui.tableJointList->model()->setData(index, cutJitter(currentValue), eJointAngleRole);
ui.tableJointList->model()->setData(index, node->getJointLimitHigh(), eJointHiRole);
ui.tableJointList->model()->setData(index, node->getJointLimitLow(), eJointLoRole);
ui.tableJointList->model()->setData(index, (int)(cutJitter(currentValue * 180.0 / M_PI) * 100) / 100.0f, eJointAngleRole);
ui.tableJointList->model()->setData(index, node->getJointLimitHigh() * 180.0 / M_PI, eJointHiRole);
ui.tableJointList->model()->setData(index, node->getJointLimitLow() * 180.0 / M_PI, eJointLoRole);
}
//update only if values changed
......@@ -1050,7 +1052,7 @@ void KinematicUnitWidgetController::updateJointVelocitiesTable()
const float currentValue = it->second;
dirty_squaresum += currentValue * currentValue;
const QString Text = QString::number(cutJitter(currentValue));
const QString Text = QString::number(cutJitter(currentValue * 180.0 / M_PI), 'g', 2);
newItem = new QTableWidgetItem(Text);
ui.tableJointList->setItem(i, eTabelColumnVelocity, newItem);
}
......@@ -1182,6 +1184,7 @@ void KinematicUnitWidgetController::reportControlModeChanged(const NameControlMo
void KinematicUnitWidgetController::reportJointCurrents(const NameValueMap& jointCurrents, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
{
boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
if (aValueChanged && jointCurrents.size() > 0)
{
jointCurrentHistory.push_back(std::pair<Ice::Long, NameValueMap>(timestamp, jointCurrents));
......@@ -1192,6 +1195,7 @@ void KinematicUnitWidgetController::reportJointCurrents(const NameValueMap& join
jointCurrentHistory.pop_front();
}
if (jointCurrentHistory.size() > 0)
{
emit jointCurrentsReported();
......@@ -1288,7 +1292,7 @@ void KinematicUnitWidgetController::highlightCriticalValues()
const float currentValue = std::fabs(it->second);
smoothedValue = 0.5f*currentValue + 0.5f*smoothedValue;
smoothedValue = 0.5f * currentValue + 0.5f * smoothedValue;
if (currentValue != 0)
{
......@@ -1300,10 +1304,14 @@ void KinematicUnitWidgetController::highlightCriticalValues()
it = reportedJointStatuses.find(rn[i]->getName());
JointStatus currentStatus = it->second;
if (isZero && currentStatus.enabled)
if (isZero)
{
// current value is zero, but joint is enabled
ui.tableJointList->item(i, eTabelColumnCurrent)->setBackground(Qt::darkYellow);
if (currentStatus.operation != eOffline)
{
// current value is zero, but joint is not offline
ui.tableJointList->item(i, eTabelColumnCurrent)->setBackground(Qt::yellow);
}
}
else if (smoothedValue > currentValueMax)
{
......@@ -1389,4 +1397,15 @@ void RangeValueDelegate::paint(QPainter* painter, const QStyleOptionViewItem& op
}
}
KinematicUnitWidgetController::~KinematicUnitWidgetController() {}
QString KinematicUnitWidgetController::getWidgetName() const
{
return "RobotControl.KinematicUnitGUI";
}
QIcon KinematicUnitWidgetController::getWidgetIcon() const
{
return QIcon("://icons/kinematic_icon.svg");
}
Q_EXPORT_PLUGIN2(robotapi_gui_KinematicUnitGuiPlugin, KinematicUnitGuiPlugin)
source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h
+ 4
− 10
View file @ 47d86206
......@@ -138,7 +138,7 @@ namespace armarx
};
KinematicUnitWidgetController();
virtual ~KinematicUnitWidgetController() {}
virtual ~KinematicUnitWidgetController();
// inherited from Component
virtual void onInitComponent();
......@@ -147,14 +147,8 @@ namespace armarx
virtual void onExitComponent();
// inherited of ArmarXWidget
virtual QString getWidgetName() const
{
return "RobotControl.KinematicUnitGUI";
}
virtual QIcon getWidgetIcon() const
{
return QIcon("://icons/kinematic_icon.svg");
}
virtual QString getWidgetName() const;
virtual QIcon getWidgetIcon() const;
QPointer<QDialog> getConfigDialog(QWidget* parent = 0);
virtual void loadSettings(QSettings* settings);
......@@ -205,7 +199,7 @@ namespace armarx
void updateJointCurrentsTable();
void updateJointStatusesTable();
void updateControlModesTable();
void updateKinematicUnitListInDialog() {}
void updateKinematicUnitListInDialog();
protected:
......
source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotter.cpp
+ 159
− 59
View file @ 47d86206
......@@ -32,6 +32,7 @@
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wpedantic"
#include <qwt_plot.h>
#include <qwt_plot_marker.h>
#include <qwt_plot_curve.h>
#include <qwt_plot_panner.h>
#include <qwt_plot_magnifier.h>
......@@ -95,6 +96,7 @@ namespace armarx
ui.qwtPlot->setAxisTitle(QwtPlot::xBottom, "Time (in sec)");
ui.qwtPlot->enableAxis(QwtPlot::yLeft, false);
ui.qwtPlot->enableAxis(QwtPlot::yRight, true);
ui.qwtPlot->setAxisAutoScale(QwtPlot::yRight, true);
// ui.qwtPlot->setAutoReplot();
......@@ -213,6 +215,8 @@ namespace armarx
selectedChannels = dialog->getSelectedChannels();
loggingDir = dialog->ui.editLoggingDirectory->text();
ui.btnLogToFile->setChecked(false);
ui.BTNPlotterStatus->setChecked(false);
markers.clear();
curves.clear();
dataMap.clear();
ui.qwtPlot->detachItems();
......@@ -243,7 +247,7 @@ namespace armarx
{
logstream << "Timestamp";
for (auto & channel : selectedChannels)
for (auto& channel : selectedChannels)
{
logstream << "," << channel.toStdString();
}
......@@ -284,8 +288,8 @@ namespace armarx
void ArmarXPlotter::pollingExec()
{
ScopedLock lock(dataMutex);
auto newData = getData(selectedChannels, dataMap);
std::map<string, VariantPtr> newData = getData(selectedChannels);
if (ui.btnLogToFile->isChecked())
{
......@@ -298,20 +302,72 @@ namespace armarx
void ArmarXPlotter::updateGraph()
{
ScopedLock lock(dataMutex);
IceUtil::Time curTime = TimeUtil::GetTime();
// erase old markers
for (auto & markerChannel : markers)
{
for (auto it = markerChannel.second.begin(); it != markerChannel.second.end();)
{
if ((curTime - it->first).toSecondsDouble() > shownInterval)
{
it = markerChannel.second.erase(it);
}
else
{
it++;
}
}
}
QPointF p;
// int size = shownInterval*1000/updateInterval;
GraphDataMap::iterator it = dataMap.begin();
IceUtil::Time curTime = TimeUtil::GetTime();
for (; it != dataMap.end(); ++it)
{
QVector<QPointF> pointList;
pointList.clear();
auto datafieldId = it->first;
std::vector<TimeData>& dataVec = it->second;
// int newSize = min(size,(int)dataVec.size());
pointList.resize(dataVec.size());
auto addMarker = [this](IceUtil::Time & age, VariantPtr & var, std::string & datafieldId)
{
ARMARX_INFO << deactivateSpam(1) << "String field: " << var->getString();
std::map < std::string, boost::shared_ptr<QwtPlotMarker>>& markerSubMap = markers[datafieldId][age];
std::string value = var->getString();
auto it2 = markerSubMap.find(value);
if (it2 != markerSubMap.end())
{
boost::shared_ptr<QwtPlotMarker> marker = it2->second;
marker->setXValue(0.001 * age.toMilliSecondsDouble());
}
else
{
boost::shared_ptr<QwtPlotMarker> marker(new QwtPlotMarker());
marker->setValue(0.001 * age.toMilliSecondsDouble(), 0);
marker->setLabel(QwtText(QString::fromStdString(value)));
marker->setLineStyle(QwtPlotMarker::VLine);
marker->setLabelAlignment(Qt::AlignBottom | Qt::AlignLeft);
int i = 0;
for (auto & markerSubMap : markers)
{
if (markerSubMap.first == datafieldId)
{
break;
}
i++;
}
marker->setLinePen(QColor(Qt::GlobalColor((i) % 5 + 13)));
marker->attach(ui.qwtPlot);
markerSubMap[value] = marker;
}
};
try
{
......@@ -328,7 +384,7 @@ namespace armarx
if (var->getInitialized())
{
float xValue = 0.001 * age.toMilliSecondsDouble();
auto type = var->getType();
if ((signed int)j < pointList.size())
{
......@@ -344,12 +400,17 @@ namespace armarx
{
pointList[j].setY(var->getInt());
}
else if (type == VariantType::String)
{
addMarker(age, var, datafieldId);
}
else
{
continue;
}
pointList[j].setX(0.001f * age.toMilliSecondsDouble());
pointList[j].setX(xValue);
}
else
{
......@@ -365,12 +426,16 @@ namespace armarx
{
p.setY(var->getInt());
}
else if (type == VariantType::String)
{
addMarker(age, var, datafieldId);
}
else
{
continue;
}
p.setX(0.001 * age.toMilliSecondsDouble());
p.setX(xValue);
pointList.push_back(p);
}
......@@ -448,7 +513,6 @@ namespace armarx
void ArmarXPlotter::plottingPaused(bool toggled)
{
ScopedLock lock(dataMutex);
__plottingPaused = toggled;
if (pollingTask)
......@@ -496,7 +560,7 @@ namespace armarx
{
QwtPlotCurve* curve = new QwtPlotCurve(label);
curve->setRenderHint(QwtPlotItem::RenderAntialiased);
curve->setPen(QColor(Qt::GlobalColor((curves.size() + 7) % 15)));
curve->setPen(QColor(Qt::GlobalColor((curves.size()) % 15 + 7)));
curve->setStyle(QwtPlotCurve::Lines);
curve->setPaintAttribute(QwtPlotCurve::CacheSymbols, true);
curve->setPaintAttribute(QwtPlotCurve::ClipPolygons, true);
......@@ -542,7 +606,7 @@ namespace armarx
ScopedLock lock(dataMutex);
markers.clear();
for (int i = 0; i < selectedChannels.size(); i++)
{
try
......@@ -552,8 +616,12 @@ namespace armarx
auto prx = getProxy<ObserverInterfacePrx>(identifier->observerName);
VariantPtr var = VariantPtr::dynamicCast(prx->getDataField(identifier));
if (VariantType::IsBasicType(var->getType()))
auto type = var->getType();
if (type == VariantType::String)
{
// do nothing for now
}
else if (VariantType::IsBasicType(type))
{
QwtPlotCurve* curve = createCurve(selectedChannels.at(i));
curves[selectedChannels.at(i).toStdString()] = curve;
......@@ -572,8 +640,8 @@ namespace armarx
VariantTypeId type = e.second->getType();
if (type == VariantType::Double
|| type == VariantType::Float
|| type == VariantType::Int)
|| type == VariantType::Float
|| type == VariantType::Int)
{
std::string key = id + "." + e.first;
ARMARX_INFO << key << ": " << *VariantPtr::dynamicCast(e.second);
......@@ -586,7 +654,7 @@ namespace armarx
}
}
}
catch(...)
catch (...)
{
handleExceptions();
}
......@@ -623,7 +691,7 @@ namespace armarx
}
std::map<string, VariantPtr> ArmarXPlotter::getData(const QStringList& channels, GraphDataMap& dataMaptoAppend)
std::map<string, VariantPtr> ArmarXPlotter::getData(const QStringList& channels)
{
map< std::string, DataFieldIdentifierBaseList > channelsSplittedByObserver;
foreach(QString channel, channels)
......@@ -633,51 +701,55 @@ namespace armarx
// ARMARX_INFO << identifier;
}
std::map<std::string, VariantPtr> newData;
// first clear to old entries
auto now = TimeUtil::GetTime();
GraphDataMap::iterator itmap = dataMaptoAppend.begin();
for (; itmap != dataMaptoAppend.end(); ++itmap)
{
std::vector<TimeData>& dataVec = itmap->second;
int stepSize = std::max<int>(1, dataVec.size() * 0.01);
int thresholdIndex = -1;
for (unsigned int i = 0; i < dataVec.size(); i += stepSize)
{
// only delete if entries are older than 2*showninterval
// and delete then all entries that are older than showninterval.
// otherwise it would delete items on every call, which would be very slow
ScopedLock lock(dataMutex);
if ((now - dataVec[i].time).toMilliSecondsDouble() > shownInterval * 2 * 1000
|| (thresholdIndex != -1 && (now - dataVec[i].time).toMilliSecondsDouble() > shownInterval * 1000)
)
{
thresholdIndex = i;
}
else
{
break;
}
}
// first clear to old entries
auto now = TimeUtil::GetTime();
GraphDataMap::iterator itmap = dataMap.begin();
if (thresholdIndex != -1)
for (; itmap != dataMap.end(); ++itmap)
{
unsigned int offset = std::min((int)dataVec.size(), thresholdIndex);
std::vector<TimeData>& dataVec = itmap->second;
int stepSize = std::max<int>(1, dataVec.size() * 0.01);
int thresholdIndex = -1;
// ARMARX_IMPORTANT << "Erasing " << offset << " fields";
if (offset > dataVec.size())
for (unsigned int i = 0; i < dataVec.size(); i += stepSize)
{
dataVec.clear();
// only delete if entries are older than 2*showninterval
// and delete then all entries that are older than showninterval.
// otherwise it would delete items on every call, which would be very slow
if ((now - dataVec[i].time).toSecondsDouble() > shownInterval * 2
|| (thresholdIndex != -1 && (now - dataVec[i].time).toSecondsDouble() > shownInterval)
)
{
thresholdIndex = i;
}
else
{
break;
}
}
else
if (thresholdIndex != -1)
{
dataVec.erase(dataVec.begin(), dataVec.begin() + offset);
unsigned int offset = std::min((int)dataVec.size(), thresholdIndex);
// ARMARX_IMPORTANT << "Erasing " << offset << " fields";
if (offset > dataVec.size())
{
dataVec.clear();
}
else
{
dataVec.erase(dataVec.begin(), dataVec.begin() + offset);
}
}
// ARMARX_IMPORTANT << deactivateSpam(5) << "size: " << dataVec.size();
}
// ARMARX_IMPORTANT << deactivateSpam(5) << "size: " << dataVec.size();
}
// now get new data
......@@ -698,6 +770,7 @@ namespace armarx
// QDateTime time(QDateTime::currentDateTime());
TimedVariantBaseList variants = proxyMap[observerName]->getDataFields(it->second);
ScopedLock lock(dataMutex);
// # ARMARX_IMPORTANT << "data from observer: " << observerName;
for (unsigned int i = 0; i < variants.size(); ++i)
{
......@@ -707,23 +780,50 @@ namespace armarx
{
continue;
}
if (VariantType::IsBasicType(var->getType()))
auto id = DataFieldIdentifierPtr::dynamicCast(it->second[i])->getIdentifierStr();
auto type = var->getType();
if (type == VariantType::String)
{
if (dataMap[id].size() == 0 || dataMap[id].rbegin()->data->getString() != var->getString())
{
// only insert if changed
dataMap[id].push_back(TimeData(time, var));
newData[id] = var;
}
}
else if (VariantType::IsBasicType(type))
{
dataMaptoAppend[DataFieldIdentifierPtr::dynamicCast(it->second[i])->getIdentifierStr()].push_back(TimeData(time, var));
newData[DataFieldIdentifierPtr::dynamicCast(it->second[i])->getIdentifierStr()] = var;
dataMap[id].push_back(TimeData(time, var));
newData[id] = var;
}
else
{
auto id = DataFieldIdentifierPtr::dynamicCast(it->second[i])->getIdentifierStr();
// ARMARX_IMPORTANT << id;
auto dict = JSONObject::ConvertToBasicVariantMap(json, var);
for (const auto & e : dict)
for (const auto& e : dict)
{
std::string key = id + "." + e.first;
// ARMARX_INFO << key << ": " << *VariantPtr::dynamicCast(e.second);
dataMaptoAppend[key].push_back(TimeData(time, VariantPtr::dynamicCast(e.second)));
newData[key] = VariantPtr::dynamicCast(e.second);
if (key == "timestamp") // TimedVariants always contain a timestamp field which is irrelevant
{
continue;
}
VariantPtr var = VariantPtr::dynamicCast(e.second);
auto type = var->getType();
if (type == VariantType::String)
{
if (dataMap[id].size() == 0 || dataMap[id].rbegin()->data->getString() != var->getString())
{
// only insert if changed
dataMap[id].push_back(TimeData(time, var));
newData[key] = var;
}
}
else
{
dataMap[key].push_back(TimeData(time, var));
newData[key] = var;
}
}
}
......@@ -762,7 +862,7 @@ namespace armarx
{
logstream << (time - logStartTime).toMilliSecondsDouble() << ",";
for (auto & channel : selectedChannels)
for (auto& channel : selectedChannels)
{
logstream << dataMaptoAppend.at(channel.toStdString())->Variant::getOutputValueOnly();
if (!selectedChannels.endsWith(channel))
......
source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotter.h
+ 3
− 2
View file @ 47d86206
......@@ -41,7 +41,6 @@
#include <ArmarXCore/observers/variant/Variant.h>
#include <ArmarXCore/util/json/JSONObject.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
// QT
#include <QWidget>
#include <QDialog>
......@@ -54,6 +53,7 @@
class QwtPlotCurve;
class QwtThermo;
class QStackedLayout;
class QwtPlotMarker;
namespace armarx
{
......@@ -152,7 +152,7 @@ namespace armarx
JSONObjectPtr json;
std::map<std::string, VariantPtr> getData(const QStringList& channels, GraphDataMap& dataMaptoAppend);
std::map<std::string, VariantPtr> getData(const QStringList& channels);
void logToFile(const IceUtil::Time& time, const std::map<std::string, VariantPtr>& dataMaptoAppend);
Mutex dataMutex;
......@@ -162,6 +162,7 @@ namespace armarx
IceUtil::Time logStartTime;
int graphStyle;
std::map<std::string, std::map<IceUtil::Time, std::map<std::string, boost::shared_ptr<QwtPlotMarker>>>> markers;
QStackedLayout* stackedLayout;
QHBoxLayout* barlayout;
std::map<std::string, QwtThermo*> bars;
......
source/RobotAPI/libraries/core/observerfilters/MatrixFilters.h
+ 12
− 0
View file @ 47d86206
......@@ -45,6 +45,8 @@ namespace armarx
VariantBasePtr calculate(const Ice::Current&) const
{
ScopedLock lock(historyMutex);
if (dataHistory.size() == 0)
{
return new Variant(new MatrixFloat(1, 1));
......@@ -74,6 +76,8 @@ namespace armarx
VariantBasePtr calculate(const Ice::Current&) const
{
ScopedLock lock(historyMutex);
if (dataHistory.size() == 0)
{
return new Variant(new MatrixFloat(1, 1));
......@@ -103,6 +107,8 @@ namespace armarx
VariantBasePtr calculate(const Ice::Current&) const
{
ScopedLock lock(historyMutex);
if (dataHistory.size() == 0)
{
return new Variant(new MatrixFloat(1, 1));
......@@ -137,6 +143,8 @@ namespace armarx
VariantBasePtr calculate(const Ice::Current&) const
{
ScopedLock lock(historyMutex);
if (dataHistory.size() == 0)
{
return new Variant(new MatrixFloat(1, 1));
......@@ -194,6 +202,8 @@ namespace armarx
VariantBasePtr calculate(const Ice::Current&) const
{
ScopedLock lock(historyMutex);
if (dataHistory.size() == 0)
{
ARMARX_IMPORTANT_S << "no data";
......@@ -241,6 +251,8 @@ namespace armarx
}
VariantBasePtr calculate(const Ice::Current&) const
{
ScopedLock lock(historyMutex);
if (dataHistory.size() == 0)
{
return new Variant(new MatrixFloat(1, 1));
......
source/RobotAPI/libraries/core/observerfilters/OffsetFilter.h
+ 4
− 0
View file @ 47d86206
......@@ -58,6 +58,8 @@ namespace armarx
VariantBasePtr calculate(const Ice::Current& c = Ice::Current()) const
{
ScopedLock lock(historyMutex);
VariantPtr newVariant;
if (initialValue && dataHistory.size() > 0)
......@@ -136,6 +138,8 @@ namespace armarx
if (firstRun)
{
ScopedLock lock(historyMutex);
if (dataHistory.size() == 0)
{
return;
......
source/RobotAPI/libraries/core/observerfilters/PoseMedianFilter.h
+ 5
− 3
View file @ 47d86206
......@@ -15,9 +15,9 @@
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
* @package
* @author
* @date
* @package
* @author
* @date
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
......@@ -53,6 +53,8 @@ namespace armarx
public:
VariantBasePtr calculate(const Ice::Current& c) const
{
ScopedLock lock(historyMutex);
if (dataHistory.size() == 0)
{
return NULL;
......
source/RobotAPI/libraries/core/observerfilters/PoseMedianOffsetFilter.cpp
+ 6
− 4
View file @ 47d86206
......@@ -15,9 +15,9 @@
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
* @package
* @author
* @date
* @package
* @author
* @date
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
......@@ -37,6 +37,8 @@ armarx::filters::PoseMedianOffsetFilter::PoseMedianOffsetFilter(int windowSize)
armarx::VariantBasePtr armarx::filters::PoseMedianOffsetFilter::calculate(const Ice::Current& c) const
{
ScopedLock lock(historyMutex);
if (dataHistory.size() == 0)
{
return NULL;
......@@ -93,7 +95,7 @@ Eigen::Vector3f armarx::filters::PoseMedianOffsetFilter::calculateMedian()
std::vector<float> values;
values.reserve(data.size());
for (const Eigen::Vector3f& v : data)
for (const Eigen::Vector3f & v : data)
{
values.push_back(v(i));
}
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment