diff --git a/etc/doxygen/pages/3.HowTos.dox b/etc/doxygen/pages/3.HowTos.dox index abb2dc4a0f95ad228aa0e763a8c3a1b641e53237..03fe8b11e4c7bcb6b3294a9cb673ef74ac20212e 100644 --- a/etc/doxygen/pages/3.HowTos.dox +++ b/etc/doxygen/pages/3.HowTos.dox @@ -7,18 +7,33 @@ \page RobotAPI-HowTos-Eigen-From-Pose How to convert a Variant pose into an Eigen Matrix \ingroup RobotAPI-HowTos +If you have a armarx::Pose or armarx::FramedPose or armarx::LinkedPose: \code -#include <RobotAPI/libraries/robotstate/remote/ArmarPose.h> -#include <VirtualRobot/LinkedCoordinate.h> -#include <Eigen/Core> +#include <RobotAPI/libraries/core/LinkedPose.h> std::string refFrame = "Base"; -FramedPositionPtr position = getInput<FramedPosition>("position"); -FramedOrientationPtr orientation = getInput<FramedOrientation>("orientation"); +FramedPosePtr pose = getInput<FramedPose>("pose"); // if you are in a statechart +Eigen::Matrix4f matrix = pose->toEigen(); -VirtualRobot::LinkedCoordinate pose = ArmarPose::createLinkedCoordinate(context->remoteRobot, position, orientation); +\endcode + +If you only have a armarx::FramedPosition and armarx::FramedOrientation: +\code +#include <RobotAPI/libraries/core/LinkedPose.h> + +std::string refFrame = "Base"; +FramedPositionPtr position = getInput<FramedPosition>("position"); // if you are in a statechart +FramedOrientationPtr orientation = getInput<FramedOrientation>("orientation"); // if you are in a statechart -Eigen::Matrix4f matrix = pose.getInFrame(refFrame); +PosePtr pose = new Pose(position, orientation); +Eigen::Matrix4f matrix = pose->toEigen(); \endcode + +For more information refer to \ref RobotAPI-FramedPose. + + +\page RobotAPI-HowTos-Change-Pose-Frame How to change a frame of a coordinate/pose +\ingroup RobotAPI-HowTos +Refer to \ref RobotAPI-FramedPose. */ diff --git a/source/RobotAPI/gui-plugins/CMakeLists.txt b/source/RobotAPI/gui-plugins/CMakeLists.txt index 04e3c1a1b03dcfa0d1806ac775c86456bf2184f0..50dd1f70661543f5dd06924c4e22b464500eacd8 100644 --- a/source/RobotAPI/gui-plugins/CMakeLists.txt +++ b/source/RobotAPI/gui-plugins/CMakeLists.txt @@ -2,7 +2,7 @@ add_subdirectory(KinematicUnitPlugin) add_subdirectory(HandUnitPlugin) add_subdirectory(PlatformUnitPlugin) -#add_subdirectory(SensorActorWidgetsPlugin) +add_subdirectory(SensorActorWidgetsPlugin) add_subdirectory(HapticUnitPlugin) add_subdirectory(RobotViewerPlugin) diff --git a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotter.cpp b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotter.cpp index 83a9d6b69f1f52c43a5055588a24dfd0f07a2576..f761341ad777fbf0ffdead7c3485465b0d4c6ec1 100644 --- a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotter.cpp +++ b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotter.cpp @@ -41,6 +41,7 @@ #include <QTimer> #include <QTime> #include <QSettings> +#include <QDir> #include <boost/date_time/posix_time/posix_time.hpp> @@ -87,7 +88,8 @@ namespace armarx // ui.qwtPlot->setAutoReplot(); dialog = new ArmarXPlotterDialog(getWidget(), NULL); - + loggingDir = (QDir::currentPath()); + dialog->ui.editLoggingDirectory->setText(loggingDir); } ArmarXPlotter::~ArmarXPlotter() @@ -123,6 +125,8 @@ namespace armarx connect( ui.qwtPlot, SIGNAL( legendChecked( QwtPlotItem *, bool ) ), SLOT( showCurve( QwtPlotItem *, bool ) ) ); + connect(ui.btnLogToFile, SIGNAL(toggled(bool)), this, SLOT(toggleLogging(bool))); + if(!QMetaObject::invokeMethod(this, "setupCurves")) ARMARX_WARNING << "Failed to invoke enable"; @@ -139,6 +143,10 @@ namespace armarx { ARMARX_VERBOSE << "closing" << flush; timer->stop(); + if(logstream.is_open()) + { + logstream.close(); + } } void ArmarXPlotter::ButtonAddSensorChannelClicked() @@ -170,6 +178,8 @@ namespace armarx shownInterval = dialog->ui.spinBoxShownInterval->value(); pollingInterval = dialog->ui.spinBoxPollingInterval->value(); selectedChannels = dialog->getSelectedChannels(); + loggingDir = dialog->ui.editLoggingDirectory->text(); + ui.btnLogToFile->setChecked(false); curves.clear(); ui.qwtPlot->detachItems(); } @@ -177,10 +187,46 @@ namespace armarx setupCurves(); } + void ArmarXPlotter::toggleLogging(bool toggled) + { + if(toggled) + { + + std::string filename = "datalog-" + IceUtil::Time::now().toDateTime() + ".csv"; + boost::replace_all(filename, "/", "-"); + filename = loggingDir.toStdString() + "/" + filename; + ARMARX_INFO << "Logging to " << filename; + logstream.open(filename); + logStartTime = IceUtil::Time::now(); + if(!logstream.is_open()) + { + ARMARX_ERROR << "Could not open file for logging: " << filename; + ui.btnLogToFile->setChecked(false); + } + else + { + logstream << "Timestamp"; + for(auto& channel: selectedChannels) + { + logstream << "," << channel.toStdString(); + } + logstream << std::endl; + } + } + else + { + logstream.close(); + } + } + void ArmarXPlotter::pollingExec() { ScopedLock lock(dataMutex); - getData(selectedChannels, dataMap); + auto newData = getData(selectedChannels, dataMap); + if(ui.btnLogToFile->isChecked()) + { + logToFile(IceUtil::Time::now(), newData); + } } @@ -192,7 +238,7 @@ namespace armarx // int size = shownInterval*1000/updateInterval; GraphDataMap::iterator it = dataMap.begin(); - boost::system_time curTime = boost::get_system_time(); + IceUtil::Time curTime = IceUtil::Time::now(); for(; it != dataMap.end(); ++it) { @@ -207,8 +253,8 @@ namespace armarx int j = 0; for (int i = dataVec.size()-1; i >=0 ; --i) { TimeData& data = dataVec[i]; - boost::posix_time::time_duration age = (data.time - curTime); - if(age.total_milliseconds() <= shownInterval*1000) + IceUtil::Time age = (data.time - curTime); + if(age.toMilliSecondsDouble() <= shownInterval*1000) { VariantPtr var = VariantPtr::dynamicCast(data.data); if(var->getInitialized()) @@ -223,7 +269,7 @@ namespace armarx else if(var->getType() == VariantType::Int) pointList[j].setY(var->getInt()); else continue; - pointList[j].setX(0.001f*age.total_milliseconds()); + pointList[j].setX(0.001f*age.toMilliSecondsDouble()); }else { if(var->getType() == VariantType::Float) @@ -233,7 +279,7 @@ namespace armarx else if(var->getType() == VariantType::Int) p.setY(var->getInt()); else continue; - p.setX(0.001*age.total_milliseconds()); + p.setX(0.001*age.toMilliSecondsDouble()); pointList.push_back(p); } j++; @@ -245,7 +291,7 @@ namespace armarx } else - break; // data too old from noow + break; // data too old from now } } catch(...) @@ -392,7 +438,7 @@ namespace armarx pollingTask->start(); } - void ArmarXPlotter::getData(const QStringList &channels, GraphDataMap &dataMaptoAppend) + std::map<string, VariantPtr> ArmarXPlotter::getData(const QStringList &channels, GraphDataMap &dataMaptoAppend) { map< std::string, DataFieldIdentifierBaseList > channelsSplittedByObserver; foreach(QString channel, channels) @@ -401,9 +447,10 @@ namespace armarx channelsSplittedByObserver[identifier->getObserverName()].push_back(identifier); // ARMARX_INFO << identifier; } + std::map<std::string, VariantPtr> newData; // first clear to old entries - auto now = boost::get_system_time(); + auto now = IceUtil::Time::now(); GraphDataMap::iterator itmap = dataMaptoAppend.begin(); for(; itmap != dataMaptoAppend.end(); ++itmap) { @@ -416,9 +463,8 @@ namespace armarx // 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).total_milliseconds() > shownInterval*2*1000 - || (thresholdIndex != -1 && (now - dataVec[i].time ).total_milliseconds() > shownInterval*1000) + if((now - dataVec[i].time).toMilliSecondsDouble() > shownInterval*2*1000 + || (thresholdIndex != -1 && (now - dataVec[i].time ).toMilliSecondsDouble() > shownInterval*1000) ) { thresholdIndex = i; @@ -437,6 +483,7 @@ namespace armarx // ARMARX_IMPORTANT << deactivateSpam(5) << "size: " << dataVec.size(); } // now get new data + IceUtil::Time time = IceUtil::Time::now(); map<string, DataFieldIdentifierBaseList >::iterator it = channelsSplittedByObserver.begin(); try { @@ -449,7 +496,6 @@ namespace armarx proxyMap[observerName] = getProxy<ObserverInterfacePrx>(observerName); } // QDateTime time(QDateTime::currentDateTime()); - boost::system_time time = boost::get_system_time(); VariantBaseList variants = proxyMap[observerName]->getDataFields(it->second); // ARMARX_IMPORTANT << "data from observer: " << observerName; for(unsigned int i = 0; i < variants.size(); ++i) @@ -457,7 +503,10 @@ namespace armarx // ARMARX_IMPORTANT << "Variant: " << VariantPtr::dynamicCast(variants[i]); VariantPtr var = VariantPtr::dynamicCast(variants[i]); if(VariantType::IsBasicType(var->getType())) + { dataMaptoAppend[DataFieldIdentifierPtr::dynamicCast(it->second[i])->getIdentifierStr()].push_back(TimeData(time, var)); + newData[DataFieldIdentifierPtr::dynamicCast(it->second[i])->getIdentifierStr()] = var; + } else { auto id = DataFieldIdentifierPtr::dynamicCast(it->second[i])->getIdentifierStr(); @@ -468,6 +517,7 @@ namespace armarx 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); } } @@ -476,18 +526,34 @@ namespace armarx } } catch(Ice::NotRegisteredException &e){ - ARMARX_WARNING << deactivateSpam(3) << "Caught Exception: " << e.what(); + ARMARX_WARNING << deactivateSpam(3) << "Caught Ice::NotRegisteredException: " << e.what(); } catch(exceptions::local::InvalidDataFieldException &e){ ARMARX_WARNING << deactivateSpam(5) << "Caught InvalidDataFieldException: " << e.what(); } catch(exceptions::local::InvalidChannelException &e){ - ARMARX_WARNING << deactivateSpam(5) << "Caught InvalidDataFieldException: " << e.what(); + ARMARX_WARNING << deactivateSpam(5) << "Caught InvalidChannelException: " << e.what(); } catch(...) { handleExceptions(); } + + return newData; + } + + void ArmarXPlotter::logToFile(const IceUtil::Time &time, const std::map<std::string, VariantPtr> &dataMaptoAppend) + { + + if(logstream.is_open() && dataMaptoAppend.size() > 0) + { + logstream << (time-logStartTime).toMilliSecondsDouble(); + for(const auto& elem : dataMaptoAppend) + { + logstream << "," /*<< elem.first << ","*/ << elem.second->getOutputValueOnly(); + } + logstream << std::endl; + } } diff --git a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotter.h b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotter.h index 33f6c200ac19cad97310609a46363050cfe3e05f..1320987e58419ecc3b6f1e4b45e52e8bcc85578e 100644 --- a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotter.h +++ b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotter.h @@ -45,6 +45,7 @@ #include <QDateTime> #include <vector> +#include <fstream> //forward declarations class QwtPlotCurve; @@ -52,9 +53,9 @@ class QwtPlotCurve; namespace armarx { struct TimeData{ - TimeData(const boost::system_time &time, const VariantPtr &data): time(time), data(data){} + TimeData(const IceUtil::Time &time, const VariantPtr &data): time(time), data(data){} - boost::system_time time; + IceUtil::Time time; VariantPtr data; }; @@ -87,6 +88,7 @@ namespace armarx int updateInterval; int shownInterval; QStringList selectedChannels; + QString loggingDir; explicit ArmarXPlotter(); ~ArmarXPlotter(); @@ -119,6 +121,7 @@ namespace armarx void autoScale(bool toggled); void plottingPaused(bool toggled); void configDone(); + void toggleLogging(bool toggled); private slots: void setupCurves(); @@ -134,11 +137,14 @@ namespace armarx JSONObjectPtr json; - void getData(const QStringList &channels, GraphDataMap &dataMaptoAppend); + std::map<std::string, VariantPtr> getData(const QStringList &channels, GraphDataMap &dataMaptoAppend); + void logToFile(const IceUtil::Time &time,const std::map<std::string, VariantPtr> &dataMaptoAppend); Mutex dataMutex; PeriodicTask<ArmarXPlotter>::pointer_type pollingTask; int pollingInterval; + std::ofstream logstream; + IceUtil::Time logStartTime; }; } diff --git a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotter.ui b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotter.ui index 9753932628e15dc46c8cb01c97eb72a96d4ac302..d22fe734809566397b306e5543fc6e0b365c3357 100644 --- a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotter.ui +++ b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotter.ui @@ -32,7 +32,16 @@ <property name="spacing"> <number>3</number> </property> - <property name="margin"> + <property name="leftMargin"> + <number>3</number> + </property> + <property name="topMargin"> + <number>3</number> + </property> + <property name="rightMargin"> + <number>3</number> + </property> + <property name="bottomMargin"> <number>3</number> </property> <item> @@ -49,7 +58,7 @@ <string>...</string> </property> <property name="icon"> - <iconset resource="../../../ArmarXGui/icons.qrc"> + <iconset> <normaloff>:/icons/configure-3.png</normaloff>:/icons/configure-3.png</iconset> </property> </widget> @@ -60,7 +69,7 @@ <string>...</string> </property> <property name="icon"> - <iconset resource="../../../ArmarXGui/icons.qrc"> + <iconset> <normaloff>:/icons/media-playback-pause.ico</normaloff>:/icons/media-playback-pause.ico</iconset> </property> <property name="checkable"> @@ -77,7 +86,7 @@ <string>Auto Scale</string> </property> <property name="icon"> - <iconset resource="../../../ArmarXGui/icons.qrc"> + <iconset> <normaloff>:/icons/zoom-original-2.png</normaloff>:/icons/zoom-original-2.png</iconset> </property> <property name="checkable"> @@ -88,6 +97,20 @@ </property> </widget> </item> + <item> + <widget class="QToolButton" name="btnLogToFile"> + <property name="text"> + <string>...</string> + </property> + <property name="icon"> + <iconset> + <normaloff>://icons/papyrus.svg</normaloff>://icons/papyrus.svg</iconset> + </property> + <property name="checkable"> + <bool>true</bool> + </property> + </widget> + </item> <item> <spacer name="horizontalSpacer"> <property name="orientation"> @@ -113,8 +136,6 @@ <container>1</container> </customwidget> </customwidgets> - <resources> - <include location="../../../ArmarXGui/icons.qrc"/> - </resources> + <resources/> <connections/> </ui> diff --git a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotterDialog.cpp b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotterDialog.cpp index 85d2c758a9c4f9c02f7f8e5c7c7f309aa56aee06..9ec9e79858fa043994e6cd3a0c832f68a208d62a 100644 --- a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotterDialog.cpp +++ b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotterDialog.cpp @@ -26,6 +26,7 @@ #include <IceUtil/UUID.h> +#include <QFileDialog> #include <sstream> @@ -187,3 +188,13 @@ void ArmarXPlotterDialog::showEvent(QShowEvent *) updateObservers(); } + +void armarx::ArmarXPlotterDialog::on_btnSelectLoggingDir_clicked() +{ + QString newLoggingDir = QFileDialog::getExistingDirectory(this, "Select a directory to which data should be logged", + ui.editLoggingDirectory->text()); + if(!newLoggingDir.isEmpty()) + { + ui.editLoggingDirectory->setText(newLoggingDir); + } +} diff --git a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotterDialog.h b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotterDialog.h index 096982ac93b9dbce75eb8da42d8ab213bb27cca3..d00f727f7498525a0a46b646a1b80278626b94dc 100644 --- a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotterDialog.h +++ b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotterDialog.h @@ -93,6 +93,8 @@ namespace armarx void treeView_doubleClick(const QModelIndex &index); void destroyed(QObject *); void showEvent(QShowEvent *); + private slots: + void on_btnSelectLoggingDir_clicked(); }; } #endif diff --git a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotterDialog.ui b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotterDialog.ui index 6b31aef4fe7e9bb668ce862a62a663bceef58d7d..4b9bd05e5b84667b21728366ff19aadf4d3470b8 100644 --- a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotterDialog.ui +++ b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotterDialog.ui @@ -39,20 +39,57 @@ </property> </widget> </item> - <item row="0" column="2"> - <widget class="QSpinBox" name="spinBoxShownInterval"> + <item row="0" column="1"> + <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> + <item row="4" column="0"> + <widget class="QLabel" name="label"> + <property name="text"> + <string>Data polling interval (in ms)</string> + </property> + </widget> + </item> + <item row="5" column="0"> + <widget class="QLabel" name="label_2"> + <property name="text"> + <string>Logging directory</string> + </property> + </widget> + </item> + <item row="5" column="1" colspan="2"> + <widget class="QLineEdit" name="editLoggingDirectory"/> + </item> + <item row="5" column="3"> + <widget class="QToolButton" name="btnSelectLoggingDir"> + <property name="text"> + <string>...</string> + </property> + </widget> + </item> + <item row="4" column="2" colspan="2"> + <widget class="QSpinBox" name="spinBoxPollingInterval"> <property name="minimum"> <number>1</number> </property> <property name="maximum"> - <number>6000</number> + <number>999999</number> </property> <property name="value"> - <number>60</number> + <number>33</number> </property> </widget> </item> - <item row="1" column="2"> + <item row="1" column="2" colspan="2"> <widget class="QSpinBox" name="spinBoxUpdateInterval"> <property name="buttonSymbols"> <enum>QAbstractSpinBox::UpDownArrows</enum> @@ -71,36 +108,16 @@ </property> </widget> </item> - <item row="0" column="1"> - <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> - <item row="2" column="0"> - <widget class="QLabel" name="label"> - <property name="text"> - <string>Data polling interval (in ms)</string> - </property> - </widget> - </item> - <item row="2" column="2"> - <widget class="QSpinBox" name="spinBoxPollingInterval"> + <item row="0" column="2" colspan="2"> + <widget class="QSpinBox" name="spinBoxShownInterval"> <property name="minimum"> <number>1</number> </property> <property name="maximum"> - <number>999999</number> + <number>6000</number> </property> <property name="value"> - <number>33</number> + <number>60</number> </property> </widget> </item> diff --git a/source/RobotAPI/libraries/drivers/XsensIMU/XsensIMU.h b/source/RobotAPI/libraries/drivers/XsensIMU/XsensIMU.h index f4eab128354920044cd2bdc2f73a392fa6410569..206796a20064c0fded18f4312c34ee710d860fcf 100644 --- a/source/RobotAPI/libraries/drivers/XsensIMU/XsensIMU.h +++ b/source/RobotAPI/libraries/drivers/XsensIMU/XsensIMU.h @@ -47,10 +47,10 @@ namespace armarx XsensIMUPropertyDefinitions(std::string prefix): InertialMeasurementUnitPropertyDefinitions(prefix) { - defineOptionalProperty("deviceConnection", "/dev/ttyUSB0", ""); + defineOptionalProperty<std::string>("deviceConnection", "/dev/ttyUSB0", ""); - defineOptionalProperty("frequency", "", ""); - defineOptionalProperty("maxPendingEvents", "", ""); + defineOptionalProperty<std::string>("frequency", "", ""); + defineOptionalProperty<std::string>("maxPendingEvents", "", ""); } };