From ec590359e3b33386fa3f62fe6c6bc60e87ef6f32 Mon Sep 17 00:00:00 2001 From: RoboDK <37006818+RoboDK@users.noreply.github.com> Date: Thu, 8 May 2025 21:10:22 +0200 Subject: [PATCH 1/4] Build success on Mac and Qt6.7 --- PluginAppLoader/AppLoader.pro | 1 + PluginAppLoader/apploader.cpp | 14 +++++++------- PluginAppLoader/installerdialog.cpp | 6 ++++-- robodk_interface/robodktools.cpp | 2 +- 4 files changed, 13 insertions(+), 10 deletions(-) diff --git a/PluginAppLoader/AppLoader.pro b/PluginAppLoader/AppLoader.pro index f6a6818..7d0bc2a 100644 --- a/PluginAppLoader/AppLoader.pro +++ b/PluginAppLoader/AppLoader.pro @@ -37,6 +37,7 @@ CONFIG += plugin #QT += core gui QT += widgets QT += network # Allows using QTcpSocket +QT += core5compat # Define your plugin name (name of the DLL file generated) TARGET = AppLoader diff --git a/PluginAppLoader/apploader.cpp b/PluginAppLoader/apploader.cpp index 928da10..413e9d9 100644 --- a/PluginAppLoader/apploader.cpp +++ b/PluginAppLoader/apploader.cpp @@ -24,6 +24,7 @@ #include #include #include +#include // Function to check and sort priority of apps struct CheckPriority { @@ -779,8 +780,8 @@ void AppLoader::AppsSearch(bool install_requirements){ } // Sort actions for the menu and the toolbar - qSort(menuActions.begin(), menuActions.end(), CheckPriority()); - qSort(toolbarActions.begin(), toolbarActions.end(), CheckPriority()); + std::sort(menuActions.begin(), menuActions.end(), CheckPriority()); + std::sort(toolbarActions.begin(), toolbarActions.end(), CheckPriority()); // Add menu actions to the menu for (int i=0; i 1){ - qSort(ListActions.begin(), ListActions.end(), CheckPriority()); + std::sort(ListActions.begin(), ListActions.end(), CheckPriority()); } // Sort menus if (ListMenus.length() > 1){ - qSort(ListMenus.begin(), ListMenus.end(), CheckPriority()); + std::sort(ListMenus.begin(), ListMenus.end(), CheckPriority()); } // Sort toolbars if (ListToolbars.length() > 1){ - qSort(ListToolbars.begin(), ListToolbars.end(), CheckPriority()); + std::sort(ListToolbars.begin(), ListToolbars.end(), CheckPriority()); } // Append Apps directories to RoboDK's PYTHONPATH @@ -1012,8 +1013,7 @@ void AppLoader::onRunScript(){ if (action->isCheckable()){ if (action->isChecked()){ // make sure we uncheck the action if the process ends or stops - connect(proc, static_cast(&QProcess::finished), proc, [action]() { - + connect(proc, &QProcess::finished, proc, [action]() { QActionGroup *grp = action->actionGroup(); if (grp != nullptr && grp->checkedAction() == action){ emit grp->triggered(action); // important to reset the group lastaction static variable diff --git a/PluginAppLoader/installerdialog.cpp b/PluginAppLoader/installerdialog.cpp index 28e3414..f59c318 100644 --- a/PluginAppLoader/installerdialog.cpp +++ b/PluginAppLoader/installerdialog.cpp @@ -10,6 +10,8 @@ #include #include #include +#include + InstallerDialog::InstallerDialog(AppLoader* apploader, QWidget* parent) : @@ -83,7 +85,7 @@ bool InstallerDialog::processPackage(const QString& package){ QString name = unzipper.entryName(); int slash = name.indexOf('/'); - if (slash < 0 || name.midRef(slash).compare(QString("/AppConfig.ini")) != 0) + if (slash < 0 || name.mid(slash).compare(QString("/AppConfig.ini")) != 0) continue; name.truncate(slash); @@ -308,7 +310,7 @@ void InstallerDialog::on_buttonBox_accepted(){ if (!name.startsWith(entity.name + "/")) continue; - QFileInfo fileInfo = folder.absoluteFilePath(name); + QFileInfo fileInfo(folder.absoluteFilePath(name)); folder.mkpath(fileInfo.absolutePath()); unzipper.entryExtract(fileInfo.absoluteFilePath()); } diff --git a/robodk_interface/robodktools.cpp b/robodk_interface/robodktools.cpp index f43cd58..2b7b9f7 100644 --- a/robodk_interface/robodktools.cpp +++ b/robodk_interface/robodktools.cpp @@ -45,7 +45,7 @@ void string_2_doubles(const QString &str, double *values, int *size_inout, const bool isok; QString line; QString strnum; - QRegExp rx(separator); + QRegularExpression rx(separator); line = str.trimmed(); QStringList strfloats = line.split(rx, behavior); *size_inout = qMin(strfloats.size(), *size_inout); From af6533ae63e2b4fcc49548699d25f7fe7b4fe1ec Mon Sep 17 00:00:00 2001 From: RoboDK <37006818+RoboDK@users.noreply.github.com> Date: Thu, 8 May 2025 21:11:30 +0200 Subject: [PATCH 2/4] Removed core5compat dependency --- PluginAppLoader/AppLoader.pro | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/PluginAppLoader/AppLoader.pro b/PluginAppLoader/AppLoader.pro index 7d0bc2a..392a141 100644 --- a/PluginAppLoader/AppLoader.pro +++ b/PluginAppLoader/AppLoader.pro @@ -37,7 +37,7 @@ CONFIG += plugin #QT += core gui QT += widgets QT += network # Allows using QTcpSocket -QT += core5compat +# QT += core5compat # Define your plugin name (name of the DLL file generated) TARGET = AppLoader From 478687abde6e9264cd2da22e7be86690b66b43e8 Mon Sep 17 00:00:00 2001 From: VDm Date: Mon, 12 May 2025 16:34:46 +0400 Subject: [PATCH 3/4] fix: update projects to support Qt 6.9 on Windows (MSVC) --- Plugin-OPC-UA/PluginOPCUA.pro | 1 + Plugin-OPC-UA/opcua_server.cpp | 5 - Plugin-OPC-UA/pluginopcua.cpp | 2 +- PluginAppLoader/AppLoader.pro | 1 - PluginAppLoader/apploader.cpp | 2 +- PluginBallbarTracker/PluginBallbarTracker.cpp | 2 +- PluginExample/pluginexample.cpp | 2 +- PluginOpenGL-Shaders/PluginChip8Opengl.pro | 3 + .../robodk_api/robodk_api.cpp | 2184 ++++++++++++----- PluginOpenGL-Shaders/robodk_api/robodk_api.h | 158 +- PluginRealTime/pluginexample.cpp | 2 +- PluginRoboUI/roboui.cpp | 10 + 12 files changed, 1669 insertions(+), 703 deletions(-) diff --git a/Plugin-OPC-UA/PluginOPCUA.pro b/Plugin-OPC-UA/PluginOPCUA.pro index 07a24dd..07e570a 100644 --- a/Plugin-OPC-UA/PluginOPCUA.pro +++ b/Plugin-OPC-UA/PluginOPCUA.pro @@ -136,6 +136,7 @@ INCLUDEPATH += ../robodk_interface # ------------------------ # Flag to compile Open62541 source for any platform win32-msvc { + DEFINES += _CRT_SECURE_NO_WARNINGS QMAKE_CXXFLAGS_WARN_ON += -wd4100 QMAKE_CFLAGS_WARN_ON += -wd4100 QMAKE_CFLAGS += -std:clatest diff --git a/Plugin-OPC-UA/opcua_server.cpp b/Plugin-OPC-UA/opcua_server.cpp index 5a38a94..7ebae32 100644 --- a/Plugin-OPC-UA/opcua_server.cpp +++ b/Plugin-OPC-UA/opcua_server.cpp @@ -20,11 +20,6 @@ #include -#ifdef _MSC_VER -#define _CRT_SECURE_NO_WARNINGS //disable fopen deprecation warning in msvs -#endif - - //---------------------------- #include #include diff --git a/Plugin-OPC-UA/pluginopcua.cpp b/Plugin-OPC-UA/pluginopcua.cpp index 0e8d4d3..1d3d891 100644 --- a/Plugin-OPC-UA/pluginopcua.cpp +++ b/Plugin-OPC-UA/pluginopcua.cpp @@ -348,7 +348,7 @@ void PluginOPCUA::SaveSettings(){ qDebug() << "Saving OPC-UA plugin settings..."; QByteArray data; - QDataStream ds(&data, QIODevice::WriteOnly); + QDataStream ds(&data, QDataStream::WriteOnly); qint64 version = 2; ds << version; ds << Server->Port; diff --git a/PluginAppLoader/AppLoader.pro b/PluginAppLoader/AppLoader.pro index 392a141..f6a6818 100644 --- a/PluginAppLoader/AppLoader.pro +++ b/PluginAppLoader/AppLoader.pro @@ -37,7 +37,6 @@ CONFIG += plugin #QT += core gui QT += widgets QT += network # Allows using QTcpSocket -# QT += core5compat # Define your plugin name (name of the DLL file generated) TARGET = AppLoader diff --git a/PluginAppLoader/apploader.cpp b/PluginAppLoader/apploader.cpp index 413e9d9..092e2e4 100644 --- a/PluginAppLoader/apploader.cpp +++ b/PluginAppLoader/apploader.cpp @@ -1013,7 +1013,7 @@ void AppLoader::onRunScript(){ if (action->isCheckable()){ if (action->isChecked()){ // make sure we uncheck the action if the process ends or stops - connect(proc, &QProcess::finished, proc, [action]() { + connect(proc, QOverload::of(&QProcess::finished), proc, [action]() { QActionGroup *grp = action->actionGroup(); if (grp != nullptr && grp->checkedAction() == action){ emit grp->triggered(action); // important to reset the group lastaction static variable diff --git a/PluginBallbarTracker/PluginBallbarTracker.cpp b/PluginBallbarTracker/PluginBallbarTracker.cpp index 974a669..fc633bc 100644 --- a/PluginBallbarTracker/PluginBallbarTracker.cpp +++ b/PluginBallbarTracker/PluginBallbarTracker.cpp @@ -329,7 +329,7 @@ void PluginBallbarTracker::update_ballbar_pose(){ double rho = 90.0 - qRadiansToDegrees(qAcos(pillar_2_tool_vec.z() / std::max(1e-6, r))); // Theta θ - double theta = 180 - qRadiansToDegrees(qAcos(pillar_2_tool_vec.x() / std::max(1e-6, qSqrt(pillar_2_tool_vec.x() * pillar_2_tool_vec.x() + pillar_2_tool_vec.y() * pillar_2_tool_vec.y())))); + double theta = 180 - qRadiansToDegrees(qAcos(pillar_2_tool_vec.x() / std::max(1e-6f, qSqrt(pillar_2_tool_vec.x() * pillar_2_tool_vec.x() + pillar_2_tool_vec.y() * pillar_2_tool_vec.y())))); if (pillar_2_tool_vec.y() > 0){ theta = -theta; } diff --git a/PluginExample/pluginexample.cpp b/PluginExample/pluginexample.cpp index 9e5c718..4a7a8b2 100644 --- a/PluginExample/pluginexample.cpp +++ b/PluginExample/pluginexample.cpp @@ -316,7 +316,7 @@ void PluginExample::callback_information(){ // Perform some timing tests using the RoboDK API RDK->ShowMessage("Starting timing tests", false); - QString text_message_html("Plugin Timing Tests Summary on " + QDateTime::currentDateTime().toString(Qt::SystemLocaleLongDate) + ":
"); + QString text_message_html("Plugin Timing Tests Summary on " + QDateTime::currentDateTime().toString() + ":
"); int ntests=10000; //Item robot = RDK->getItem("", IItem::ITEM_TYPE_ROBOT); diff --git a/PluginOpenGL-Shaders/PluginChip8Opengl.pro b/PluginOpenGL-Shaders/PluginChip8Opengl.pro index f126574..ab6fa14 100644 --- a/PluginOpenGL-Shaders/PluginChip8Opengl.pro +++ b/PluginOpenGL-Shaders/PluginChip8Opengl.pro @@ -36,6 +36,9 @@ QT += opengl QT += gui QT += concurrent +greaterThan(QT_MAJOR_VERSION, 5) { + QT += openglwidgets +} #-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-* diff --git a/PluginOpenGL-Shaders/robodk_api/robodk_api.cpp b/PluginOpenGL-Shaders/robodk_api/robodk_api.cpp index 05b646c..d018395 100644 --- a/PluginOpenGL-Shaders/robodk_api/robodk_api.cpp +++ b/PluginOpenGL-Shaders/robodk_api/robodk_api.cpp @@ -1,8 +1,11 @@ #include "robodk_api.h" -#include -#include + #include #include +#include + +#include +#include #include @@ -30,7 +33,9 @@ +#ifndef M_PI #define M_PI 3.14159265358979323846264338327950288 +#endif #ifndef RDK_SKIP_NAMESPACE @@ -39,311 +44,417 @@ namespace RoboDK_API { //----------------------------------- Joints class ------------------------ -tJoints::tJoints(int ndofs){ +tJoints::tJoints(int ndofs) +{ _nDOFs = qMin(ndofs, RDK_SIZE_JOINTS_MAX); - for (int i=0; i<_nDOFs; i++){ + for (int i=0; i < _nDOFs; i++) + { _Values[i] = 0.0; } } -tJoints::tJoints(const tJoints ©){ + +tJoints::tJoints(const tJoints ©) +{ SetValues(copy._Values, copy._nDOFs); } -tJoints::tJoints(const double *joints, int ndofs){ + +tJoints::tJoints(const double *joints, int ndofs) +{ SetValues(joints, ndofs); } -tJoints::tJoints(const float *joints, int ndofs){ + +tJoints::tJoints(const float *joints, int ndofs) +{ int ndofs_ok = qMin(ndofs, RDK_SIZE_JOINTS_MAX); double jnts[RDK_SIZE_JOINTS_MAX]; - for (int i=0; i= ncols){ + +tJoints::tJoints(const tMatrix2D *mat2d, int column, int ndofs) +{ + if (column >= Matrix2D_Size(mat2d, 2)) + { _nDOFs = 0; - qDebug()<<"Warning: tMatrix2D column outside range when creating joints"; + qDebug() << "Warning: tMatrix2D column outside range when creating joints"; } - if (ndofs < 0){ + if (ndofs < 0) + { ndofs = Matrix2D_Size(mat2d, 1); } _nDOFs = qMin(ndofs, RDK_SIZE_JOINTS_MAX); + double *ptr = Matrix2D_Get_col(mat2d, column); SetValues(ptr, _nDOFs); } -tJoints::tJoints(const QString &str){ + +tJoints::tJoints(const QString &str) +{ _nDOFs = 0; FromString(str); } -const double* tJoints::ValuesD() const{ +const double* tJoints::ValuesD() const +{ return _Values; } -const float* tJoints::ValuesF() const{ - for (int i=0; i= 0){ +void tJoints::SetValues(const double *values, int ndofs) +{ + if (ndofs >= 0) + { _nDOFs = qMin(ndofs, RDK_SIZE_JOINTS_MAX); } - for (int i=0; i<_nDOFs; i++){ + + for (int i = 0; i < _nDOFs; i++) + { _Values[i] = values[i]; } } -void tJoints::SetValues(const float *values, int ndofs){ - if (ndofs >= 0){ +void tJoints::SetValues(const float *values, int ndofs) +{ + if (ndofs >= 0) + { _nDOFs = qMin(ndofs, RDK_SIZE_JOINTS_MAX); } - for (int i=0; i<_nDOFs; i++){ + + for (int i = 0; i < _nDOFs; i++) + { _Values[i] = values[i]; } } -int tJoints::GetValues(double *values){ - for (int i=0; i<_nDOFs; i++){ + +int tJoints::GetValues(double *values) +{ + for (int i = 0; i < _nDOFs; i++) + { values[i] = _Values[i]; } + return _nDOFs; } -QString tJoints::ToString(const QString &separator, int precision) const { - if (!Valid()){ + +QString tJoints::ToString(const QString &separator, int precision) const +{ + if (!Valid()) + { return "tJoints(Invalid)"; } - QString values("tJoints({"); - if (_nDOFs <= 0){ - return values; - } - values.append(QString::number(_Values[0],'f',precision)); - for (int i=1; i<_nDOFs; i++){ - values.append(separator); - values.append(QString::number(_Values[i],'f',precision)); + + QString values; + for (int i = 0; i < _nDOFs; i++) + { + if (i > 0) + { + values.append(separator); + } + values.append(QString::number(_Values[i], 'f', precision)); } - values.append("} , " + QString::number(_nDOFs) + ")"); return values; } -bool tJoints::FromString(const QString &str){ - QStringList jnts_list = QString(str).replace(";",",").replace("\t",",").split(",", QString::SkipEmptyParts); - _nDOFs = qMin(jnts_list.length(), RDK_SIZE_JOINTS_MAX); - for (int i=0; i<_nDOFs; i++){ - QString stri(jnts_list.at(i)); - _Values[i] = stri.trimmed().toDouble(); + +bool tJoints::FromString(const QString &str) +{ + const QLatin1Char separator(','); + + QString s = str; + s.replace(QLatin1Char(';'), separator); + s.replace(QLatin1Char('\t'), separator); + +#if (QT_VERSION < QT_VERSION_CHECK(5, 14, 0)) + const QString::SplitBehavior behavior = QString::SkipEmptyParts; +#else + const Qt::SplitBehavior behavior = Qt::SkipEmptyParts; +#endif + + QStringList jointList = s.split(separator, behavior); + _nDOFs = qMin(jointList.length(), RDK_SIZE_JOINTS_MAX); + for (int i = 0; i < _nDOFs; i++) + { + _Values[i] = jointList[i].trimmed().toDouble(); } return true; } -int tJoints::Length() const { +int tJoints::Length() const +{ return _nDOFs; } -void tJoints::setLength(int new_length) { - if (new_length >= 0 && new_length < _nDOFs){ +void tJoints::setLength(int new_length) +{ + if (new_length >= 0 && new_length < _nDOFs) + { _nDOFs = new_length; } } -bool tJoints::Valid() const { +bool tJoints::Valid() const +{ return _nDOFs > 0; } //--------------------------------------------------------------------- - - - - -Mat transl(double x, double y, double z){ +//----------------------------------- Mat class ----------------------- +Mat transl(double x, double y, double z) +{ return Mat::transl(x,y,z); } -Mat rotx(double rx){ +Mat rotx(double rx) +{ return Mat::rotx(rx); } -Mat roty(double ry){ +Mat roty(double ry) +{ return Mat::roty(ry); } -Mat rotz(double rz){ +Mat rotz(double rz) +{ return Mat::rotz(rz); } -Mat::Mat() : QMatrix4x4() { - _valid = true; +Mat::Mat() + : QMatrix4x4() + , _valid(true) +{ setToIdentity(); } -Mat::Mat(bool valid) : QMatrix4x4() { - _valid = valid; + +Mat::Mat(bool valid) + : QMatrix4x4() + , _valid(valid) +{ setToIdentity(); } -Mat::Mat(const QMatrix4x4 &matrix) : QMatrix4x4(matrix) { - // just copy - _valid = true; -} -Mat::Mat(const Mat &matrix) : QMatrix4x4(matrix) { - // just copy - _valid = matrix._valid; +Mat::Mat(const QMatrix4x4 &matrix) + : QMatrix4x4(matrix) + , _valid(true) +{ } -Mat::Mat(double nx, double ox, double ax, double tx, double ny, double oy, double ay, double ty, double nz, double oz, double az, double tz) : - QMatrix4x4(nx, ox, ax, tx, ny, oy, ay, ty, nz, oz, az, tz, 0,0,0,1) +Mat::Mat(const Mat &matrix) + : QMatrix4x4(matrix) + , _valid(matrix._valid) { - _valid = true; } -Mat::Mat(const double v[16]) : - QMatrix4x4(v[0], v[4], v[8], v[12], v[1], v[5], v[9], v[13], v[2], v[6], v[10], v[14], v[3], v[7], v[11], v[15]) + +Mat::Mat(double nx, double ox, double ax, double tx, + double ny, double oy, double ay, double ty, + double nz, double oz, double az, double tz) + : QMatrix4x4(nx, ox, ax, tx, ny, oy, ay, ty, nz, oz, az, tz, 0.0, 0.0, 0.0, 1.0) + , _valid(true) { - _valid = true; } -Mat::Mat(const float v[16]) : - QMatrix4x4(v[0], v[4], v[8], v[12], v[1], v[5], v[9], v[13], v[2], v[6], v[10], v[14], v[3], v[7], v[11], v[15]) + +Mat::Mat(const double v[16]) + : QMatrix4x4(v[0], v[4], v[8], v[12], v[1], v[5], v[9], v[13], + v[2], v[6], v[10], v[14], v[3], v[7], v[11], v[15]) + , _valid(true) { - _valid = true; } - - -Mat::~Mat(){ - +Mat::Mat(const float v[16]) + : QMatrix4x4(v[0], v[4], v[8], v[12], v[1], v[5], v[9], v[13], + v[2], v[6], v[10], v[14], v[3], v[7], v[11], v[15]) + , _valid(true) +{ } +Mat::~Mat() +{ +} -void Mat::VX(tXYZ xyz) const { +void Mat::VX(tXYZ xyz) const +{ xyz[0] = Get(0, 0); xyz[1] = Get(1, 0); xyz[2] = Get(2, 0); } -void Mat::VY(tXYZ xyz) const { + +void Mat::VY(tXYZ xyz) const +{ xyz[0] = Get(0, 1); xyz[1] = Get(1, 1); xyz[2] = Get(2, 1); } -void Mat::VZ(tXYZ xyz) const { + +void Mat::VZ(tXYZ xyz) const +{ xyz[0] = Get(0, 2); xyz[1] = Get(1, 2); xyz[2] = Get(2, 2); } -void Mat::Pos(tXYZ xyz) const { + +void Mat::Pos(tXYZ xyz) const +{ xyz[0] = Get(0, 3); xyz[1] = Get(1, 3); xyz[2] = Get(2, 3); } -void Mat::setVX(double x, double y, double z){ + +void Mat::setVX(double x, double y, double z) +{ Set(0,0, x); Set(1,0, y); Set(2,0, z); } -void Mat::setVY(double x, double y, double z){ + +void Mat::setVY(double x, double y, double z) +{ Set(0,1, x); Set(1,1, y); Set(2,1, z); } -void Mat::setVZ(double x, double y, double z){ +void Mat::setVZ(double x, double y, double z) +{ Set(0,2, x); Set(1,2, y); Set(2,2, z); } -void Mat::setPos(double x, double y, double z){ +void Mat::setPos(double x, double y, double z) +{ Set(0,3, x); Set(1,3, y); Set(2,3, z); } -void Mat::setVX(double xyz[3]){ + +void Mat::setVX(double xyz[3]) +{ Set(0,0, xyz[0]); Set(1,0, xyz[1]); Set(2,0, xyz[2]); } -void Mat::setVY(double xyz[3]){ + +void Mat::setVY(double xyz[3]) +{ Set(0,1, xyz[0]); Set(1,1, xyz[1]); Set(2,1, xyz[2]); } -void Mat::setVZ(double xyz[3]){ + +void Mat::setVZ(double xyz[3]) +{ Set(0,2, xyz[0]); Set(1,2, xyz[1]); Set(2,2, xyz[2]); } -void Mat::setPos(double xyz[3]){ + +void Mat::setPos(double xyz[3]) +{ Set(0,3, xyz[0]); Set(1,3, xyz[1]); Set(2,3, xyz[2]); } -void Mat::Set(int i, int j, double value){ +void Mat::Set(int i, int j, double value) +{ QVector4D rw(this->row(i)); rw[j] = value; setRow(i, rw); - // the following should not crash!! - //float **dt_ok = (float**) data(); - //dt_ok[i][j] = value; } -double Mat::Get(int i, int j) const{ +double Mat::Get(int i, int j) const +{ return row(i)[j]; - // the following hsould be allowed!! - //return ((const float**)data())[i][j]; } - -Mat Mat::inv() const{ +Mat Mat::inv() const +{ return this->inverted(); } -bool Mat::isHomogeneous() const { +bool Mat::isHomogeneous() const +{ const bool debug_info = false; tXYZ vx, vy, vz; const double tol = 1e-7; VX(vx); VY(vy); VZ(vz); - if (fabs((double) DOT(vx,vy)) > tol){ - if (debug_info){ + + if (fabs((double) DOT(vx,vy)) > tol) + { + if (debug_info) + { qDebug() << "Vector X and Y are not perpendicular!"; } return false; - } else if (fabs((double) DOT(vx,vz)) > tol){ - if (debug_info){ + } + else if (fabs((double) DOT(vx,vz)) > tol) + { + if (debug_info) + { qDebug() << "Vector X and Z are not perpendicular!"; } return false; - } else if (fabs((double) DOT(vy,vz)) > tol){ - if (debug_info){ + } + else if (fabs((double) DOT(vy,vz)) > tol) + { + if (debug_info) + { qDebug() << "Vector Y and Z are not perpendicular!"; } return false; - } else if (fabs((double) (NORM(vx)-1.0)) > tol){ - if (debug_info){ + } + else if (fabs((double) (NORM(vx)-1.0)) > tol) + { + if (debug_info) + { qDebug() << "Vector X is not unitary! " << NORM(vx); } return false; - } else if (fabs((double) (NORM(vy)-1.0)) > tol){ - if (debug_info){ + } + else if (fabs((double) (NORM(vy)-1.0)) > tol) + { + if (debug_info) + { qDebug() << "Vector Y is not unitary! " << NORM(vy); } return false; - } else if (fabs((double) (NORM(vz)-1.0)) > tol){ - if (debug_info){ + } + else if (fabs((double) (NORM(vz)-1.0)) > tol) + { + if (debug_info) + { qDebug() << "Vector Z is not unitary! " << NORM(vz); } return false; @@ -351,15 +462,13 @@ bool Mat::isHomogeneous() const { return true; } -bool Mat::MakeHomogeneous(){ +bool Mat::MakeHomogeneous() +{ tXYZ vx, vy, vz; VX(vx); VY(vy); VZ(vz); bool is_homogeneous = isHomogeneous(); - //if (is_homogeneous){ - // return false; - //} NORMALIZE(vx); CROSS(vz, vx, vy); @@ -376,27 +485,32 @@ bool Mat::MakeHomogeneous(){ return !is_homogeneous; } - -//---------------------------------------------------- - -void Mat::ToXYZRPW(tXYZWPR xyzwpr) const{ +void Mat::ToXYZRPW(tXYZWPR xyzwpr) const +{ double x = Get(0,3); double y = Get(1,3); double z = Get(2,3); double w, p, r; - if (Get(2,0) > (1.0 - 1e-6)){ + + if (Get(2,0) > (1.0 - 1e-6)) + { p = -M_PI*0.5; r = 0; w = atan2(-Get(1,2), Get(1,1)); - } else if (Get(2,0) < -1.0 + 1e-6){ + } + else if (Get(2,0) < -1.0 + 1e-6) + { p = 0.5*M_PI; r = 0; w = atan2(Get(1,2),Get(1,1)); - } else { + } + else + { p = atan2(-Get(2, 0), sqrt(Get(0, 0) * Get(0, 0) + Get(1, 0) * Get(1, 0))); w = atan2(Get(1, 0), Get(0, 0)); r = atan2(Get(2, 1), Get(2, 2)); } + xyzwpr[0] = x; xyzwpr[1] = y; xyzwpr[2] = z; @@ -405,34 +519,49 @@ void Mat::ToXYZRPW(tXYZWPR xyzwpr) const{ xyzwpr[5] = w*180.0/M_PI; } -QString Mat::ToString(const QString &separator, int precision, bool xyzwpr_only) const { - if (!Valid()){ +QString Mat::ToString(const QString &separator, int precision, bool xyzwpr_only) const +{ + if (!Valid()) + { return "Mat(Invalid)"; } + QString str; - if (!isHomogeneous()){ + if (!isHomogeneous()) + { str.append("Warning!! Pose is not homogeneous! Use Mat::MakeHomogeneous() to make this matrix homogeneous\n"); } - str.append("Mat(XYZRPW_2_Mat("); + + //str.append("Mat(XYZRPW_2_Mat("); tXYZWPR xyzwpr; ToXYZRPW(xyzwpr); - str.append(QString::number(xyzwpr[0],'f',precision)); - for (int i=1; i<6; i++){ - str.append(separator); - str.append(QString::number(xyzwpr[i],'f',precision)); + + for (int i = 0; i < 6; i++) + { + if (i > 0) + { + str.append(separator); + } + str.append(QString::number(xyzwpr[i], 'f', precision)); } - str.append("))"); + //str.append("))"); - if (xyzwpr_only){ + if (xyzwpr_only) + { return str; } + str.append("\n"); - for (int i=0; i<4; i++){ + + for (int i = 0; i < 4; i++) + { str.append("["); - for (int j=0; j<4; j++){ + for (int j = 0; j < 4; j++) + { str.append(QString::number(row(i)[j], 'f', precision)); - if (j < 3){ + if (j < 3) + { str.append(separator); } } @@ -441,27 +570,41 @@ QString Mat::ToString(const QString &separator, int precision, bool xyzwpr_only) return str; } -bool Mat::FromString(const QString &pose_str){ - QStringList values_list = QString(pose_str).replace(";",",").replace("\t",",").split(",", QString::SkipEmptyParts); +bool Mat::FromString(const QString &pose_str) +{ +#if (QT_VERSION < QT_VERSION_CHECK(5, 14, 0)) + const QString::SplitBehavior behavior = QString::SkipEmptyParts; +#else + const Qt::SplitBehavior behavior = Qt::SkipEmptyParts; +#endif + + QStringList values_list = QString(pose_str).replace(";",",").replace("\t",",").split(",", behavior); int nvalues = qMin(values_list.length(), 6); tXYZWPR xyzwpr; - for (int i=0; i<6; i++){ + for (int i = 0; i < 6; i++) + { xyzwpr[i] = 0.0; } - if (nvalues < 6){ + + if (nvalues < 6) + { FromXYZRPW(xyzwpr); return false; } - for (int i=0; iSet(i,j, newmat.Get(i,j)); } } } -const double* Mat::ValuesD() const { - double* _ddata16_non_const = (double*) _ddata16; - for(int i=0; i<16; ++i){ - _ddata16_non_const[i] = constData()[i]; +const double* Mat::ValuesD() const +{ + for(int i = 0; i < 16; ++i) + { + _ddata16[i] = constData()[i]; } return _ddata16; } -const float* Mat::ValuesF() const { + +const float* Mat::ValuesF() const +{ return constData(); } #ifdef ROBODK_API_FLOATS -const float* Mat::Values() const { +const float* Mat::Values() const +{ return constData(); } #else -const double* Mat::Values() const { +const double* Mat::Values() const +{ return ValuesD(); } - #endif - - -void Mat::Values(double data[16]) const{ - for(int i=0; i<16; ++i){ +void Mat::Values(double data[16]) const +{ + for(int i = 0; i < 16; ++i) + { data[i] = constData()[i]; } } -void Mat::Values(float data[16]) const{ - for(int i=0; i<16; ++i){ + +void Mat::Values(float data[16]) const +{ + for(int i = 0; i < 16; ++i) + { data[i] = constData()[i]; } } -bool Mat::Valid() const{ + +bool Mat::Valid() const +{ return _valid; } -Mat Mat::transl(double x, double y, double z){ +Mat Mat::transl(double x, double y, double z) +{ Mat mat; mat.setToIdentity(); mat.setPos(x, y, z); return mat; } -Mat Mat::rotx(double rx){ +Mat Mat::rotx(double rx) +{ double cx = cos(rx); double sx = sin(rx); return Mat(1, 0, 0, 0, 0, cx, -sx, 0, 0, sx, cx, 0); } -Mat Mat::roty(double ry){ +Mat Mat::roty(double ry) +{ double cy = cos(ry); double sy = sin(ry); return Mat(cy, 0, sy, 0, 0, 1, 0, 0, -sy, 0, cy, 0); } -Mat Mat::rotz(double rz){ +Mat Mat::rotz(double rz) +{ double cz = cos(rz); double sz = sin(rz); return Mat(cz, -sz, 0, 0, sz, cz, 0, 0, 0, 0, 1, 0); } - - - //--------------------------------------------------------------------------------------------------- //--------------------------------------------------------------------------------------------------- //--------------------------------------------------------------------------------------------------- //--------------------------------------------------------------------------------------------------- /////////////////////////////////// Item CLASS //////////////////////////////////////////////////// -Item::Item(RoboDK *rdk, quint64 ptr, qint32 type) { +Item::Item(RoboDK *rdk, quint64 ptr, qint32 type) +{ _RDK = rdk; _PTR = ptr; _TYPE = type; } -Item::Item(const Item &other) { + +Item::Item(const Item &other) +{ _RDK = other._RDK; _PTR = other._PTR; _TYPE = other._TYPE; } -Item::~Item(){ +Item::~Item() +{ } -QString Item::ToString() const { - if (this->Valid()){ +QString Item::ToString() const +{ + if (this->Valid()) + { return QString("Item(&RDK, %1, %2); // %3").arg(_PTR).arg(_TYPE).arg(Name()); } return "Item(Invalid)"; @@ -583,14 +747,16 @@ QString Item::ToString() const { /// Returns the RoboDK link Robolink(). /// /// -RoboDK* Item::RDK(){ +RoboDK* Item::RDK() +{ return _RDK; } /// /// Create a new communication link for RoboDK. Use this for robots if you use a multithread application running multiple robots at the same time. /// -void Item::NewLink(){ +void Item::NewLink() +{ _RDK = new RoboDK(); } @@ -599,7 +765,8 @@ void Item::NewLink(){ /// Returns the type of an item (robot, object, target, reference frame, ...) /// /// -int Item::Type() const{ +int Item::Type() const +{ _RDK->_check_connection(); _RDK->_send_Line("G_Item_Type"); _RDK->_send_Item(this); @@ -614,14 +781,16 @@ int Item::Type() const{ /// Save a station or object to a file /// /// -void Item::Save(const QString &filename){ +void Item::Save(const QString &filename) +{ _RDK->Save(filename, this); } /// /// Deletes an item and its childs from the station. /// -void Item::Delete(){ +void Item::Delete() +{ _RDK->_check_connection(); _RDK->_send_Line("Remove"); _RDK->_send_Item(this); @@ -634,8 +803,10 @@ void Item::Delete(){ /// Checks if the item is valid. An invalid item will be returned by an unsuccessful function call. /// /// true if valid, false if invalid -bool Item::Valid(bool check_deleted) const { - if (check_deleted){ +bool Item::Valid(bool check_deleted) const +{ + if (check_deleted) + { return Type() > 0; } return _PTR != 0; @@ -644,7 +815,8 @@ bool Item::Valid(bool check_deleted) const { /// Attaches the item to a new parent while maintaining the relative position with its parent. The absolute position is changed. /// /// -void Item::setParent(Item parent){ +void Item::setParent(Item parent) +{ _RDK->_check_connection(); _RDK->_send_Line("S_Parent"); _RDK->_send_Item(this); @@ -657,7 +829,8 @@ void Item::setParent(Item parent){ /// The relationship between this item and its parent is changed to maintain the abosolute position. /// /// parent item to attach this item -void Item::setParentStatic(Item parent) { +void Item::setParentStatic(Item parent) +{ _RDK->_check_connection(); _RDK->_send_Line("S_Parent_Static"); _RDK->_send_Item(this); @@ -669,7 +842,8 @@ void Item::setParentStatic(Item parent) { /// Attach the closest object to the tool. Returns the item that was attached. /// /// Attached item -Item Item::AttachClosest() { +Item Item::AttachClosest() +{ _RDK->_check_connection(); _RDK->_send_Line("Attach_Closest"); _RDK->_send_Item(this); @@ -682,7 +856,8 @@ Item Item::AttachClosest() { /// Detach the closest object attached to the tool (see also setParentStatic). /// /// Detached item -Item Item::DetachClosest(Item parent) { +Item Item::DetachClosest(Item parent) +{ _RDK->_check_connection(); _RDK->_send_Line("Detach_Closest"); _RDK->_send_Item(this); @@ -695,7 +870,8 @@ Item Item::DetachClosest(Item parent) { /// /// Detach any object attached to a tool. /// -void Item::DetachAll(Item parent) { +void Item::DetachAll(Item parent) +{ _RDK->_check_connection(); _RDK->_send_Line("Detach_All"); _RDK->_send_Item(this); @@ -708,7 +884,8 @@ void Item::DetachAll(Item parent) { /// Return the parent item of this item /// /// Parent item -Item Item::Parent() const { +Item Item::Parent() const +{ _RDK->_check_connection(); _RDK->_send_Line("G_Parent"); _RDK->_send_Item(this); @@ -723,7 +900,8 @@ Item Item::Parent() const { /// Returns a list of the item childs that are attached to the provided item. /// /// item x n -> list of child items -QList Item::Childs() const { +QList Item::Childs() const +{ _RDK->_check_connection(); _RDK->_send_Line("G_Childs"); _RDK->_send_Item(this); @@ -741,7 +919,8 @@ QList Item::Childs() const { /// Returns 1 if the item is visible, otherwise, returns 0. /// /// true if visible, false if not visible -bool Item::Visible() const { +bool Item::Visible() const +{ _RDK->_check_connection(); _RDK->_send_Line("G_Visible"); _RDK->_send_Item(this); @@ -754,7 +933,8 @@ bool Item::Visible() const { /// /// /// srt the visible reference frame (1) or not visible (0) -void Item::setVisible(bool visible, int visible_frame){ +void Item::setVisible(bool visible, int visible_frame) +{ if (visible_frame < 0) { visible_frame = visible ? 1 : 0; @@ -771,7 +951,8 @@ void Item::setVisible(bool visible, int visible_frame){ /// Returns the name of an item. The name of the item is always displayed in the RoboDK station tree /// /// name of the item -QString Item::Name() const { +QString Item::Name() const +{ _RDK->_check_connection(); _RDK->_send_Line("G_Name"); _RDK->_send_Item(this); @@ -784,7 +965,8 @@ QString Item::Name() const { /// Set the name of a RoboDK item. /// /// -void Item::setName(const QString &name){ +void Item::setName(const QString &name) +{ _RDK->_check_connection(); _RDK->_send_Line("S_Name"); _RDK->_send_Item(this); @@ -799,7 +981,8 @@ void Item::setName(const QString &name){ /// If a robot is provided, it will set the pose of the end efector. /// /// 4x4 homogeneous matrix -void Item::setPose(const Mat pose){ +void Item::setPose(const Mat pose) +{ _RDK->_check_connection(); _RDK->_send_Line("S_Hlocal"); _RDK->_send_Item(this); @@ -812,7 +995,8 @@ void Item::setPose(const Mat pose){ /// If a robot is provided, it will get the pose of the end efector /// /// 4x4 homogeneous matrix (pose) -Mat Item::Pose() const { +Mat Item::Pose() const +{ _RDK->_check_connection(); _RDK->_send_Line("G_Hlocal"); _RDK->_send_Item(this); @@ -825,7 +1009,8 @@ Mat Item::Pose() const { /// Sets the position (pose) the object geometry with respect to its own reference frame. This procedure works for tools and objects. /// /// 4x4 homogeneous matrix -void Item::setGeometryPose(const Mat pose){ +void Item::setGeometryPose(const Mat pose) +{ _RDK->_check_connection(); _RDK->_send_Line("S_Hgeom"); _RDK->_send_Item(this); @@ -837,7 +1022,8 @@ void Item::setGeometryPose(const Mat pose){ /// Returns the position (pose) the object geometry with respect to its own reference frame. This procedure works for tools and objects. /// /// 4x4 homogeneous matrix (pose) -Mat Item::GeometryPose(){ +Mat Item::GeometryPose() +{ _RDK->_check_connection(); _RDK->_send_Line("G_Hgeom"); _RDK->_send_Item(this); @@ -876,7 +1062,8 @@ Mat Item::Htool(){ /// Returns the tool pose of an item. If a robot is provided it will get the tool pose of the active tool held by the robot. /// /// 4x4 homogeneous matrix (pose) -Mat Item::PoseTool(){ +Mat Item::PoseTool() +{ _RDK->_check_connection(); _RDK->_send_Line("G_Tool"); _RDK->_send_Item(this); @@ -889,7 +1076,8 @@ Mat Item::PoseTool(){ /// Returns the reference frame pose of an item. If a robot is provided it will get the tool pose of the active reference frame used by the robot. /// /// 4x4 homogeneous matrix (pose) -Mat Item::PoseFrame(){ +Mat Item::PoseFrame() +{ _RDK->_check_connection(); _RDK->_send_Line("G_Frame"); _RDK->_send_Item(this); @@ -903,7 +1091,8 @@ Mat Item::PoseFrame(){ /// If "frame" is an item, it links the robot to the frame item. If frame is a pose, it updates the linked pose of the robot frame (with respect to the robot reference frame). /// /// 4x4 homogeneous matrix (pose) -void Item::setPoseFrame(const Mat frame_pose){ +void Item::setPoseFrame(const Mat frame_pose) +{ _RDK->_check_connection(); _RDK->_send_Line("S_Frame"); _RDK->_send_Pose(frame_pose); @@ -916,7 +1105,8 @@ void Item::setPoseFrame(const Mat frame_pose){ /// If the item is a tool, it links the robot to the tool item.If tool is a pose, it updates the current robot TCP. /// /// 4x4 homogeneous matrix (pose) -void Item::setPoseFrame(const Item frame_item){ +void Item::setPoseFrame(const Item frame_item) +{ _RDK->_check_connection(); _RDK->_send_Line("S_Frame_ptr"); _RDK->_send_Item(frame_item); @@ -929,7 +1119,8 @@ void Item::setPoseFrame(const Item frame_item){ /// If the item is a tool, it links the robot to the tool item.If tool is a pose, it updates the current robot TCP. /// /// 4x4 homogeneous matrix (pose) -void Item::setPoseTool(const Mat tool_pose){ +void Item::setPoseTool(const Mat tool_pose) +{ _RDK->_check_connection(); _RDK->_send_Line("S_Tool"); _RDK->_send_Pose(tool_pose); @@ -942,7 +1133,8 @@ void Item::setPoseTool(const Mat tool_pose){ /// If the item is a tool, it links the robot to the tool item.If tool is a pose, it updates the current robot TCP. /// /// Tool item -void Item::setPoseTool(const Item tool_item){ +void Item::setPoseTool(const Item tool_item) +{ _RDK->_check_connection(); _RDK->_send_Line("S_Tool_ptr"); _RDK->_send_Item(tool_item); @@ -954,7 +1146,8 @@ void Item::setPoseTool(const Item tool_item){ /// Sets the global position (pose) of an item. For example, the position of an object/frame/target with respect to the station origin. /// /// 4x4 homogeneous matrix (pose) -void Item::setPoseAbs(const Mat pose){ +void Item::setPoseAbs(const Mat pose) +{ _RDK->_check_connection(); _RDK->_send_Line("S_Hlocal_Abs"); _RDK->_send_Item(this); @@ -967,7 +1160,8 @@ void Item::setPoseAbs(const Mat pose){ /// Returns the global position (pose) of an item. For example, the position of an object/frame/target with respect to the station origin. /// /// 4x4 homogeneous matrix (pose) -Mat Item::PoseAbs(){ +Mat Item::PoseAbs() +{ _RDK->_check_connection(); _RDK->_send_Line("G_Hlocal_Abs"); _RDK->_send_Item(this); @@ -980,7 +1174,8 @@ Mat Item::PoseAbs(){ /// Set the color of an object, tool or robot. /// A color must in the format COLOR=[R,G,B,(A=1)] where all values range from 0 to 1. /// -void Item::setColor(double colorRGBA[4]){ +void Item::setColor(double colorRGBA[4]) +{ _RDK->_check_connection(); _RDK->_send_Line("S_Color"); _RDK->_send_Item(this); @@ -1000,7 +1195,8 @@ void Item::setColor(double colorRGBA[4]){ /// The scale can be uniform (if scale is a float value) or per axis (if scale is a vector). /// /// scale to apply as scale or [scale_x, scale_y, scale_z] -void Item::Scale(double scale){ +void Item::Scale(double scale) +{ double scale_xyz[3]; scale_xyz[0] = scale; scale_xyz[1] = scale; @@ -1013,7 +1209,8 @@ void Item::Scale(double scale){ /// The scale can be uniform (if scale is a float value) or per axis (if scale is a vector). /// /// scale to apply as [scale_x, scale_y, scale_z] -void Item::Scale(double scale_xyz[3]){ +void Item::Scale(double scale_xyz[3]) +{ _RDK->_check_connection(); _RDK->_send_Line("Scale"); _RDK->_send_Item(this); @@ -1021,6 +1218,43 @@ void Item::Scale(double scale_xyz[3]){ _RDK->_check_status(); } +GetPointsResult Item::GetPoints(int featureType, int featureId) +{ + GetPointsResult result; + result.featureType = RoboDK::FEATURE_NONE; + result.featureId = 0; + result.points = nullptr; + + if (featureType >= RoboDK::FEATURE_HOVER_OBJECT_MESH) + { + return result; + } + + _RDK->_check_connection(); + _RDK->_send_Line("G_ObjPoint"); + _RDK->_send_Item(this); + _RDK->_send_Int(featureType); + _RDK->_send_Int(featureId); + _RDK->_recv_Matrix2D(&result.points); + result.featureName = _RDK->_recv_Line(); + result.featureType = featureType; + result.featureId = featureId; + _RDK->_check_status(); + return result; +} + +bool Item::SelectedFeature(int& featureType, int& featureId) +{ + _RDK->_check_connection(); + _RDK->_send_Line("G_ObjSelection"); + _RDK->_send_Item(this); + int isSelected = _RDK->_recv_Int(); + featureType = _RDK->_recv_Int(); + featureId = _RDK->_recv_Int(); + _RDK->_check_status(); + return isSelected != 0; +} + @@ -1044,7 +1278,7 @@ Item Item::setMachiningParameters(QString ncfile, Item part_obj, QString options _RDK->_TIMEOUT = 3600 * 1000; Item program = _RDK->_recv_Item(); _RDK->_TIMEOUT = ROBODK_API_TIMEOUT; - _RDK->_recv_Int(); + _RDK->_recv_Int(); // Status (unused) _RDK->_check_status(); return program; } @@ -1052,7 +1286,8 @@ Item Item::setMachiningParameters(QString ncfile, Item part_obj, QString options /// /// Sets a target as a cartesian target. A cartesian target moves to cartesian coordinates. /// -void Item::setAsCartesianTarget(){ +void Item::setAsCartesianTarget() +{ _RDK->_check_connection(); _RDK->_send_Line("S_Target_As_RT"); _RDK->_send_Item(this); @@ -1062,7 +1297,8 @@ void Item::setAsCartesianTarget(){ /// /// Sets a target as a joint target. A joint target moves to a joints position without regarding the cartesian coordinates. /// -void Item::setAsJointTarget(){ +void Item::setAsJointTarget() +{ _RDK->_check_connection(); _RDK->_send_Line("S_Target_As_JT"); _RDK->_send_Item(this); @@ -1072,7 +1308,8 @@ void Item::setAsJointTarget(){ /// /// Returns True if a target is a joint target (green icon). Otherwise, the target is a Cartesian target (red icon). /// -bool Item::isJointTarget() const { +bool Item::isJointTarget() const +{ _RDK->_check_connection(); _RDK->_send_Line("Target_Is_JT"); _RDK->_send_Item(this); @@ -1087,7 +1324,8 @@ bool Item::isJointTarget() const { /// Returns the current joints of a robot or the joints of a target. If the item is a cartesian target, it returns the preferred joints (configuration) to go to that cartesian position. /// /// double x n -> joints matrix -tJoints Item::Joints() const { +tJoints Item::Joints() const +{ tJoints jnts; _RDK->_check_connection(); _RDK->_send_Line("G_Thetas"); @@ -1103,7 +1341,8 @@ tJoints Item::Joints() const { /// Returns the home joints of a robot. These joints can be manually set in the robot "Parameters" menu, then select "Set home position" /// /// double x n -> joints array -tJoints Item::JointsHome() const { +tJoints Item::JointsHome() const +{ tJoints jnts; _RDK->_check_connection(); _RDK->_send_Line("G_Home"); @@ -1118,7 +1357,8 @@ tJoints Item::JointsHome() const { /// Sets the joint position of the "home" joints for a robot. /// /// -void Item::setJointsHome(const tJoints &jnts){ +void Item::setJointsHome(const tJoints &jnts) +{ _RDK->_check_connection(); _RDK->_send_Line("S_Home"); _RDK->_send_Array(&jnts); @@ -1131,7 +1371,8 @@ void Item::setJointsHome(const tJoints &jnts){ /// /// link index(0 for the robot base, 1 for the first link, ...) /// -Item Item::ObjectLink(int link_id){ +Item Item::ObjectLink(int link_id) +{ _RDK->_check_connection(); _RDK->_send_Line("G_LinkObjId"); _RDK->_send_Item(this); @@ -1146,7 +1387,8 @@ Item Item::ObjectLink(int link_id){ /// /// type of linked object to retrieve /// -Item Item::getLink(int type_linked){ +Item Item::getLink(int type_linked) +{ _RDK->_check_connection(); _RDK->_send_Line("G_LinkType"); _RDK->_send_Item(this); @@ -1161,7 +1403,8 @@ Item Item::getLink(int type_linked){ /// Sets the current joints of a robot or the joints of a target. It the item is a cartesian target, it returns the preferred joints (configuration) to go to that cartesian position. /// /// -void Item::setJoints(const tJoints &jnts){ +void Item::setJoints(const tJoints &jnts) +{ _RDK->_check_connection(); _RDK->_send_Line("S_Thetas"); _RDK->_send_Array(&jnts); @@ -1174,13 +1417,14 @@ void Item::setJoints(const tJoints &jnts){ /// /// /// -void Item::JointLimits(tJoints *lower_limits, tJoints *upper_limits){ +void Item::JointLimits(tJoints *lower_limits, tJoints *upper_limits) +{ _RDK->_check_connection(); _RDK->_send_Line("G_RobLimits"); _RDK->_send_Item(this); _RDK->_recv_Array(lower_limits); _RDK->_recv_Array(upper_limits); - _RDK->_recv_Int(); + _RDK->_recv_Int(); // Joints Type (unused) _RDK->_check_status(); } @@ -1189,7 +1433,8 @@ void Item::JointLimits(tJoints *lower_limits, tJoints *upper_limits){ /// /// /// -void Item::setJointLimits(const tJoints &lower_limits, const tJoints &upper_limits){ +void Item::setJointLimits(const tJoints &lower_limits, const tJoints &upper_limits) +{ _RDK->_check_connection(); _RDK->_send_Line("S_RobLimits"); _RDK->_send_Item(this); @@ -1204,7 +1449,8 @@ void Item::setJointLimits(const tJoints &lower_limits, const tJoints &upper_limi /// If the robot is not provided, the first available robot will be chosen automatically. /// /// Robot item -void Item::setRobot(const Item &robot){ +void Item::setRobot(const Item &robot) +{ _RDK->_check_connection(); _RDK->_send_Line("S_Robot"); _RDK->_send_Item(this); @@ -1219,7 +1465,8 @@ void Item::setRobot(const Item &robot){ /// pose -> TCP as a 4x4 Matrix (pose of the tool frame) /// New tool name /// new item created -Item Item::AddTool(const Mat &tool_pose, const QString &tool_name){ +Item Item::AddTool(const Mat &tool_pose, const QString &tool_name) +{ _RDK->_check_connection(); _RDK->_send_Line("AddToolEmpty"); _RDK->_send_Item(this); @@ -1235,7 +1482,8 @@ Item Item::AddTool(const Mat &tool_pose, const QString &tool_name){ /// /// /// 4x4 homogeneous matrix: pose of the robot flange with respect to the robot base -Mat Item::SolveFK(const tJoints &joints, const Mat *tool, const Mat *ref){ +Mat Item::SolveFK(const tJoints &joints, const Mat *tool, const Mat *ref) +{ _RDK->_check_connection(); _RDK->_send_Line("G_FK"); _RDK->_send_Array(&joints); @@ -1257,7 +1505,8 @@ Mat Item::SolveFK(const tJoints &joints, const Mat *tool, const Mat *ref){ /// /// array of joints /// 3-array -> configuration status as [REAR, LOWERARM, FLIP] -void Item::JointsConfig(const tJoints &joints, tConfig config){ +void Item::JointsConfig(const tJoints &joints, tConfig config) +{ _RDK->_check_connection(); _RDK->_send_Line("G_Thetas_Config"); _RDK->_send_Array(&joints); @@ -1275,7 +1524,8 @@ void Item::JointsConfig(const tJoints &joints, tConfig config){ /// 4x4 matrix -> Optionally provide a tool, otherwise, the robot flange is used. Tip: use robot.PoseTool() to retrieve the active robot tool. /// 4x4 matrix -> Optionally provide a reference, otherwise, the robot base is used. Tip: use robot.PoseFrame() to retrieve the active robot reference frame. /// array of joints -tJoints Item::SolveIK(const Mat &pose, const Mat *tool, const Mat *ref){ +tJoints Item::SolveIK(const Mat &pose, const Mat *tool, const Mat *ref) +{ tJoints jnts; Mat base2flange(pose); if (tool != nullptr){ @@ -1303,12 +1553,15 @@ tJoints Item::SolveIK(const Mat &pose, const Mat *tool, const Mat *ref){ /// 4x4 matrix -> Optionally provide a tool, otherwise, the robot flange is used. Tip: use robot.PoseTool() to retrieve the active robot tool. /// 4x4 matrix -> Optionally provide a reference, otherwise, the robot base is used. Tip: use robot.PoseFrame() to retrieve the active robot reference frame. /// array of joints -tJoints Item::SolveIK(const Mat pose, tJoints joints_approx, const Mat *tool, const Mat *ref){ +tJoints Item::SolveIK(const Mat pose, tJoints joints_approx, const Mat *tool, const Mat *ref) +{ Mat base2flange(pose); - if (tool != nullptr){ + if (tool != nullptr) + { base2flange = pose*tool->inv(); } - if (ref != nullptr){ + if (ref != nullptr) + { base2flange = (*ref) * base2flange; } _RDK->_check_connection(); @@ -1328,13 +1581,16 @@ tJoints Item::SolveIK(const Mat pose, tJoints joints_approx, const Mat *tool, co /// /// 4x4 matrix -> pose of the robot tool with respect to the robot frame /// double x n x m -> joint list (2D matrix) -tMatrix2D* Item::SolveIK_All_Mat2D(const Mat &pose, const Mat *tool, const Mat *ref){ +tMatrix2D* Item::SolveIK_All_Mat2D(const Mat &pose, const Mat *tool, const Mat *ref) +{ tMatrix2D *mat2d = nullptr; Mat base2flange(pose); - if (tool != nullptr){ + if (tool != nullptr) + { base2flange = pose*tool->inv(); } - if (ref != nullptr){ + if (ref != nullptr) + { base2flange = (*ref) * base2flange; } _RDK->_check_connection(); @@ -1345,11 +1601,14 @@ tMatrix2D* Item::SolveIK_All_Mat2D(const Mat &pose, const Mat *tool, const Mat * _RDK->_check_status(); return mat2d; } -QList Item::SolveIK_All(const Mat &pose, const Mat *tool, const Mat *ref){ + +QList Item::SolveIK_All(const Mat &pose, const Mat *tool, const Mat *ref) +{ tMatrix2D *mat2d = SolveIK_All_Mat2D(pose, tool, ref); QList jnts_list; int nsol = Matrix2D_Size(mat2d, 2); - for (int i=0; i Item::SolveIK_All(const Mat &pose, const Mat *tool, const Mat *re /// /// IP of the robot to connect. Leave empty to use the one defined in RoboDK /// status -> true if connected successfully, false if connection failed -bool Item::Connect(const QString &robot_ip){ +bool Item::Connect(const QString &robot_ip) +{ _RDK->_check_connection(); _RDK->_send_Line("Connect"); _RDK->_send_Item(this); @@ -1376,7 +1636,8 @@ bool Item::Connect(const QString &robot_ip){ /// Disconnect from a real robot (when the robot driver is used) /// /// status -> true if disconnected successfully, false if it failed. It can fail if it was previously disconnected manually for example. -bool Item::Disconnect(){ +bool Item::Disconnect() +{ _RDK->_check_connection(); _RDK->_send_Line("Disconnect"); _RDK->_send_Item(this); @@ -1390,15 +1651,19 @@ bool Item::Disconnect(){ /// /// target -> target to move to as a target item (RoboDK target item) /// blocking -> True if we want the instruction to block until the robot finished the movement (default=true) -void Item::MoveJ(const Item &itemtarget, bool blocking){ - if (_TYPE == RoboDK::ITEM_TYPE_PROGRAM){ +void Item::MoveJ(const Item &itemtarget, bool blocking) +{ + if (_TYPE == RoboDK::ITEM_TYPE_PROGRAM) + { _RDK->_check_connection(); _RDK->_send_Line("Add_INSMOVE"); _RDK->_send_Item(itemtarget); _RDK->_send_Item(this); _RDK->_send_Int(1); _RDK->_check_status(); - } else { + } + else + { _RDK->_moveX(&itemtarget, nullptr, nullptr, this, 1, blocking); } } @@ -1408,7 +1673,8 @@ void Item::MoveJ(const Item &itemtarget, bool blocking){ /// /// joints -> joint target to move to. /// blocking -> True if we want the instruction to block until the robot finished the movement (default=true) -void Item::MoveJ(const tJoints &joints, bool blocking){ +void Item::MoveJ(const tJoints &joints, bool blocking) +{ _RDK->_moveX(nullptr, &joints, nullptr, this, 1, blocking); } @@ -1417,7 +1683,8 @@ void Item::MoveJ(const tJoints &joints, bool blocking){ /// /// pose -> pose target to move to. It must be a 4x4 Homogeneous matrix /// blocking -> True if we want the instruction to block until the robot finished the movement (default=true) -void Item::MoveJ(const Mat &target, bool blocking){ +void Item::MoveJ(const Mat &target, bool blocking) +{ _RDK->_moveX(nullptr, nullptr, &target, this, 1, blocking); } @@ -1426,15 +1693,19 @@ void Item::MoveJ(const Mat &target, bool blocking){ /// /// target -> target to move to as a target item (RoboDK target item) /// blocking -> True if we want the instruction to block until the robot finished the movement (default=true) -void Item::MoveL(const Item &itemtarget, bool blocking){ - if (_TYPE == RoboDK::ITEM_TYPE_PROGRAM){ +void Item::MoveL(const Item &itemtarget, bool blocking) +{ + if (_TYPE == RoboDK::ITEM_TYPE_PROGRAM) + { _RDK->_check_connection(); _RDK->_send_Line("Add_INSMOVE"); _RDK->_send_Item(itemtarget); _RDK->_send_Item(this); _RDK->_send_Int(2); _RDK->_check_status(); - } else { + } + else + { _RDK->_moveX(&itemtarget, nullptr, nullptr, this, 2, blocking); } } @@ -1444,7 +1715,8 @@ void Item::MoveL(const Item &itemtarget, bool blocking){ /// /// joints -> joint target to move to. /// blocking -> True if we want the instruction to block until the robot finished the movement (default=true) -void Item::MoveL(const tJoints &joints, bool blocking){ +void Item::MoveL(const tJoints &joints, bool blocking) +{ _RDK->_moveX(nullptr, &joints, nullptr, this, 2, blocking); } @@ -1453,7 +1725,8 @@ void Item::MoveL(const tJoints &joints, bool blocking){ /// /// pose -> pose target to move to. It must be a 4x4 Homogeneous matrix /// blocking -> True if we want the instruction to block until the robot finished the movement (default=true) -void Item::MoveL(const Mat &target, bool blocking){ +void Item::MoveL(const Mat &target, bool blocking) +{ _RDK->_moveX(nullptr, nullptr, &target, this, 2, blocking); } @@ -1463,7 +1736,8 @@ void Item::MoveL(const Mat &target, bool blocking){ /// target -> intermediate target to move to as a target item (RoboDK target item) /// target -> final target to move to as a target item (RoboDK target item) /// blocking -> True if we want the instruction to block until the robot finished the movement (default=true) -void Item::MoveC(const Item &itemtarget1, const Item &itemtarget2, bool blocking){ +void Item::MoveC(const Item &itemtarget1, const Item &itemtarget2, bool blocking) +{ _RDK->_moveC(&itemtarget1, nullptr, nullptr, &itemtarget2, nullptr, nullptr, this, blocking); } @@ -1473,7 +1747,8 @@ void Item::MoveC(const Item &itemtarget1, const Item &itemtarget2, bool blocking /// joints -> intermediate joint target to move to. /// joints -> final joint target to move to. /// blocking -> True if we want the instruction to block until the robot finished the movement (default=true) -void Item::MoveC(const tJoints &joints1, const tJoints &joints2, bool blocking){ +void Item::MoveC(const tJoints &joints1, const tJoints &joints2, bool blocking) +{ _RDK->_moveC(nullptr, &joints1, nullptr, nullptr, &joints2, nullptr, this, blocking); } @@ -1483,7 +1758,8 @@ void Item::MoveC(const tJoints &joints1, const tJoints &joints2, bool blocking){ /// pose -> intermediate pose target to move to. It must be a 4x4 Homogeneous matrix /// pose -> final pose target to move to. It must be a 4x4 Homogeneous matrix /// blocking -> True if we want the instruction to block until the robot finished the movement (default=true) -void Item::MoveC(const Mat &target1, const Mat &target2, bool blocking){ +void Item::MoveC(const Mat &target1, const Mat &target2, bool blocking) +{ _RDK->_moveC(nullptr, nullptr, &target1, nullptr, nullptr, &target2, this, blocking); } @@ -1494,7 +1770,8 @@ void Item::MoveC(const Mat &target1, const Mat &target2, bool blocking){ /// joints -> destination joints /// (optional): maximum joint step in degrees /// collision : returns 0 if the movement is free of collision. Otherwise it returns the number of pairs of objects that collided if there was a collision. -int Item::MoveJ_Test(const tJoints &j1, const tJoints &j2, double minstep_deg){ +int Item::MoveJ_Test(const tJoints &j1, const tJoints &j2, double minstep_deg) +{ _RDK->_check_connection(); _RDK->_send_Line("CollisionMove"); _RDK->_send_Item(this); @@ -1515,7 +1792,8 @@ int Item::MoveJ_Test(const tJoints &j1, const tJoints &j2, double minstep_deg){ /// joints -> destination pose (active tool with respect to the active reference frame) /// (optional): maximum joint step in degrees /// collision : returns 0 if the movement is free of collision. Otherwise it returns the number of pairs of objects that collided if there was a collision. -int Item::MoveL_Test(const tJoints &j1, const Mat &pose2, double minstep_deg){ +int Item::MoveL_Test(const tJoints &j1, const Mat &pose2, double minstep_deg) +{ _RDK->_check_connection(); _RDK->_send_Line("CollisionMoveL"); _RDK->_send_Item(this); @@ -1534,17 +1812,18 @@ int Item::MoveL_Test(const tJoints &j1, const Mat &pose2, double minstep_deg){ /// Sets the speed and/or the acceleration of a robot. /// /// linear speed in mm/s (-1 = no change) -/// linear acceleration in mm/s2 (-1 = no change) /// joint speed in deg/s (-1 = no change) +/// linear acceleration in mm/s2 (-1 = no change) /// joint acceleration in deg/s2 (-1 = no change) -void Item::setSpeed(double speed_linear, double accel_linear, double speed_joints, double accel_joints){ +void Item::setSpeed(double speed_linear, double speed_joints, double accel_linear, double accel_joints) +{ _RDK->_check_connection(); _RDK->_send_Line("S_Speed4"); _RDK->_send_Item(this); double speed_accel[4]; speed_accel[0] = speed_linear; - speed_accel[1] = accel_linear; - speed_accel[2] = speed_joints; + speed_accel[1] = speed_joints; + speed_accel[2] = accel_linear; speed_accel[3] = accel_joints; _RDK->_send_Array(speed_accel, 4); _RDK->_check_status(); @@ -1554,7 +1833,8 @@ void Item::setSpeed(double speed_linear, double accel_linear, double speed_joint /// Sets the robot movement smoothing accuracy (also known as zone data value). /// /// zonedata value (int) (robot dependent, set to -1 for fine movements) -void Item::setRounding(double zonedata){ +void Item::setRounding(double zonedata) +{ _RDK->_check_connection(); _RDK->_send_Line("S_ZoneData"); _RDK->_send_Int((int)(zonedata * 1000.0)); @@ -1566,7 +1846,8 @@ void Item::setRounding(double zonedata){ /// Displays a sequence of joints /// /// joint sequence as a 6xN matrix or instruction sequence as a 7xN matrix -void Item::ShowSequence(tMatrix2D *sequence){ +void Item::ShowSequence(tMatrix2D *sequence) +{ _RDK->_check_connection(); _RDK->_send_Line("Show_Seq"); _RDK->_send_Matrix2D(sequence); @@ -1579,7 +1860,8 @@ void Item::ShowSequence(tMatrix2D *sequence){ /// Checks if a robot or program is currently running (busy or moving) /// /// busy status (true=moving, false=stopped) -bool Item::Busy(){ +bool Item::Busy() +{ _RDK->_check_connection(); _RDK->_send_Line("IsBusy"); _RDK->_send_Item(this); @@ -1592,7 +1874,8 @@ bool Item::Busy(){ /// Stops a program or a robot /// /// -void Item::Stop(){ +void Item::Stop() +{ _RDK->_check_connection(); _RDK->_send_Line("Stop"); _RDK->_send_Item(this); @@ -1603,7 +1886,8 @@ void Item::Stop(){ /// Waits (blocks) until the robot finishes its movement. /// /// timeout -> Max time to wait for robot to finish its movement (in seconds) -void Item::WaitMove(double timeout_sec) const{ +void Item::WaitMove(double timeout_sec) const +{ _RDK->_check_connection(); _RDK->_send_Line("WaitMove"); _RDK->_send_Item(this); @@ -1623,7 +1907,8 @@ void Item::WaitMove(double timeout_sec) const{ /// Sets the accuracy of the robot active or inactive. A robot must have been calibrated to properly use this option. /// /// set to 1 to use the accurate model or 0 to use the nominal model -void Item::setAccuracyActive(int accurate){ +void Item::setAccuracyActive(int accurate) +{ _RDK->_check_connection(); _RDK->_send_Line("S_AbsAccOn"); _RDK->_send_Item(this); @@ -1641,7 +1926,8 @@ void Item::setAccuracyActive(int accurate){ /// /// File path of the program /// success -bool Item::MakeProgram(const QString &filename){ +bool Item::MakeProgram(const QString &filename) +{ _RDK->_check_connection(); _RDK->_send_Line("MakeProg"); _RDK->_send_Item(this); @@ -1650,7 +1936,8 @@ bool Item::MakeProgram(const QString &filename){ QString prog_log_str = _RDK->_recv_Line(); _RDK->_check_status(); bool success = false; - if (prog_status > 1) { + if (prog_status > 1) + { success = true; } return success; // prog_log_str @@ -1661,7 +1948,8 @@ bool Item::MakeProgram(const QString &filename){ /// Use: "PROGRAM_RUN_ON_SIMULATOR" to set the program to run on the simulator only or "PROGRAM_RUN_ON_ROBOT" to force the program to run on the robot. /// /// number of instructions that can be executed -void Item::setRunType(int program_run_type){ +void Item::setRunType(int program_run_type) +{ _RDK->_check_connection(); _RDK->_send_Line("S_ProgRunType"); _RDK->_send_Item(this); @@ -1678,7 +1966,8 @@ void Item::setRunType(int program_run_type){ /// if setRunMode(RUNMODE_RUN_ROBOT) is used together with program.setRunType(PROGRAM_RUN_ON_ROBOT) -> the program will run sequentially on the robot the same way as if we right clicked the program and selected "Run on robot" in the RoboDK GUI /// /// number of instructions that can be executed -int Item::RunProgram(){ +int Item::RunProgram() +{ _RDK->_check_connection(); _RDK->_send_Line("RunProg"); _RDK->_send_Item(this); @@ -1697,12 +1986,16 @@ int Item::RunProgram(){ /// if setRunMode(RUNMODE_RUN_ROBOT) is used together with program.setRunType(PROGRAM_RUN_ON_ROBOT) -> the program will run sequentially on the robot the same way as if we right clicked the program and selected "Run on robot" in the RoboDK GUI /// /// Number of instructions that can be executed -int Item::RunCode(const QString ¶meters){ +int Item::RunCode(const QString ¶meters) +{ _RDK->_check_connection(); - if (parameters.isEmpty()){ + if (parameters.isEmpty()) + { _RDK->_send_Line("RunProg"); _RDK->_send_Item(this); - } else { + } + else + { _RDK->_send_Line("RunProgParam"); _RDK->_send_Item(this); _RDK->_send_Line(parameters); @@ -1717,7 +2010,8 @@ int Item::RunCode(const QString ¶meters){ /// /// /// INSTRUCTION_* variable to specify if the code is a progra -int Item::RunInstruction(const QString &code, int run_type){ +int Item::RunInstruction(const QString &code, int run_type) +{ _RDK->_check_connection(); _RDK->_send_Line("RunCode2"); _RDK->_send_Item(this); @@ -1732,7 +2026,8 @@ int Item::RunInstruction(const QString &code, int run_type){ /// Generates a pause instruction for a robot or a program when generating code. Set it to -1 (default) if you want the robot to stop and let the user resume the program anytime. /// /// Time in milliseconds -void Item::Pause(double time_ms){ +void Item::Pause(double time_ms) +{ _RDK->_check_connection(); _RDK->_send_Line("RunPause"); _RDK->_send_Item(this); @@ -1746,7 +2041,8 @@ void Item::Pause(double time_ms){ /// /// io_var -> digital output (string or number) /// io_value -> value (string or number) -void Item::setDO(const QString &io_var, const QString &io_value){ +void Item::setDO(const QString &io_var, const QString &io_value) +{ _RDK->_check_connection(); _RDK->_send_Line("setDO"); _RDK->_send_Item(this); @@ -1759,7 +2055,8 @@ void Item::setDO(const QString &io_var, const QString &io_value){ /// /// Analog Output /// Value as a string -void Item::setAO(const QString &io_var, const QString &io_value){ +void Item::setAO(const QString &io_var, const QString &io_value) +{ _RDK->_check_connection(); _RDK->_send_Line("setAO"); _RDK->_send_Item(this); @@ -1772,7 +2069,8 @@ void Item::setAO(const QString &io_var, const QString &io_value){ /// Get a Digital Input (DI). This function is only useful when connected to a real robot using the robot driver. It returns a string related to the state of the Digital Input (1=True, 0=False). This function returns an empty string if the script is not executed on the robot. /// /// io_var -> digital input (string or number as a string) -QString Item::getDI(const QString &io_var){ +QString Item::getDI(const QString &io_var) +{ _RDK->_check_connection(); _RDK->_send_Line("getDI"); _RDK->_send_Item(this); @@ -1786,7 +2084,8 @@ QString Item::getDI(const QString &io_var){ /// Get an Analog Input (AI). This function is only useful when connected to a real robot using the robot driver. It returns a string related to the state of the Digital Input (0-1 or other range depending on the robot driver). This function returns an empty string if the script is not executed on the robot. /// /// io_var -> analog input (string or number as a string) -QString Item::getAI(const QString &io_var){ +QString Item::getAI(const QString &io_var) +{ _RDK->_check_connection(); _RDK->_send_Line("getAI"); _RDK->_send_Item(this); @@ -1802,7 +2101,8 @@ QString Item::getAI(const QString &io_var){ /// io_var -> digital output (string or number) /// io_value -> value (string or number) /// int (optional) -> timeout in miliseconds -void Item::waitDI(const QString &io_var, const QString &io_value, double timeout_ms){ +void Item::waitDI(const QString &io_var, const QString &io_value, double timeout_ms) +{ _RDK->_check_connection(); _RDK->_send_Line("waitDI"); _RDK->_send_Item(this); @@ -1822,7 +2122,8 @@ void Item::waitDI(const QString &io_var, const QString &io_value, double timeout /// True if blocking, 0 if it is a non blocking executable trigger /// Command to run through the driver when connected to the robot /// :param name: digital input (string or number) -void Item::customInstruction(const QString &name, const QString &path_run, const QString &path_icon, bool blocking, const QString &cmd_run_on_robot){ +void Item::customInstruction(const QString &name, const QString &path_run, const QString &path_icon, bool blocking, const QString &cmd_run_on_robot) +{ _RDK->_check_connection(); _RDK->_send_Line("InsCustom2"); _RDK->_send_Item(this); @@ -1840,7 +2141,8 @@ void Item::customInstruction(const QString &name, const QString &path_run, const /// Adds a new robot move joint instruction to a program. /// /// target to move to -void Item::addMoveJ(const Item &itemtarget){ +void Item::addMoveJ(const Item &itemtarget) +{ _RDK->_check_connection(); _RDK->_send_Line("Add_INSMOVE"); _RDK->_send_Item(itemtarget); @@ -1853,7 +2155,8 @@ void Item::addMoveJ(const Item &itemtarget){ /// Adds a new robot move linear instruction to a program. /// /// target to move to -void Item::addMoveL(const Item &itemtarget){ +void Item::addMoveL(const Item &itemtarget) +{ _RDK->_check_connection(); _RDK->_send_Line("Add_INSMOVE"); _RDK->_send_Item(itemtarget); @@ -1867,7 +2170,8 @@ void Item::addMoveL(const Item &itemtarget){ /// Show or hide instruction items of a program in the RoboDK tree /// /// -void Item::ShowInstructions(bool visible){ +void Item::ShowInstructions(bool visible) +{ _RDK->_check_connection(); _RDK->_send_Line("Prog_ShowIns"); _RDK->_send_Item(this); @@ -1879,7 +2183,8 @@ void Item::ShowInstructions(bool visible){ /// Show or hide targets of a program in the RoboDK tree /// /// -void Item::ShowTargets(bool visible){ +void Item::ShowTargets(bool visible) +{ _RDK->_check_connection(); _RDK->_send_Line("Prog_ShowTargets"); _RDK->_send_Item(this); @@ -1893,7 +2198,8 @@ void Item::ShowTargets(bool visible){ /// Returns the number of instructions of a program. /// /// -int Item::InstructionCount(){ +int Item::InstructionCount() +{ _RDK->_check_connection(); _RDK->_send_Line("Prog_Nins"); _RDK->_send_Item(this); @@ -1912,7 +2218,8 @@ int Item::InstructionCount(){ /// /// /// -void Item::Instruction(int ins_id, QString &name, int &instype, int &movetype, bool &isjointtarget, Mat &target, tJoints &joints){ +void Item::Instruction(int ins_id, QString &name, int &instype, int &movetype, bool &isjointtarget, Mat &target, tJoints &joints) +{ _RDK->_check_connection(); _RDK->_send_Line("Prog_GIns"); _RDK->_send_Item(this); @@ -1923,7 +2230,8 @@ void Item::Instruction(int ins_id, QString &name, int &instype, int &movetype, b isjointtarget = false; //target = null; //joints = null; - if (instype == RoboDK::INS_TYPE_MOVE) { + if (instype == RoboDK::INS_TYPE_MOVE) + { movetype = _RDK->_recv_Int(); isjointtarget = _RDK->_recv_Int() > 0 ? true : false; target = _RDK->_recv_Pose(); @@ -1942,7 +2250,8 @@ void Item::Instruction(int ins_id, QString &name, int &instype, int &movetype, b /// /// /// -void Item::setInstruction(int ins_id, const QString &name, int instype, int movetype, bool isjointtarget, const Mat &target, const tJoints &joints){ +void Item::setInstruction(int ins_id, const QString &name, int instype, int movetype, bool isjointtarget, const Mat &target, const tJoints &joints) +{ _RDK->_check_connection(); _RDK->_send_Line("Prog_SIns"); _RDK->_send_Item(this); @@ -1965,7 +2274,8 @@ void Item::setInstruction(int ins_id, const QString &name, int instype, int move /// /// the matrix of instructions /// Returns 0 if success -int Item::InstructionList(tMatrix2D *instructions){ +int Item::InstructionList(tMatrix2D *instructions) +{ _RDK->_check_connection(); _RDK->_send_Line("G_ProgInsList"); _RDK->_send_Item(this); @@ -1987,7 +2297,8 @@ int Item::InstructionList(tMatrix2D *instructions){ /// Maximum step in millimeters for linear movements (millimeters). Set to -1 to use the default, as specified in Tools-Options-Motion. /// Maximum step for joint movements (degrees). Set to -1 to use the default, as specified in Tools-Options-Motion. /// 1.0 if there are no problems with the path or less than 1.0 if there is a problem in the path (ratio of problem) -double Item::Update(int collision_check, int timeout_sec, double *out_nins_time_dist, double mm_step, double deg_step){ +double Item::Update(int collision_check, int timeout_sec, double *out_nins_time_dist, double mm_step, double deg_step) +{ _RDK->_check_connection(); _RDK->_send_Line("Update2"); _RDK->_send_Item(this); @@ -2026,7 +2337,8 @@ double Item::Update(int collision_check, int timeout_sec, double *out_nins_time_ /// set to 1 to include the timings between movements, set to 2 to also include the joint speeds (deg/s), set to 3 to also include the accelerations, set to 4 to include all previous information and make the splitting time-based. /// (optional) set the time step in seconds for time based calculation. This value is only used when the result flag is set to 4 (time based). /// Returns 0 if success, otherwise, it will return negative values -int Item::InstructionListJoints(QString &error_msg, tMatrix2D **joint_list, double mm_step, double deg_step, const QString &save_to_file, bool collision_check, int result_flag, double time_step_s){ +int Item::InstructionListJoints(QString &error_msg, tMatrix2D **joint_list, double mm_step, double deg_step, const QString &save_to_file, bool collision_check, int result_flag, double time_step_s) +{ _RDK->_check_connection(); _RDK->_send_Line("G_ProgJointList"); _RDK->_send_Item(this); @@ -2034,10 +2346,13 @@ int Item::InstructionListJoints(QString &error_msg, tMatrix2D **joint_list, doub _RDK->_send_Array(step_mm_deg, 5); _RDK->_TIMEOUT = 3600 * 1000; //joint_list = save_to_file; - if (save_to_file.isEmpty()) { + if (save_to_file.isEmpty()) + { _RDK->_send_Line(""); _RDK->_recv_Matrix2D(joint_list); - } else { + } + else + { _RDK->_send_Line(save_to_file); joint_list = nullptr; } @@ -2056,7 +2371,8 @@ int Item::InstructionListJoints(QString &error_msg, tMatrix2D **joint_list, doub /// item parameter /// value /// -QString Item::setParam(const QString ¶m, const QString &value){ +QString Item::setParam(const QString ¶m, const QString &value) +{ _RDK->_check_connection(); _RDK->_send_Line("ICMD"); _RDK->_send_Item(this); @@ -2067,16 +2383,39 @@ QString Item::setParam(const QString ¶m, const QString &value){ return result; } +void Item::setParam(const QString& param, const QByteArray& value) +{ + _RDK->_check_connection(); + _RDK->_send_Line("S_ItmDataParam"); + _RDK->_send_Item(this); + _RDK->_send_Line(param); + _RDK->_send_Bytes(value); + _RDK->_check_status(); +} + +QByteArray Item::getParam(const QString& param) +{ + _RDK->_check_connection(); + _RDK->_send_Line("G_ItmDataParam"); + _RDK->_send_Item(this); + _RDK->_send_Line(param); + QByteArray result = _RDK->_recv_Bytes(); + _RDK->_check_status(); + return result; +} + /// /// Disconnect from the RoboDK API. This flushes any pending program generation. /// /// -bool Item::Finish(){ +bool Item::Finish() +{ _RDK->Finish(); return true; } -quint64 Item::GetID(){ +quint64 Item::GetID() +{ return _PTR; } @@ -2098,66 +2437,89 @@ quint64 Item::GetID(){ //--------------------------------------------------------------------------------------------------- //--------------------------------------------------------------------------------------------------- /////////////////////////////////// RoboDK CLASS //////////////////////////////////////////////////// -RoboDK::RoboDK(const QString &robodk_ip, int com_port, const QString &args, const QString &path) { +RoboDK::RoboDK(QAbstractSocket* socket, bool fUseExceptions) +{ + _COM = socket; + _USE_EXCPETIONS = fUseExceptions; + _OWN_SOCKET = false; + _TIMEOUT = ROBODK_API_TIMEOUT; + _PROCESS = 0; + _PORT = ROBODK_DEFAULT_PORT; + _ROBODK_BIN = ROBODK_DEFAULT_PATH_BIN; + _connect_smart(); +} + +RoboDK::RoboDK(const QString &robodk_ip, int com_port, const QString &args, const QString &path, bool fUseExceptions) +{ _COM = nullptr; + _USE_EXCPETIONS = fUseExceptions; + _OWN_SOCKET = true; _IP = robodk_ip; _TIMEOUT = ROBODK_API_TIMEOUT; _PROCESS = 0; _PORT = com_port; _ROBODK_BIN = path; - if (_PORT < 0){ + if (_PORT < 0) + { _PORT = ROBODK_DEFAULT_PORT; } - if (_ROBODK_BIN.isEmpty()){ + if (_ROBODK_BIN.isEmpty()) + { _ROBODK_BIN = ROBODK_DEFAULT_PATH_BIN; } _ARGUMENTS = args; - if (com_port > 0){ + if (com_port > 0) + { _ARGUMENTS.append(" /PORT=" + QString::number(com_port)); } _connect_smart(); } -RoboDK::~RoboDK(){ +RoboDK::~RoboDK() +{ _disconnect(); } -quint64 RoboDK::ProcessID(){ +quint64 RoboDK::ProcessID() +{ if (_PROCESS == 0) { QString response = Command("MainProcess_ID"); - _PROCESS = response.toInt(); + _PROCESS = response.toULongLong(); } return _PROCESS; } -quint64 RoboDK::WindowID(){ - qint64 window_id = 0; - if (window_id == 0) { - QString response = Command("MainWindow_ID"); - window_id = response.toInt(); - } - return window_id; +quint64 RoboDK::WindowID() +{ + QString response = Command("MainWindow_ID"); + return response.toULongLong(); } -bool RoboDK::Connected(){ +bool RoboDK::Connected() +{ return _connected(); } -bool RoboDK::Connect(){ +bool RoboDK::Connect() +{ return _connect(); } + /// /// Disconnect from the RoboDK API. This flushes any pending program generation. /// /// -void RoboDK::Disconnect(){ +void RoboDK::Disconnect() +{ _disconnect(); } + /// /// Disconnect from the RoboDK API. This flushes any pending program generation. /// /// -void RoboDK::Finish(){ +void RoboDK::Finish() +{ Disconnect(); } @@ -2169,12 +2531,16 @@ void RoboDK::Finish(){ /// Item name /// Filter by item type RoboDK.ITEM_TYPE_... /// -Item RoboDK::getItem(QString name, int itemtype){ +Item RoboDK::getItem(QString name, int itemtype) +{ _check_connection(); - if (itemtype < 0){ + if (itemtype < 0) + { _send_Line("G_Item"); _send_Line(name); - } else { + } + else + { _send_Line("G_Item2"); _send_Line(name); _send_Int(itemtype); @@ -2190,17 +2556,22 @@ Item RoboDK::getItem(QString name, int itemtype){ /// /// ITEM_TYPE /// -QStringList RoboDK::getItemListNames(int filter){ +QStringList RoboDK::getItemListNames(int filter) +{ _check_connection(); - if (filter < 0) { + if (filter < 0) + { _send_Line("G_List_Items"); - } else { + } + else + { _send_Line("G_List_Items_Type"); _send_Int(filter); } qint32 numitems = _recv_Int(); QStringList listnames; - for (int i = 0; i < numitems; i++) { + for (int i = 0; i < numitems; i++) + { listnames.append(_recv_Line()); } _check_status(); @@ -2213,17 +2584,22 @@ QStringList RoboDK::getItemListNames(int filter){ /// /// ITEM_TYPE /// -QList RoboDK::getItemList(int filter) { +QList RoboDK::getItemList(int filter) +{ _check_connection(); - if (filter < 0) { + if (filter < 0) + { _send_Line("G_List_Items_ptr"); - } else { + } + else + { _send_Line("G_List_Items_Type_ptr"); _send_Int(filter); } int numitems = _recv_Int(); QList listitems; - for (int i = 0; i < numitems; i++) { + for (int i = 0; i < numitems; i++) + { listitems.append(_recv_Item()); } _check_status(); @@ -2239,7 +2615,8 @@ QList RoboDK::getItemList(int filter) { /// Message to pop up /// optionally filter by RoboDK.ITEM_TYPE_* /// -Item RoboDK::ItemUserPick(const QString &message, int itemtype) { +Item RoboDK::ItemUserPick(const QString &message, int itemtype) +{ _check_connection(); _send_Line("PickItem"); _send_Line(message); @@ -2254,7 +2631,8 @@ Item RoboDK::ItemUserPick(const QString &message, int itemtype) { /// /// Shows or raises the RoboDK window /// -void RoboDK::ShowRoboDK() { +void RoboDK::ShowRoboDK() +{ _check_connection(); _send_Line("RAISE"); _check_status(); @@ -2263,7 +2641,8 @@ void RoboDK::ShowRoboDK() { /// /// Hides the RoboDK window /// -void RoboDK::HideRoboDK() { +void RoboDK::HideRoboDK() +{ _check_connection(); _send_Line("HIDE"); _check_status(); @@ -2272,7 +2651,8 @@ void RoboDK::HideRoboDK() { /// /// Closes RoboDK window and finishes RoboDK execution /// -void RoboDK::CloseRoboDK() { +void RoboDK::CloseRoboDK() +{ _check_connection(); _send_Line("QUIT"); _check_status(); @@ -2285,7 +2665,7 @@ QString RoboDK::Version() _check_connection(); _send_Line("Version"); QString appName = _recv_Line(); - _recv_Int(); + _recv_Int(); // Architecture (32- or 64-bit) (unused) QString ver4 = _recv_Line(); QString dateBuild = _recv_Line(); _check_status(); @@ -2298,7 +2678,8 @@ QString RoboDK::Version() /// Set the state of the RoboDK window /// /// -void RoboDK::setWindowState(int windowstate){ +void RoboDK::setWindowState(int windowstate) +{ _check_connection(); _send_Line("S_WindowState"); _send_Int(windowstate); @@ -2310,7 +2691,8 @@ void RoboDK::setWindowState(int windowstate){ /// Update the RoboDK flags. RoboDK flags allow defining how much access the user has to RoboDK features. Use FLAG_ROBODK_* variables to set one or more flags. /// /// state of the window(FLAG_ROBODK_*) -void RoboDK::setFlagsRoboDK(int flags){ +void RoboDK::setFlagsRoboDK(int flags) +{ _check_connection(); _send_Line("S_RoboDK_Rights"); _send_Int(flags); @@ -2322,7 +2704,8 @@ void RoboDK::setFlagsRoboDK(int flags){ /// /// /// -void RoboDK::setFlagsItem(Item item, int flags){ +void RoboDK::setFlagsItem(Item item, int flags) +{ _check_connection(); _send_Line("S_Item_Rights"); _send_Item(item); @@ -2335,7 +2718,8 @@ void RoboDK::setFlagsItem(Item item, int flags){ /// /// /// -int RoboDK::getFlagsItem(Item item){ +int RoboDK::getFlagsItem(Item item) +{ _check_connection(); _send_Line("S_Item_Rights"); _send_Item(item); @@ -2349,7 +2733,8 @@ int RoboDK::getFlagsItem(Item item){ /// /// Message to display /// Set to true to make the message blocking or set to false to make it non blocking -void RoboDK::ShowMessage(const QString &message, bool popup){ +void RoboDK::ShowMessage(const QString &message, bool popup) +{ _check_connection(); if (popup) { @@ -2365,14 +2750,14 @@ void RoboDK::ShowMessage(const QString &message, bool popup){ _send_Line(message); _check_status(); } - } /// /// Makes a copy of an item (same as Ctrl+C), which can be pasted (Ctrl+V) using Paste(). /// /// Item to copy -void RoboDK::Copy(const Item &tocopy){ +void RoboDK::Copy(const Item &tocopy) +{ _check_connection(); _send_Line("Copy"); _send_Item(tocopy); @@ -2384,7 +2769,8 @@ void RoboDK::Copy(const Item &tocopy){ /// /// Item to attach the copied item (optional) /// New item created -Item RoboDK::Paste(const Item *paste_to){ +Item RoboDK::Paste(const Item *paste_to) +{ _check_connection(); _send_Line("Paste"); _send_Item(paste_to); @@ -2399,12 +2785,15 @@ Item RoboDK::Paste(const Item *paste_to){ /// absolute path of the file /// parent to attach. Leave empty for new stations or to load an object at the station root /// Newly added object. Check with item.Valid() for a successful load -Item RoboDK::AddFile(const QString &filename, const Item *parent){ +Item RoboDK::AddFile(const QString &filename, const Item *parent) +{ _check_connection(); _send_Line("Add"); _send_Line(filename); _send_Item(parent); + _TIMEOUT = 3600 * 1000; Item newitem = _recv_Item(); + _TIMEOUT = ROBODK_API_TIMEOUT; _check_status(); return newitem; } @@ -2414,7 +2803,8 @@ Item RoboDK::AddFile(const QString &filename, const Item *parent){ /// /// absolute path to save the file /// object or station to save. Leave empty to automatically save the current station. -void RoboDK::Save(const QString &filename, const Item *itemsave){ +void RoboDK::Save(const QString &filename, const Item *itemsave) +{ _check_connection(); _send_Line("Save"); _send_Line(filename); @@ -2437,7 +2827,8 @@ void RoboDK::Save(const QString &filename, const Item *itemsave){ Item RoboDK::AddShape(tMatrix2D *trianglePoints, Item *addTo, bool shapeOverride, Color *color) { double colorArray[4] = {0.6,0.6,0.8,1.0}; - if (color != nullptr){ + if (color != nullptr) + { colorArray[0] = color->r; colorArray[1] = color->g; colorArray[2] = color->b; @@ -2529,13 +2920,11 @@ Item RoboDK::AddStation(const QString &name) return newitem; } - - - /// /// Closes the current station without suggesting to save /// -void RoboDK::CloseStation(){ +void RoboDK::CloseStation() +{ _check_connection(); _send_Line("RemoveStn"); _check_status(); @@ -2548,7 +2937,8 @@ void RoboDK::CloseStation(){ /// parent to attach to (such as a frame) /// main robot that will be used to go to self target /// the new target created -Item RoboDK::AddTarget(const QString &name, Item *itemparent, Item *itemrobot){ +Item RoboDK::AddTarget(const QString &name, Item *itemparent, Item *itemrobot) +{ _check_connection(); _send_Line("Add_TARGET"); _send_Line(name); @@ -2565,7 +2955,8 @@ Item RoboDK::AddTarget(const QString &name, Item *itemparent, Item *itemrobot){ /// name of the reference frame /// parent to attach to (such as the robot base frame) /// the new reference frame created -Item RoboDK::AddFrame(const QString &name, Item *itemparent){ +Item RoboDK::AddFrame(const QString &name, Item *itemparent) +{ _check_connection(); _send_Line("Add_FRAME"); _send_Line(name); @@ -2581,7 +2972,8 @@ Item RoboDK::AddFrame(const QString &name, Item *itemparent){ /// name of the program /// robot that will be used /// the new program created -Item RoboDK::AddProgram(const QString &name, Item *itemrobot){ +Item RoboDK::AddProgram(const QString &name, Item *itemrobot) +{ _check_connection(); _send_Line("Add_PROG"); _send_Line(name); @@ -2610,15 +3002,14 @@ Item RoboDK::AddMachiningProject(const QString &name, Item *itemrobot) return newitem; } - - QList RoboDK::getOpenStation() { _check_connection(); _send_Line("G_AllStn"); int nstn = _recv_Int(); QList *listStn = new QList(); - for (int i = 0;i < nstn;i++){ + for (int i = 0; i < nstn; i++) + { Item station = _recv_Item(); listStn->append(station); } @@ -2626,12 +3017,12 @@ QList RoboDK::getOpenStation() return *listStn; } - /// /// Returns the active station item (station currently visible) /// /// -Item RoboDK::getActiveStation() { +Item RoboDK::getActiveStation() +{ _check_connection(); _send_Line("G_ActiveStn"); Item station = _recv_Item(); @@ -2643,7 +3034,8 @@ Item RoboDK::getActiveStation() { /// Set the active station (project currently visible) /// /// station item, it can be previously loaded as an RDK file -void RoboDK::setActiveStation(Item station) { +void RoboDK::setActiveStation(Item station) +{ _check_connection(); _send_Line("S_ActiveStn"); _send_Item(station); @@ -2657,7 +3049,8 @@ void RoboDK::setActiveStation(Item station) { /// /// Function name with parameters (if any) /// -int RoboDK::RunProgram(const QString &function_w_params){ +int RoboDK::RunProgram(const QString &function_w_params) +{ return RunCode(function_w_params, true); } @@ -2667,7 +3060,8 @@ int RoboDK::RunProgram(const QString &function_w_params){ /// /// /// -int RoboDK::RunCode(const QString &code, bool code_is_fcn_call){ +int RoboDK::RunCode(const QString &code, bool code_is_fcn_call) +{ _check_connection(); _send_Line("RunCode"); _send_Int(code_is_fcn_call ? 1 : 0); @@ -2682,7 +3076,8 @@ int RoboDK::RunCode(const QString &code, bool code_is_fcn_call){ /// /// /// -void RoboDK::RunMessage(const QString &message, bool message_is_comment){ +void RoboDK::RunMessage(const QString &message, bool message_is_comment) +{ _check_connection(); _send_Line("RunMessage"); _send_Int(message_is_comment ? 1 : 0); @@ -2694,7 +3089,8 @@ void RoboDK::RunMessage(const QString &message, bool message_is_comment){ /// Renders the scene. This function turns off rendering unless always_render is set to true. /// /// -void RoboDK::Render(bool always_render){ +void RoboDK::Render(bool always_render) +{ bool auto_render = !always_render; _check_connection(); _send_Line("Render"); @@ -2720,7 +3116,8 @@ void RoboDK::Update() /// /// /// -bool RoboDK::IsInside(Item object_inside, Item object_parent){ +bool RoboDK::IsInside(Item object_inside, Item object_parent) +{ _check_connection(); _send_Line("IsInside"); _send_Item(object_inside); @@ -2735,7 +3132,8 @@ bool RoboDK::IsInside(Item object_inside, Item object_parent){ /// /// /// Number of pairs of objects in a collision state -int RoboDK::setCollisionActive(int check_state){ +int RoboDK::setCollisionActive(int check_state) +{ _check_connection(); _send_Line("Collision_SetState"); _send_Int(check_state); @@ -2754,7 +3152,8 @@ int RoboDK::setCollisionActive(int check_state){ /// Joint id for Item 1 (if Item 1 is a robot or a mechanism) /// Joint id for Item 2 (if Item 2 is a robot or a mechanism) /// Returns true if succeeded. Returns false if setting the pair failed (wrong id was provided) -bool RoboDK::setCollisionActivePair(int check_state, Item item1, Item item2, int id1, int id2){ +bool RoboDK::setCollisionActivePair(int check_state, Item item1, Item item2, int id1, int id2) +{ _check_connection(); _send_Line("Collision_SetPair"); _send_Item(item1); @@ -2771,7 +3170,8 @@ bool RoboDK::setCollisionActivePair(int check_state, Item item1, Item item2, int /// Returns the number of pairs of objects that are currently in a collision state. /// /// -int RoboDK::Collisions(){ +int RoboDK::Collisions() +{ _check_connection(); _send_Line("Collisions"); int ncollisions = _recv_Int(); @@ -2785,7 +3185,8 @@ int RoboDK::Collisions(){ /// /// /// -int RoboDK::Collision(Item item1, Item item2){ +int RoboDK::Collision(Item item1, Item item2) +{ _check_connection(); _send_Line("Collided"); _send_Item(item1); @@ -2795,22 +3196,21 @@ int RoboDK::Collision(Item item1, Item item2){ return ncollisions; } -QList RoboDK::getCollisionItems(QList link_id_list) +QList RoboDK::getCollisionItems(QList& link_id_list) { + link_id_list.clear(); + QList itemList; + _check_connection(); - _send_Line("Collision Items"); + _send_Line("Collision_Items"); int nitems = _recv_Int(); - QList itemList = QList(); - if (!link_id_list.isEmpty()){ - link_id_list.clear(); - } - for (int i = 0; i < nitems; i++){ + itemList.reserve(nitems); + link_id_list.reserve(nitems); + for (int i = 0; i < nitems; i++) + { itemList.append(_recv_Item()); - int linkId = _recv_Int(); - if (!link_id_list.isEmpty()){ - link_id_list.append(linkId); - } - _recv_Int(); + link_id_list.append(_recv_Int()); + _recv_Int(); // Number of objects it is in collisions with (unused) } _check_status(); return itemList; @@ -2820,7 +3220,8 @@ QList RoboDK::getCollisionItems(QList link_id_list) /// Sets the current simulation speed. Set the speed to 1 for a real-time simulation. The slowest speed allowed is 0.001 times the real speed. Set to a high value (>100) for fast simulation results. /// /// -void RoboDK::setSimulationSpeed(double speed){ +void RoboDK::setSimulationSpeed(double speed) +{ _check_connection(); _send_Line("SimulateSpeed"); _send_Int((int)(speed * 1000.0)); @@ -2831,13 +3232,15 @@ void RoboDK::setSimulationSpeed(double speed){ /// Gets the current simulation speed. Set the speed to 1 for a real-time simulation. /// /// -double RoboDK::SimulationSpeed(){ +double RoboDK::SimulationSpeed() +{ _check_connection(); _send_Line("GetSimulateSpeed"); double speed = ((double)_recv_Int()) / 1000.0; _check_status(); return speed; } + /// /// Sets the behavior of the RoboDK API. By default, robodk shows the path simulation for movement instructions (run_mode=1=RUNMODE_SIMULATE). /// Setting the run_mode to RUNMODE_QUICKVALIDATE allows performing a quick check to see if the path is feasible. @@ -2848,7 +3251,8 @@ double RoboDK::SimulationSpeed(){ /// RUNMODE_QUICKVALIDATE=2 performs a quick check to validate the robot movements /// RUNMODE_MAKE_ROBOTPROG=3 makes the robot program /// RUNMODE_RUN_REAL=4 moves the real robot is it is connected -void RoboDK::setRunMode(int run_mode){ +void RoboDK::setRunMode(int run_mode) +{ _check_connection(); _send_Line("S_RunMode"); _send_Int(run_mode); @@ -2863,25 +3267,29 @@ void RoboDK::setRunMode(int run_mode){ /// RUNMODE_QUICKVALIDATE=2 performs a quick check to validate the robot movements /// RUNMODE_MAKE_ROBOTPROG=3 makes the robot program /// RUNMODE_RUN_REAL=4 moves the real robot is it is connected -int RoboDK::RunMode(){ +int RoboDK::RunMode() +{ _check_connection(); _send_Line("G_RunMode"); int runmode = _recv_Int(); _check_status(); return runmode; } + /// /// Gets all the user parameters from the open RoboDK station. /// The parameters can also be modified by right clicking the station and selecting "shared parameters" /// User parameters can be added or modified by the user /// /// list of pairs of strings as parameter-value (list of a list) -QList> RoboDK::getParams(){ +QList> RoboDK::getParams() +{ _check_connection(); _send_Line("G_Params"); QList> paramlist; int nparam = _recv_Int(); - for (int i = 0; i < nparam; i++) { + for (int i = 0; i < nparam; i++) + { QString param = _recv_Line(); QString value = _recv_Line(); paramlist.append(qMakePair(param, value)); @@ -2901,12 +3309,14 @@ QList> RoboDK::getParams(){ /// /// RoboDK parameter /// value -QString RoboDK::getParam(const QString ¶m){ +QString RoboDK::getParam(const QString ¶m) +{ _check_connection(); _send_Line("G_Param"); _send_Line(param); QString value = _recv_Line(); - if (value.startsWith("UNKNOWN ")) { + if (value.startsWith("UNKNOWN ")) + { value = ""; } _check_status(); @@ -2920,7 +3330,8 @@ QString RoboDK::getParam(const QString ¶m){ /// RoboDK parameter /// value /// -void RoboDK::setParam(const QString ¶m, const QString &value){ +void RoboDK::setParam(const QString ¶m, const QString &value) +{ _check_connection(); _send_Line("S_Param"); _send_Line(param); @@ -2934,7 +3345,8 @@ void RoboDK::setParam(const QString ¶m, const QString &value){ /// Command Name, such as Trace, Threads or Window. /// Comand value (optional, not all commands require a value) /// -QString RoboDK::Command(const QString &cmd, const QString &value){ +QString RoboDK::Command(const QString &cmd, const QString &value) +{ _check_connection(); _send_Line("SCMD"); _send_Line(cmd); @@ -2952,7 +3364,8 @@ bool RoboDK::LaserTrackerMeasure(tXYZ xyz, tXYZ estimate, bool search) _send_Int(search ? 1 : 0); _recv_XYZ(xyz); _check_status(); - if (xyz[0]*xyz[0] + xyz[1]*xyz[1] + xyz[2]*xyz[2] < 0.0001){ + if (xyz[0]*xyz[0] + xyz[1]*xyz[1] + xyz[2]*xyz[2] < 0.0001) + { return false; } @@ -2962,17 +3375,20 @@ bool RoboDK::LaserTrackerMeasure(tXYZ xyz, tXYZ estimate, bool search) void RoboDK::ShowAsCollided(QList itemList, QList collidedList, QList *robot_link_id) { int nitems = qMin(itemList.length(),collidedList.length()); - if (robot_link_id != nullptr){ + if (robot_link_id != nullptr) + { nitems = qMin(nitems, robot_link_id->length()); } _check_connection(); _send_Line("ShowAsCollidedList"); _send_Int(nitems); - for (int i = 0; i < nitems; i++){ + for (int i = 0; i < nitems; i++) + { _send_Item(itemList[i]); _send_Int(collidedList[i] ? 1 : 0); int link_id = 0; - if (robot_link_id != nullptr){ + if (robot_link_id != nullptr) + { link_id = robot_link_id->at(i); } _send_Int(link_id); @@ -2993,7 +3409,8 @@ void RoboDK::ShowAsCollided(QList itemList, QList collidedList, QLis /// Robot used for calibration (if using joint values) /// TCP as [x, y, z] - calculated TCP /// -void RoboDK::CalibrateTool(tMatrix2D *poses_joints, tXYZ tcp_xyz, int format, int algorithm, Item *robot, double *error_stats){ +void RoboDK::CalibrateTool(tMatrix2D *poses_joints, tXYZ tcp_xyz, int format, int algorithm, Item *robot, double *error_stats) +{ _check_connection(); _send_Line("CalibTCP2"); _send_Matrix2D(poses_joints); @@ -3002,9 +3419,12 @@ void RoboDK::CalibrateTool(tMatrix2D *poses_joints, tXYZ tcp_xyz, int format, in _send_Item(robot); int nxyz = 3; _recv_Array(tcp_xyz, &nxyz); - if (error_stats != nullptr){ + if (error_stats != nullptr) + { _recv_Array(error_stats); - } else { + } + else + { double errors_ignored[20]; _recv_Array(errors_ignored); } @@ -3022,7 +3442,8 @@ void RoboDK::CalibrateTool(tMatrix2D *poses_joints, tXYZ tcp_xyz, int format, in /// use points or joint values. The robot item must be provided if joint values is used. /// /// -Mat RoboDK::CalibrateReference(tMatrix2D *poses_joints, int method, bool use_joints, Item *robot){ +Mat RoboDK::CalibrateReference(tMatrix2D *poses_joints, int method, bool use_joints, Item *robot) +{ _check_connection(); _send_Line("CalibFrame"); _send_Matrix2D(poses_joints); @@ -3037,7 +3458,8 @@ Mat RoboDK::CalibrateReference(tMatrix2D *poses_joints, int method, bool use_joi } -int RoboDK::ProgramStart(const QString &progname, const QString &defaultfolder, const QString &postprocessor, Item *robot){ +int RoboDK::ProgramStart(const QString &progname, const QString &defaultfolder, const QString &postprocessor, Item *robot) +{ _check_connection(); _send_Line("ProgramStart"); _send_Line(progname); @@ -3053,7 +3475,8 @@ int RoboDK::ProgramStart(const QString &progname, const QString &defaultfolder, /// Set the pose of the wold reference frame with respect to the view (camera/screen) /// /// -void RoboDK::setViewPose(const Mat &pose){ +void RoboDK::setViewPose(const Mat &pose) +{ _check_connection(); _send_Line("S_ViewPose"); _send_Pose(pose); @@ -3064,15 +3487,18 @@ void RoboDK::setViewPose(const Mat &pose){ /// Get the pose of the wold reference frame with respect to the view (camera/screen) /// /// -Mat RoboDK::ViewPose(){ +Mat RoboDK::ViewPose() +{ _check_connection(); _send_Line("G_ViewPose"); Mat pose = _recv_Pose(); _check_status(); return pose; } -//INCOMPLETE!!!!!!! -/*bool RoboDK::SetRobotParams(Item *robot, tMatrix2D dhm, Mat poseBase, Mat poseTool) + +//INCOMPLETE function! +/* +bool RoboDK::SetRobotParams(Item *robot, tMatrix2D dhm, Mat poseBase, Mat poseTool) { _check_connection(); _send_Line("S_AbsAccParam"); @@ -3099,7 +3525,45 @@ Mat RoboDK::ViewPose(){ _send_Array(nullptr); _check_status(); return true; -}*/ +} +*/ + +Item RoboDK::Cam2D_Add(const Item &item_object, const QString &cam_params, const Item *cam_item) +{ + _check_connection(); + _send_Line("Cam2D_PtrAdd"); + _send_Item(item_object); + _send_Item(cam_item); + _send_Line(cam_params); + Item cam_item_return = _recv_Item(); + _check_status(); + return cam_item_return; +} + +int RoboDK::Cam2D_Snapshot(const QString &file_save_img, const Item &cam_item, const QString ¶ms) +{ + _check_connection(); + _send_Line("Cam2D_PtrSnapshot"); + _send_Item(cam_item); + _send_Line(file_save_img); + _send_Line(params); + _TIMEOUT = 3600 * 1000; + int status = _recv_Int(); + _TIMEOUT = ROBODK_API_TIMEOUT; + _check_status(); + return status; +} + +int RoboDK::Cam2D_SetParams(const QString ¶ms, const Item &cam_item) +{ + _check_connection(); + _send_Line("Cam2D_PtrSetParams"); + _send_Item(cam_item); + _send_Line(params); + int status = _recv_Int(); + _check_status(); + return status; +} Item RoboDK::getCursorXYZ(int x, int y, tXYZ xyzStation) { @@ -3107,11 +3571,12 @@ Item RoboDK::getCursorXYZ(int x, int y, tXYZ xyzStation) _send_Line("Proj2d3d"); _send_Int(x); _send_Int(y); - _recv_Int(); + _recv_Int(); // Selection (unused) Item selectedItem = _recv_Item(); tXYZ xyz; _recv_XYZ(xyz); - if (xyzStation != nullptr){ + if (xyzStation != nullptr) + { xyzStation[0] = xyz[0]; xyzStation[1] = xyz[1]; xyzStation[2] = xyz[2]; @@ -3120,6 +3585,38 @@ Item RoboDK::getCursorXYZ(int x, int y, tXYZ xyzStation) return selectedItem; } +GetPointsResult RoboDK::GetPoints(int featureType) +{ + GetPointsResult result; + result.featureType = RoboDK::FEATURE_NONE; + result.featureId = 0; + result.points = nullptr; + + if (featureType < FEATURE_HOVER_OBJECT_MESH) + { + return result; + } + + _check_connection(); + _send_Line("G_ObjPoint"); + _send_Item(nullptr); + _send_Int(featureType); + _send_Int(0); + + if (featureType == RoboDK::FEATURE_HOVER_OBJECT_MESH) + { + _recv_Matrix2D(&result.points); + } + + result.item = _recv_Item(); + _recv_Int(); // Skip isFrame + result.featureType = _recv_Int(); + result.featureId = _recv_Int(); + result.featureName = _recv_Line(); + _check_status(); + return result; +} + ///---------------------------------- add get/set robot parameters @@ -3128,7 +3625,8 @@ Item RoboDK::getCursorXYZ(int x, int y, tXYZ xyzStation) /// Returns the license string (as shown in the RoboDK main window) /// /// -QString RoboDK::License(){ +QString RoboDK::License() +{ _check_connection(); _send_Line("G_License"); QString license = _recv_Line(); @@ -3140,12 +3638,14 @@ QString RoboDK::License(){ /// Returns the list of items selected (it can be one or more items) /// /// -QList RoboDK::Selection(){ +QList RoboDK::Selection() +{ _check_connection(); _send_Line("G_Selection"); int nitems = _recv_Int(); QList list_items; - for (int i = 0; i < nitems; i++) { + for (int i = 0; i < nitems; i++) + { list_items.append(_recv_Item()); } _check_status(); @@ -3156,11 +3656,13 @@ QList RoboDK::Selection(){ /// Sets the selection in the tree (it can be one or more items). /// /// List of items to set as selected -void RoboDK::setSelection(QList list_items){ +void RoboDK::setSelection(QList list_items) +{ _check_connection(); _send_Line("S_Selection"); _send_Int(list_items.length()); - for (int i = 0; i < list_items.length(); i++) { + for (int i = 0; i < list_items.length(); i++) + { _send_Item(list_items[i]); } _check_status(); @@ -3168,21 +3670,53 @@ void RoboDK::setSelection(QList list_items){ /// -/// Show the popup menu to create the ISO9283 path for path accuracy and performance testing +/// Load or unload the specified plugin (path to DLL, dylib or SO file). If the plugin is already loaded it will unload the plugin and reload it. Pass an empty plugin_name to reload all plugins. /// -/// IS9283 Program -Item RoboDK::Popup_ISO9283_CubeProgram(Item *robot, tXYZ center, double side, bool blocking){ - //_require_build(5177); - Item iso_program; +void RoboDK::PluginLoad(const QString &pluginName, int load) +{ _check_connection(); - if (center == nullptr){ - _send_Line("Popup_ProgISO9283"); - _send_Item(robot); - _TIMEOUT = 3600 * 1000; + _send_Line("PluginLoad"); + _send_Line(pluginName); + _send_Int(load); + _check_status(); +} + +/// +/// Send a specific command to a RoboDK plugin. The command and value (optional) must be handled by your plugin. It returns the result as a string. +/// +QString RoboDK::PluginCommand(const QString &pluginName, const QString &pluginCommand, const QString &pluginValue) +{ + _check_connection(); + _send_Line("PluginCommand"); + _send_Line(pluginName); + _send_Line(pluginCommand); + _send_Line(pluginValue); + QString result = _recv_Line(); + _check_status(); + return result; +} + + +/// +/// Show the popup menu to create the ISO9283 path for path accuracy and performance testing +/// +/// IS9283 Program +Item RoboDK::Popup_ISO9283_CubeProgram(Item *robot, tXYZ center, double side, bool blocking) +{ + //_require_build(5177); + Item iso_program; + _check_connection(); + if (center == nullptr) + { + _send_Line("Popup_ProgISO9283"); + _send_Item(robot); + _TIMEOUT = 3600 * 1000; iso_program = _recv_Item(); _TIMEOUT = ROBODK_API_TIMEOUT; _check_status(); - } else { + } + else + { _send_Line("Popup_ProgISO9283_Param"); _send_Item(robot); double values[5]; @@ -3192,7 +3726,8 @@ Item RoboDK::Popup_ISO9283_CubeProgram(Item *robot, tXYZ center, double side, bo values[3] = side; values[4] = blocking ? 1 : 0; _send_Array(values, 4); - if (blocking){ + if (blocking) + { _TIMEOUT = 3600 * 1000; iso_program = _recv_Item(); _TIMEOUT = ROBODK_API_TIMEOUT; @@ -3202,26 +3737,48 @@ Item RoboDK::Popup_ISO9283_CubeProgram(Item *robot, tXYZ center, double side, bo return iso_program; } - - - -bool RoboDK::FileSet(const QString &path_file_local, const QString &file_remote, bool load_file, Item *attach_to){ - if (!_check_connection()){ return false; } - if (!_send_Line("FileRecvBin")){ return false; } +bool RoboDK::FileSet(const QString &path_file_local, const QString &file_remote, bool load_file, Item *attach_to) +{ + if (!_check_connection()) + { + return false; + } + if (!_send_Line("FileRecvBin")) + { + return false; + } QFile file(path_file_local); - if (!_send_Line(file_remote)){ return false; } + if (!_send_Line(file_remote)) + { + return false; + } int nbytes = file.size(); - if (!_send_Int(nbytes)){ return false; } - if (!_send_Item(attach_to)){ return false; } - if (!_send_Int(load_file ? 1 : 0)){ return false; } - if (!_check_status()){ return false; } + if (!_send_Int(nbytes)) + { + return false; + } + if (!_send_Item(attach_to)) + { + return false; + } + if (!_send_Int(load_file ? 1 : 0)) + { + return false; + } + if (!_check_status()) + { + return false; + } int sz_sent = 0; - if (!file.open(QFile::ReadOnly)){ + if (!file.open(QFile::ReadOnly)) + { return false; } - while (true){ + while (true) + { QByteArray buffer(file.read(1024)); - if (buffer.size() == 0){ + if (buffer.size() == 0) + { break; } // warning! Nothing guarantees that all bytes are sent @@ -3232,33 +3789,54 @@ bool RoboDK::FileSet(const QString &path_file_local, const QString &file_remote, return true; } -bool RoboDK::FileGet(const QString &path_file_local, Item *station, const QString path_file_remote){ - if (!_check_connection()){ return false; } - if (!_send_Line("FileSendBin")){ return false; } - if (!_send_Item(station)){ return false; } - if (!_send_Line(path_file_remote)){ return false; } +bool RoboDK::FileGet(const QString &path_file_local, Item *station, const QString path_file_remote) +{ + if (!_check_connection()) + { + return false; + } + if (!_send_Line("FileSendBin")) + { + return false; + } + if (!_send_Item(station)) + { + return false; + } + if (!_send_Line(path_file_remote)) + { + return false; + } int nbytes = _recv_Int(); int remaining = nbytes; QFile file(path_file_local); - if (!file.open(QFile::WriteOnly)){ + if (!file.open(QFile::WriteOnly)) + { qDebug() << "Can not open file for writting " << path_file_local; return false; } - while (remaining > 0){ + while (remaining > 0) + { QByteArray buffer(_COM->read(qMin(remaining, 1024))); remaining -= buffer.size(); file.write(buffer); } file.close(); - if (!_check_status()){ return false;} + if (!_check_status()) + { + return false; + } return true; } - bool RoboDK::EmbedWindow(QString window_name, QString docked_name, int size_w, int size_h, quint64 pid, int area_add, int area_allowed, int timeout) { - if (!_check_connection()){ return false; } - if (docked_name == "") { + if (!_check_connection()) + { + return false; + } + if (docked_name == "") + { docked_name = window_name; } _check_connection(); @@ -3276,13 +3854,15 @@ bool RoboDK::EmbedWindow(QString window_name, QString docked_name, int size_w, i return result > 0; } - bool RoboDK::EventsListen() { _COM_EVT = new QTcpSocket(); - if (_IP.isEmpty()){ + if (_IP.isEmpty()) + { _COM_EVT->connectToHost("127.0.0.1", _PORT); //QHostAddress::LocalHost, _PORT); - } else { + } + else + { _COM_EVT->connectToHost(_IP, _PORT); } qDebug() << _COM_EVT->state(); @@ -3292,7 +3872,7 @@ bool RoboDK::EventsListen() _send_Int(0, _COM_EVT); QString response = _recv_Line(_COM_EVT); qDebug() << response; - _recv_Int(_COM_EVT); + _recv_Int(_COM_EVT); // vet_evt (unused) int status = _recv_Int(_COM_EVT); if (response != "RDK_EVT" || status != 0) { @@ -3311,12 +3891,14 @@ bool RoboDK::WaitForEvent(int &evt, Item &itm) } //Receives 24 doubles of data from the event loop -bool RoboDK::Event_Receive_3D_POS(double *data, int *valueCount) { +bool RoboDK::Event_Receive_3D_POS(double *data, int *valueCount) +{ return _recv_Array(data,valueCount,_COM_EVT); } //Receives the 3 bytes of mouse data -bool RoboDK::Event_Receive_Mouse_data(int *data) { +bool RoboDK::Event_Receive_Mouse_data(int *data) +{ data[0] = _recv_Int(_COM_EVT); data[1] = _recv_Int(_COM_EVT); data[2] = _recv_Int(_COM_EVT); @@ -3324,7 +3906,8 @@ bool RoboDK::Event_Receive_Mouse_data(int *data) { return true; } -bool RoboDK::Event_Receive_Event_Moved(Mat *pose_rel_out) { +bool RoboDK::Event_Receive_Event_Moved(Mat *pose_rel_out) +{ int nvalues = _recv_Int(_COM_EVT); Mat pose_rel = _recv_Pose(_COM_EVT); *pose_rel_out = pose_rel; @@ -3344,13 +3927,16 @@ bool RoboDK::Event_Connected() //-------------------------- private --------------------------------------- -bool RoboDK::_connected(){ +bool RoboDK::_connected() +{ return _COM != nullptr && _COM->state() == QTcpSocket::ConnectedState; } -bool RoboDK::_check_connection(){ - if (_connected()){ +bool RoboDK::_check_connection() +{ + if (_connected()) + { return true; } bool connection_ok = _connect_smart(); @@ -3360,71 +3946,120 @@ bool RoboDK::_check_connection(){ return connection_ok; } -bool RoboDK::_check_status(){ +bool RoboDK::_check_status() +{ qint32 status = _recv_Int(); - if (status == 0) { + if (status == 0) + { // everything is OK //status = status - } else if (status > 0 && status < 10) { + } + else if (status > 0 && status < 10) + { QString strproblems("Unknown error"); - if (status == 1) { + if (status == 1) + { strproblems = "Invalid item provided: The item identifier provided is not valid or it does not exist."; - } else if (status == 2) { //output warning only + } + else if (status == 2) + { + //output warning only strproblems = _recv_Line(); qDebug() << "RoboDK API WARNING: " << strproblems; return 0; - } else if (status == 3){ // output error + } + else if (status == 3) + { + // output error strproblems = _recv_Line(); qDebug() << "RoboDK API ERROR: " << strproblems; - } else if (status == 9) { + } + else if (status == 9) + { qDebug() << "Invalid RoboDK License"; } //print(strproblems); - //throw new RDKException(strproblems); //raise Exception(strproblems) - } else if (status < 100){ + if (_USE_EXCPETIONS == true) + { + throw std::runtime_error(strproblems.toStdString() + ": " + QString::number(status).toStdString()); + } + } + else if (status < 100) + { QString strproblems = _recv_Line(); qDebug() << "RoboDK API ERROR: " << strproblems; - } else { - //throw new RDKException("Communication problems with the RoboDK API"); //raise Exception('Problems running function'); + if (_USE_EXCPETIONS == true) + { + QString errorMessage = QString("RoboDK API ERROR: ") + strproblems; + throw std::runtime_error(errorMessage.toStdString() + ": " + QString::number(status).toStdString()); + } + } + else + { + if (_USE_EXCPETIONS == true) + { + throw std::runtime_error("Communication problems with the RoboDK API: " + QString::number(status).toStdString()); + } qDebug() << "Communication problems with the RoboDK API"; } return status; } - - -void RoboDK::_disconnect(){ - if (_COM != nullptr){ - _COM->deleteLater(); - _COM = nullptr; +void RoboDK::_disconnect() +{ + if (!_COM || !_OWN_SOCKET) + { + return; } + + _COM->deleteLater(); + _COM = nullptr; } // attempt a simple connection to RoboDK and start RoboDK if it is not running -bool RoboDK::_connect_smart(){ +bool RoboDK::_connect_smart() +{ //Establishes a connection with robodk. robodk must be running, otherwise, it will attempt to start it - if (_connect()){ + if (_connect()) + { qDebug() << "The RoboDK API is connected"; return true; } + // Don't try to start RoboDK in case we have custom socket + if (!_OWN_SOCKET) + { + return false; + } + +#if (QT_VERSION < QT_VERSION_CHECK(5, 14, 0)) + const QString::SplitBehavior behavior = QString::SkipEmptyParts; +#else + const Qt::SplitBehavior behavior = Qt::SkipEmptyParts; +#endif + qDebug() << "...Trying to start RoboDK: " << _ROBODK_BIN << " " << _ARGUMENTS; // Start RoboDK QProcess *p = new QProcess(); //_ARGUMENTS = "/DEBUG"; - p->start(_ROBODK_BIN, _ARGUMENTS.split(" ", QString::SkipEmptyParts)); + p->start(_ROBODK_BIN, _ARGUMENTS.split(" ", behavior)); p->setReadChannel(QProcess::StandardOutput); //p->waitForReadyRead(10000); _PROCESS = p->processId(); - while (p->canReadLine() || p->waitForReadyRead(5000)){ + while (p->canReadLine() || p->waitForReadyRead(5000)) + { QString line = QString::fromUtf8(p->readLine().trimmed()); //qDebug() << "RoboDK process: " << line; - if (line.contains("Running", Qt::CaseInsensitive)){ + if (line.contains("Running", Qt::CaseInsensitive)) + { qDebug() << "RoboDK is Running... Connecting API"; bool is_connected = _connect(); - if (is_connected){ + if (is_connected) + { qDebug() << "The RoboDK API is connected"; - } else { + } + else + { qDebug() << "The RoboDK API is NOT connected!"; } return is_connected; @@ -3435,16 +4070,27 @@ bool RoboDK::_connect_smart(){ } // attempt a simple connection to RoboDK -bool RoboDK::_connect(){ +bool RoboDK::_connect() +{ + // Do nothing with custom socket, just return its state + if (!_OWN_SOCKET) + { + return _connected(); + } + _disconnect(); _COM = new QTcpSocket(); - if (_IP.isEmpty()){ + if (_IP.isEmpty()) + { _COM->connectToHost("127.0.0.1", _PORT); //QHostAddress::LocalHost, _PORT); - } else { + } + else + { _COM->connectToHost(_IP, _PORT); } // usually, 5 msec should be enough for localhost - if (!_COM->waitForConnected(_TIMEOUT)){ + if (!_COM->waitForConnected(_TIMEOUT)) + { _COM->deleteLater(); _COM = nullptr; return false; @@ -3461,14 +4107,16 @@ bool RoboDK::_connect(){ return false; }*/ // 10 msec should be enough for localhost - if (!_COM->canReadLine() && !_COM->waitForReadyRead(_TIMEOUT)){ + if (!_COM->canReadLine() && !_COM->waitForReadyRead(_TIMEOUT)) + { _COM->deleteLater(); _COM = nullptr; return false; } QString read(_COM->readAll()); // make sure we receive the OK from RoboDK - if (!read.startsWith(ROBODK_API_READY_STRING)){ + if (!read.startsWith(ROBODK_API_READY_STRING)) + { _COM->deleteLater(); _COM = nullptr; return false; @@ -3478,25 +4126,37 @@ bool RoboDK::_connect(){ ///////////////////////////////////////////// -bool RoboDK::_waitline(QTcpSocket *com){ - if (com == nullptr) { +bool RoboDK::_waitline(QAbstractSocket* com) +{ + if (com == nullptr) + { com = _COM; } - if (com == nullptr){ return false; } - while (!com->canReadLine()){ - if (!com->waitForReadyRead(_TIMEOUT)){ + if (com == nullptr) + { + return false; + } + while (!com->canReadLine()) + { + if (!com->waitForReadyRead(_TIMEOUT)) + { return false; } } return true; } -QString RoboDK::_recv_Line(QTcpSocket *com){//QString &string){ - if (com == nullptr) { + +QString RoboDK::_recv_Line(QAbstractSocket* com) +{ + if (com == nullptr) + { com = _COM; } QString string; - if (!_waitline(com)){ - if (com != nullptr){ + if (!_waitline(com)) + { + if (com != nullptr) + { //if this happens it means that there are problems: delete buffer com->readAll(); } @@ -3506,25 +4166,38 @@ QString RoboDK::_recv_Line(QTcpSocket *com){//QString &string){ string.append(QString::fromUtf8(line)); return string; } -bool RoboDK::_send_Line(const QString& string,QTcpSocket *com){ - if (com == nullptr) { + +bool RoboDK::_send_Line(const QString& string, QAbstractSocket* com) +{ + if (com == nullptr) + { com = _COM; } - if (com == nullptr || !com->isOpen()){ return false; } + if (com == nullptr || !com->isOpen()) + { + return false; + } com->write(string.toUtf8()); com->write(ROBODK_API_LF, 1); return true; } -int RoboDK::_recv_Int(QTcpSocket *com){//qint32 &value){ - if (com == nullptr){ +int RoboDK::_recv_Int(QAbstractSocket* com) +{ + if (com == nullptr) + { com = _COM; } qint32 value; // do not change type - if (com == nullptr){ return false; } - if (com->bytesAvailable() < sizeof(qint32)){ + if (com == nullptr) + { + return false; + } + if (com->bytesAvailable() < static_cast(sizeof(qint32))) + { com->waitForReadyRead(_TIMEOUT); - if (com->bytesAvailable() < sizeof(qint32)){ + if (com->bytesAvailable() < static_cast(sizeof(qint32))) + { return -1; } } @@ -3532,27 +4205,40 @@ int RoboDK::_recv_Int(QTcpSocket *com){//qint32 &value){ ds >> value; return value; } -bool RoboDK::_send_Int(qint32 value,QTcpSocket *com){ - if (com == nullptr) { + +bool RoboDK::_send_Int(qint32 value, QAbstractSocket* com) +{ + if (com == nullptr) + { com = _COM; } - if (com == nullptr || !com->isOpen()){ return false; } + if (com == nullptr || !com->isOpen()) + { + return false; + } QDataStream ds(com); ds << value; return true; } -Item RoboDK::_recv_Item(QTcpSocket *com){//Item *item){ - if (com == nullptr) { +Item RoboDK::_recv_Item(QAbstractSocket* com) +{ + if (com == nullptr) + { com = _COM; } Item item(this); - if (com == nullptr){ return item; } + if (com == nullptr) + { + return item; + } item._PTR = 0; item._TYPE = -1; - if (com->bytesAvailable() < sizeof(quint64)){ + if (com->bytesAvailable() < static_cast(sizeof(quint64))) + { com->waitForReadyRead(_TIMEOUT); - if (com->bytesAvailable() < sizeof(quint64)){ + if (com->bytesAvailable() < static_cast(sizeof(quint64))) + { return item; } } @@ -3561,31 +4247,46 @@ Item RoboDK::_recv_Item(QTcpSocket *com){//Item *item){ ds >> item._TYPE; return item; } -bool RoboDK::_send_Item(const Item *item){ - if (_COM == nullptr || !_COM->isOpen()){ return false; } + +bool RoboDK::_send_Item(const Item *item) +{ + if (_COM == nullptr || !_COM->isOpen()) + { + return false; + } QDataStream ds(_COM); quint64 ptr = 0; - if (item != nullptr){ + if (item != nullptr) + { ptr = item->_PTR; } ds << ptr; return true; } -bool RoboDK::_send_Item(const Item &item){ + +bool RoboDK::_send_Item(const Item &item) +{ return _send_Item(&item); } -Mat RoboDK::_recv_Pose(QTcpSocket *com){//Mat &pose){ - if (com == nullptr) { +Mat RoboDK::_recv_Pose(QAbstractSocket* com) +{ + if (com == nullptr) + { com = _COM; } Mat pose; - if (com == nullptr){ return pose; } + if (com == nullptr) + { + return pose; + } int size = 16*sizeof(double); - if (com->bytesAvailable() < size){ + if (com->bytesAvailable() < size) + { com->waitForReadyRead(_TIMEOUT); - if (com->bytesAvailable() < size){ + if (com->bytesAvailable() < size) + { return pose; } } @@ -3593,8 +4294,10 @@ Mat RoboDK::_recv_Pose(QTcpSocket *com){//Mat &pose){ ds.setFloatingPointPrecision(QDataStream::DoublePrecision); //ds.setByteOrder(QDataStream::LittleEndian); double valuei; - for (int j=0; j<4; j++){ - for (int i=0; i<4; i++){ + for (int j = 0; j < 4; j++) + { + for (int i = 0; i < 4; i++) + { ds >> valuei; pose.Set(i,j,valuei); //pose.data()[i][j] = valuei; @@ -3602,26 +4305,41 @@ Mat RoboDK::_recv_Pose(QTcpSocket *com){//Mat &pose){ } return pose; } -bool RoboDK::_send_Pose(const Mat &pose){ - if (_COM == nullptr || !_COM->isOpen()){ return false; } + +bool RoboDK::_send_Pose(const Mat &pose) +{ + if (_COM == nullptr || !_COM->isOpen()) + { + return false; + } QDataStream ds(_COM); ds.setFloatingPointPrecision(QDataStream::DoublePrecision); //ds.setByteOrder(QDataStream::LittleEndian); double valuei; - for (int j=0; j<4; j++){ - for (int i=0; i<4; i++){ + for (int j = 0; j < 4; j++) + { + for (int i = 0; i < 4; i++) + { valuei = pose.Get(i,j); ds << valuei; } } return true; } -bool RoboDK::_recv_XYZ(tXYZ pos){ - if (_COM == nullptr){ return false; } - int size = 3*sizeof(double); - if (_COM->bytesAvailable() < size){ + +bool RoboDK::_recv_XYZ(tXYZ pos) +{ + if (_COM == nullptr) + { + return false; + } + + int size = 3 * sizeof(double); + if (_COM->bytesAvailable() < size) + { _COM->waitForReadyRead(_TIMEOUT); - if (_COM->bytesAvailable() < size){ + if (_COM->bytesAvailable() < size) + { return false; } } @@ -3629,59 +4347,88 @@ bool RoboDK::_recv_XYZ(tXYZ pos){ ds.setFloatingPointPrecision(QDataStream::DoublePrecision); //ds.setByteOrder(QDataStream::LittleEndian); double valuei; - for (int i=0; i<3; i++){ + for (int i = 0; i < 3; i++) + { ds >> valuei; pos[i] = valuei; } return true; } -bool RoboDK::_send_XYZ(const tXYZ pos){ - if (_COM == nullptr || !_COM->isOpen()){ return false; } + +bool RoboDK::_send_XYZ(const tXYZ pos) +{ + if (_COM == nullptr || !_COM->isOpen()) + { + return false; + } QDataStream ds(_COM); ds.setFloatingPointPrecision(QDataStream::DoublePrecision); //ds.setByteOrder(QDataStream::LittleEndian); double valuei; - for (int i=0; i<3; i++){ + for (int i = 0; i < 3; i++) + { valuei = pos[i]; ds << valuei; } return true; } -bool RoboDK::_recv_Array(tJoints *jnts){ + +bool RoboDK::_recv_Array(tJoints *jnts) +{ return _recv_Array(jnts->_Values, &(jnts->_nDOFs)); } -bool RoboDK::_send_Array(const tJoints *jnts){ - if (jnts == nullptr){ + +bool RoboDK::_send_Array(const tJoints *jnts) +{ + if (jnts == nullptr) + { return _send_Int(0); } return _send_Array(jnts->_Values, jnts->_nDOFs); } -bool RoboDK::_send_Array(const Mat *mat){ - if (mat == nullptr){ + +bool RoboDK::_send_Array(const Mat *mat) +{ + if (mat == nullptr) + { return _send_Int(0); } double m44[16]; - for (int c=0; c<4; c++){ - for (int r=0; r<4; r++){ + for (int c = 0; c < 4; c++) + { + for (int r = 0; r < 4; r++) + { m44[c*4+r] = mat->Get(r,c); } } return _send_Array(m44, 16); } -bool RoboDK::_recv_Array(double *values, int *psize, QTcpSocket *com){ - if (com == nullptr) { + +bool RoboDK::_recv_Array(double *values, int *psize, QAbstractSocket* com) +{ + if (com == nullptr) + { com = _COM; } int nvalues = _recv_Int(); - if (com == nullptr || nvalues < 0) {return false;} - if (psize != nullptr){ + if (com == nullptr || nvalues < 0) + { + return false; + } + if (psize != nullptr) + { *psize = nvalues; } - if (nvalues < 0 || nvalues > 50){return false;} //check if the value is not too big - int size = nvalues*sizeof(double); - if (com->bytesAvailable() < size){ + if (nvalues < 0 || nvalues > 50) //check if the value is not too big + { + return false; + } + int size = nvalues * sizeof(double); + if (com->bytesAvailable() < size) + { com->waitForReadyRead(_TIMEOUT); - if (com->bytesAvailable() < size){ + if (com->bytesAvailable() < size) + { return false; } } @@ -3689,44 +4436,64 @@ bool RoboDK::_recv_Array(double *values, int *psize, QTcpSocket *com){ ds.setFloatingPointPrecision(QDataStream::DoublePrecision); //ds.setByteOrder(QDataStream::LittleEndian); double valuei; - for (int i=0; i> valuei; values[i] = valuei; } return true; } -bool RoboDK::_send_Array(const double *values, int nvalues){ - if (_COM == nullptr || !_COM->isOpen()){ return false; } - if (!_send_Int((qint32)nvalues)){ return false; } +bool RoboDK::_send_Array(const double *values, int nvalues) +{ + if (_COM == nullptr || !_COM->isOpen()) + { + return false; + } + if (!_send_Int((qint32)nvalues)) + { + return false; + } QDataStream ds(_COM); ds.setFloatingPointPrecision(QDataStream::DoublePrecision); //ds.setByteOrder(QDataStream::LittleEndian); double valuei; - for (int i=0; ibytesAvailable() <= 0 && !_COM->waitForReadyRead(_TIMEOUT)){ + if (remaining <= 0) + { + return true; + } + if (_COM->bytesAvailable() <= 0 && !_COM->waitForReadyRead(_TIMEOUT)) + { Matrix2D_Delete(mat); return false; } @@ -3734,7 +4501,8 @@ bool RoboDK::_recv_Matrix2D(tMatrix2D **mat){ // needs to delete after! int np = buffer.size() / sizeof(double); QDataStream indata(buffer); indata.setFloatingPointPrecision(QDataStream::DoublePrecision); - for (int i=0; i> value; (*mat)->data[count] = value; count = count + 1; @@ -3743,8 +4511,13 @@ bool RoboDK::_recv_Matrix2D(tMatrix2D **mat){ // needs to delete after! } return false;// we should never arrive here... } -bool RoboDK::_send_Matrix2D(tMatrix2D *mat){ - if (_COM == nullptr || !_COM->isOpen()){ return false; } + +bool RoboDK::_send_Matrix2D(tMatrix2D *mat) +{ + if (_COM == nullptr || !_COM->isOpen()) + { + return false; + } QDataStream ds(_COM); ds.setFloatingPointPrecision(QDataStream::DoublePrecision); //ds.setByteOrder(QDataStream::LittleEndian); @@ -3752,101 +4525,171 @@ bool RoboDK::_send_Matrix2D(tMatrix2D *mat){ qint32 dim2 = Matrix2D_Size(mat, 2); bool ok1 = _send_Int(dim1); bool ok2 = _send_Int(dim2); - if (!ok1 || !ok2) {return false; } + if (!ok1 || !ok2) + { + return false; + } double valuei; - for (int j=0; jisOpen()) + { + return false; + } + + { + QDataStream ds(com); + ds << data.size(); + } + com->write(data); + return true; +} + +QByteArray RoboDK::_recv_Bytes(QAbstractSocket* com) +{ + if (!com) + { + com = _COM; + } + if (!com || !com->isOpen()) + { + return QByteArray(); + } + + int size = _recv_Int(com); + + while (com->bytesAvailable() < size) + { + if (!com->waitForReadyRead(_TIMEOUT)) + { + return QByteArray(); + } + } + + return com->read(size); +} + // private move type, to be used by public methods (MoveJ and MoveL) -void RoboDK::_moveX(const Item *target, const tJoints *joints, const Mat *mat_target, const Item *itemrobot, int movetype, bool blocking){ +void RoboDK::_moveX(const Item *target, const tJoints *joints, const Mat *mat_target, const Item *itemrobot, int movetype, bool blocking) +{ itemrobot->WaitMove(); _send_Line("MoveX"); _send_Int(movetype); - if (target != nullptr){ + if (target != nullptr) + { _send_Int(3); _send_Array((tJoints*)nullptr); _send_Item(target); - } else if (joints != nullptr){ + } + else if (joints != nullptr) + { _send_Int(1); _send_Array(joints); _send_Item(nullptr); - } else if (mat_target != nullptr){// && mat_target.IsHomogeneous()) { + } + else if (mat_target != nullptr) + { _send_Int(2); _send_Array(mat_target); // keep it as array! _send_Item(nullptr); - } else { - //throw new RDKException("Invalid target type"); //raise Exception('Problems running function'); + } + else + { + if (_USE_EXCPETIONS == true) + { + throw std::runtime_error("Invalid target type"); + } throw 0; } _send_Item(itemrobot); _check_status(); - if (blocking){ + if (blocking) + { itemrobot->WaitMove(); } } + // private move type, to be used by public methods (MoveJ and MoveL) -void RoboDK::_moveC(const Item *target1, const tJoints *joints1, const Mat *mat_target1, const Item *target2, const tJoints *joints2, const Mat *mat_target2, const Item *itemrobot, bool blocking){ +void RoboDK::_moveC(const Item *target1, const tJoints *joints1, const Mat *mat_target1, const Item *target2, const tJoints *joints2, const Mat *mat_target2, const Item *itemrobot, bool blocking) +{ itemrobot->WaitMove(); _send_Line("MoveC"); _send_Int(3); - if (target1 != nullptr){ + if (target1 != nullptr) + { _send_Int(3); _send_Array((tJoints*)nullptr); _send_Item(target1); - } else if (joints1 != nullptr) { + } + else if (joints1 != nullptr) + { _send_Int(1); _send_Array(joints1); _send_Item(nullptr); - } else if (mat_target1 != nullptr){// && mat_target1.IsHomogeneous()) { + } + else if (mat_target1 != nullptr) + { _send_Int(2); _send_Array(mat_target1); _send_Item(nullptr); - } else { - throw 0; - //throw new RDKException("Invalid type of target 1"); + } + else + { + if (_USE_EXCPETIONS == true) + { + throw std::runtime_error("Invalid type of target 1"); + } } ///////////////////////////////////// - if (target2 != nullptr) { + if (target2 != nullptr) + { _send_Int(3); _send_Array((tJoints*)nullptr); _send_Item(target2); - } else if (joints2 != nullptr) { + } + else if (joints2 != nullptr) + { _send_Int(1); _send_Array(joints2); _send_Item(nullptr); - } else if (mat_target2 != nullptr){// && mat_target2.IsHomogeneous()) { + } + else if (mat_target2 != nullptr) + { _send_Int(2); _send_Array(mat_target2); _send_Item(nullptr); - } else { - throw 0; - //throw new RDKException("Invalid type of target 2"); + } + else + { + if (_USE_EXCPETIONS == true) + { + throw std::runtime_error("Invalid type of target 2"); + } } ///////////////////////////////////// _send_Item(itemrobot); _check_status(); - if (blocking){ + if (blocking) + { itemrobot->WaitMove(); } } - - - - - - - - - - - - //--------------------------------------------------------------------------------------------------- //--------------------------------------------------------------------------------------------------- //--------------------------------------------------------------------------------------------------- @@ -3867,21 +4710,25 @@ void emxInit_real_T(tMatrix2D **pEmxArray, int numDimensions) emxArray->size = (int *)malloc((unsigned int)(sizeof(int) * numDimensions)); emxArray->allocatedSize = 0; emxArray->canFreeData = true; - for (i = 0; i < numDimensions; i++) { + for (i = 0; i < numDimensions; i++) + { emxArray->size[i] = 0; } } /// -tMatrix2D* Matrix2D_Create() { +tMatrix2D* Matrix2D_Create() +{ tMatrix2D *matrix; emxInit_real_T((tMatrix2D**)(&matrix), 2); return matrix; } - -void emxFree_real_T(tMatrix2D **pEmxArray){ - if (*pEmxArray != (tMatrix2D *)NULL) { - if (((*pEmxArray)->data != (double *)NULL) && (*pEmxArray)->canFreeData) { +void emxFree_real_T(tMatrix2D **pEmxArray) +{ + if (*pEmxArray != (tMatrix2D *)NULL) + { + if (((*pEmxArray)->data != (double *)NULL) && (*pEmxArray)->canFreeData) + { free((void *)(*pEmxArray)->data); } free((void *)(*pEmxArray)->size); @@ -3890,39 +4737,49 @@ void emxFree_real_T(tMatrix2D **pEmxArray){ } } -void Matrix2D_Delete(tMatrix2D **mat) { +void Matrix2D_Delete(tMatrix2D **mat) +{ emxFree_real_T((tMatrix2D**)(mat)); } - - -void emxEnsureCapacity(tMatrix2D *emxArray, int oldNumel, unsigned int elementSize){ +void emxEnsureCapacity(tMatrix2D *emxArray, int oldNumel, unsigned int elementSize) +{ int newNumel; int i; double *newData; - if (oldNumel < 0) { + if (oldNumel < 0) + { oldNumel = 0; } newNumel = 1; - for (i = 0; i < emxArray->numDimensions; i++) { + for (i = 0; i < emxArray->numDimensions; i++) + { newNumel *= emxArray->size[i]; } - if (newNumel > emxArray->allocatedSize) { + if (newNumel > emxArray->allocatedSize) + { i = emxArray->allocatedSize; - if (i < 16) { + if (i < 16) + { i = 16; } - while (i < newNumel) { - if (i > 1073741823) { + while (i < newNumel) + { + if (i > 1073741823) + { i =(2147483647);//MAX_int32_T; - } else { + } + else + { i <<= 1; } } newData = (double*) calloc((unsigned int)i, elementSize); - if (emxArray->data != NULL) { + if (emxArray->data != NULL) + { memcpy(newData, emxArray->data, elementSize * oldNumel); - if (emxArray->canFreeData) { + if (emxArray->canFreeData) + { free(emxArray->data); } } @@ -3932,46 +4789,58 @@ void emxEnsureCapacity(tMatrix2D *emxArray, int oldNumel, unsigned int elementSi } } -void Matrix2D_Set_Size(tMatrix2D *mat, int rows, int cols) { +void Matrix2D_Set_Size(tMatrix2D *mat, int rows, int cols) +{ int old_numel; - int numbel; old_numel = mat->size[0] * mat->size[1]; mat->size[0] = rows; mat->size[1] = cols; - numbel = rows*cols; emxEnsureCapacity(mat, old_numel, sizeof(double)); - /*for (i=0; idata[i] = 0.0; - }*/ } -int Matrix2D_Size(const tMatrix2D *var, int dim) { // ONE BASED!! - if (var->numDimensions >= dim) { +int Matrix2D_Size(const tMatrix2D *var, int dim) +{ + // ONE BASED!! + if (var->numDimensions >= dim) + { return var->size[dim - 1]; } - else { + else + { return 0; } } -int Matrix2D_Get_ncols(const tMatrix2D *var) { + +int Matrix2D_Get_ncols(const tMatrix2D *var) +{ return Matrix2D_Size(var, 2); } -int Matrix2D_Get_nrows(const tMatrix2D *var) { + +int Matrix2D_Get_nrows(const tMatrix2D *var) +{ return Matrix2D_Size(var, 1); } -double Matrix2D_Get_ij(const tMatrix2D *var, int i, int j) { // ZERO BASED!! + +double Matrix2D_Get_ij(const tMatrix2D *var, int i, int j) +{ + // ZERO BASED!! return var->data[var->size[0] * j + i]; } -void Matrix2D_SET_ij(const tMatrix2D *var, int i, int j, double value) { // ZERO BASED!! + +void Matrix2D_SET_ij(const tMatrix2D *var, int i, int j, double value) +{ + // ZERO BASED!! var->data[var->size[0] * j + i] = value; } -double *Matrix2D_Get_col(const tMatrix2D *var, int col) { // ZERO BASED!! +double *Matrix2D_Get_col(const tMatrix2D *var, int col) +{ + // ZERO BASED!! return (var->data + var->size[0] * col); } - -void Matrix2D_Add(tMatrix2D *var, const double *array, int numel){ +void Matrix2D_Add(tMatrix2D *var, const double *array, int numel) +{ int oldnumel; int size1 = var->size[0]; int size2 = var->size[1]; @@ -3979,44 +4848,52 @@ void Matrix2D_Add(tMatrix2D *var, const double *array, int numel){ var->size[1] = size2 + 1; emxEnsureCapacity(var, oldnumel, (int)sizeof(double)); numel = qMin(numel, size1); - for (int i=0; idata[size1*size2 + i] = array[i]; } } -void Matrix2D_Add(tMatrix2D *var, const tMatrix2D *varadd){ +void Matrix2D_Add(tMatrix2D *var, const tMatrix2D *varadd) +{ int oldnumel; int size1 = var->size[0]; int size2 = var->size[1]; int size1_ap = varadd->size[0]; int size2_ap = varadd->size[1]; int numel = size1_ap*size2_ap; - if (size1 != size1_ap){ + if (size1 != size1_ap) + { return; } oldnumel = size1*size2; var->size[1] = size2 + size2_ap; emxEnsureCapacity(var, oldnumel, (int)sizeof(double)); - for (int i=0; idata[size1*size2 + i] = varadd->data[i]; } } -void Debug_Array(const double *array, int arraysize) { +void Debug_Array(const double *array, int arraysize) +{ int i; - for (i = 0; i < arraysize; i++) { + for (i = 0; i < arraysize; i++) + { //char chararray[500]; // You had better have room for what you are sprintf()ing! //sprintf(chararray, "%.3f", array[i]); //std::cout << chararray; printf("%.3f", array[i]); - if (i < arraysize - 1) { + if (i < arraysize - 1) + { //std::cout << " , "; printf(" , "); } } } -void Debug_Matrix2D(const tMatrix2D *emx) { +void Debug_Matrix2D(const tMatrix2D *emx) +{ int size1; int size2; int j; @@ -4025,41 +4902,14 @@ void Debug_Matrix2D(const tMatrix2D *emx) { size2 = Matrix2D_Get_ncols(emx); printf("Matrix size = [%i, %i]\n", size1, size2); //std::out << "Matrix size = [%i, %i]" << size1 << " " << size2 << "]\n"; - for (j = 0; j 0) { - POSE_TR(pose_tr, pose); - printf("Pose size = [4x4]\n"); - //std::cout << "Pose size = [4x4]\n"; - for (j = 0; j < 4; j++) { - Debug_Array(pose_tr + j * 4, 4); - printf("\n"); - //std::cout << "\n"; - } - } - else { - POSE_2_XYZWPR(xyzwpr, pose); - //std::cout << "XYZWPR = [ "; - printf("XYZWPR = [ "); - Debug_Array(xyzwpr, 6); - printf(" ]\n"); - //std::cout << " ]\n"; - } -} -*/ - - - #ifndef RDK_SKIP_NAMESPACE } diff --git a/PluginOpenGL-Shaders/robodk_api/robodk_api.h b/PluginOpenGL-Shaders/robodk_api/robodk_api.h index 671d1c3..e5f4e8b 100644 --- a/PluginOpenGL-Shaders/robodk_api/robodk_api.h +++ b/PluginOpenGL-Shaders/robodk_api/robodk_api.h @@ -1,8 +1,8 @@ -// Copyright 2015-2020 - RoboDK Inc. - https://robodk.com/ +// Copyright 2015-2021 - RoboDK Inc. - https://robodk.com/ // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at -// https://www.apache.org/licenses/LICENSE-2.0 +// http://www.apache.org/licenses/LICENSE-2.0 // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. @@ -30,7 +30,7 @@ // https://robodk.com/help#PostProcessor // // Visit the Matrix and Quaternions FAQ for more information about pose/homogeneous transformations -// https://www.j3d.org/matrix_faq/matrfaq_latest.html +// http://www.j3d.org/matrix_faq/matrfaq_latest.html // //--------------------------------------------- // TIPS: @@ -69,7 +69,7 @@ * The RoboDK class defines the interface to the RoboDK API. The original Python reference is available here: https://robodk.com/doc/en/RoboDK-API.html#RoboDKAPI. * * More information about the RoboDK API is available here: -* - Python Reference: https://robodk.com/doc/en/PythonAPI/index.html +* - Python Reference: https://robodk.com/doc/en/PythonAPI/robodk.html#robolink-py * * \subsection LinkItem Item class * The Item class can be used to operate on any item available in the RoboDK tree. Use functions such as class: IRoboDK::getItem or class: IRoboDK::getItemList to retrieve items from the RoboDK station tree. @@ -217,8 +217,8 @@ * * This list provides some useful links and tips for programming with Qt: * - Double click the RoboDK-API-Cpp-Sample.pro file to open the example project using Qt Creator. -* - Use Qt signal/slots mechanism for action/button callbacks (https://doc.qt.io/qt-5/signalsandslots.html). Signals and slots are thread safe. -* - Wrap your strings using tr("your string") or QObject::tr("your string") to allow translation using Qt Linguist. For more information: https://doc.qt.io/qt-5/qtlinguist-index.html. +* - Use Qt signal/slots mechanism for action/button callbacks (http://doc.qt.io/qt-5/signalsandslots.html). Signals and slots are thread safe. +* - Wrap your strings using tr("your string") or QObject::tr("your string") to allow translation using Qt Linguist. For more information: http://doc.qt.io/qt-5/qtlinguist-index.html. * - If you experience strange build issues it may be useful to delete the build folder that is automatically created to force a new build. * - If you experience strange plugin load issues in RoboDK it is recommended to delete the libraries and create the plugin library with a new build. * - More information about Qt: https://www.qt.io/. @@ -227,7 +227,7 @@ * \section LinkRefs Useful Links * Useful links involving the RoboDK API: * - Standard RoboDK API Introduction: https://robodk.com/doc/en/RoboDK-API.html#RoboDKAPI. -* - Standard RoboDK API Reference (based on Python): https://robodk.com/doc/en/PythonAPI/index.html. +* - Standard RoboDK API Reference (based on Python): https://robodk.com/doc/en/PythonAPI/robodk.html#robolink-py. * - Latest RoboDK API on GitHub (you'll find RoboDK C++ in a subfolder): https://github.com/RoboDK/RoboDK-API. * - RoboDK API Introductory video: https://www.youtube.com/watch?v=3I6OK1Kd2Eo. * - RoboDK API using the Plugin Interface: https://robodk.com/doc/en/PlugIns/index.html. @@ -263,7 +263,7 @@ #include -class QTcpSocket; +class QAbstractSocket; #ifndef RDK_SKIP_NAMESPACE @@ -275,6 +275,7 @@ namespace RoboDK_API { class Item; class RoboDK; +struct GetPointsResult; /// maximum size of robot joints (maximum allowed degrees of freedom for a robot) @@ -488,12 +489,10 @@ class ROBODK tJoints { double _Values[RDK_SIZE_JOINTS_MAX]; /// joint values (floats, used to return a copy as a float pointer) - float _ValuesF[RDK_SIZE_JOINTS_MAX]; + mutable float _ValuesF[RDK_SIZE_JOINTS_MAX]; }; - - /// \brief The Mat class represents a 4x4 pose matrix. The main purpose of this object is to represent a pose in the 3D space (position and orientation). /// In other words, a pose is a 4x4 matrix that represents the position and orientation of one reference frame with respect to another one, in the 3D space. /// Poses are commonly used in robotics to place objects, reference frames and targets with respect to each other. @@ -742,6 +741,7 @@ class ROBODK Mat : public QMatrix4x4 { /// static Mat rotz(double rz); + private: /// Flags if a matrix is not valid. bool _valid; @@ -749,7 +749,7 @@ class ROBODK Mat : public QMatrix4x4 { // this is a dummy variable to easily obtain a pointer to a 16-double-array for matrix multiplications private: /// Copy of the data as a double array. - double _ddata16[16]; + mutable double _ddata16[16]; }; @@ -762,7 +762,8 @@ class ROBODK RoboDK { friend class RoboDK_API::Item; public: - RoboDK(const QString &robodk_ip="", int com_port=-1, const QString &args="", const QString &path=""); + explicit RoboDK(QAbstractSocket *socket, bool fUseExceptions = false); + RoboDK(const QString &robodk_ip="", int com_port=-1, const QString &args="", const QString &path="", bool fUseExceptions = false); ~RoboDK(); quint64 ProcessID(); @@ -1072,7 +1073,7 @@ class ROBODK RoboDK { /// /// List of robot link IDs that are in collision (0 for objects and tools). /// List of items that are in a collision state. - QList getCollisionItems(QList link_id_list); + QList getCollisionItems(QList& link_id_list); /// /// Sets the current simulation speed. Set the speed to 1 for a real-time simulation. The slowest speed allowed is 0.001 times the real speed. Set to a high value (>100) for fast simulation results. @@ -1218,6 +1219,32 @@ class ROBODK RoboDK { /// View pose. Mat ViewPose(); + /// + /// Add a simulated 2D or depth camera as an item. Use Delete to delete it. + /// + /// Reference or object to attach the camera + /// Camera settings as described here: https://robodk.com/doc/en/PythonAPI/robodk.html#robodk.robolink.Robolink.Cam2D_Add + /// Provide an existing camera item to modify it + /// Camera item + Item Cam2D_Add(const Item &item_object, const QString &cam_params, const Item *cam_item=nullptr); + + /// + /// Take a snapshot of a simulated camera and save it as an image. + /// + /// file path to save. Formats supported include PNG, JPEG, TIFF, ... + /// Camera item + /// additional options (use, "Grayscale", "Depth" or "Color" to modify the output of a camera snapshot) + /// Returns 1 if success, 0 otherwise + int Cam2D_Snapshot(const QString &file_save_img, const Item &cam_item, const QString ¶ms=""); + + /// + /// Set the camera parameters. + /// + /// Camera parameters: The same parameters that can be set using Cam2D_Add() + /// Camera item + /// Returns 1 if success, 0 otherwise + int Cam2D_SetParams(const QString &cam_params, const Item &cam_item); + /// /// Set the nominal robot parameters. /// @@ -1239,6 +1266,13 @@ class ROBODK RoboDK { /// Item under the mouse cursor. Item getCursorXYZ(int x = -1, int y = -1, tXYZ xyzStation = nullptr); + /// + /// Retrieves the object under the mouse cursor. + /// + /// Set to FEATURE_HOVER_OBJECT_MESH to retrieve object under the mouse cursor, the selected feature and mesh, or FEATURE_HOVER_OBJECT if you don't need the mesh (faster). + /// GetPointsResult object. + GetPointsResult GetPoints(int featureType = FEATURE_HOVER_OBJECT_MESH); + /// /// Returns the license as a readable string (same name shown in the RoboDK's title bar, on top of the main menu). /// @@ -1256,6 +1290,16 @@ class ROBODK RoboDK { /// /// List of items to set as selected void setSelection(QList list_items); + + /// + /// Load or unload the specified plugin (path to DLL, dylib or SO file). If the plugin is already loaded it will unload the plugin and reload it. Pass an empty plugin_name to reload all plugins. + /// + void PluginLoad(const QString &pluginName, int load=1); + + /// + /// Send a specific command to a RoboDK plugin. The command and value (optional) must be handled by your plugin. It returns the result as a string. + /// + QString PluginCommand(const QString &pluginName, const QString &pluginCommand, const QString &pluginValue=""); /// /// Show the popup menu to create the ISO9283 path for position accuracy, repeatability and path accuracy performance testing. @@ -1542,7 +1586,24 @@ class ROBODK RoboDK { FEATURE_CURVE = 2, /// Point selection - FEATURE_POINT = 3 + FEATURE_POINT = 3, + + /// Object mesh (using ID) feature type flag + FEATURE_OBJECT_MESH = 7, + + /// Surface preview feature type flag + FEATURE_SURFACE_PREVIEW = 8, + + /// Mesh (under the mouse cursor) feature flag + FEATURE_MESH = 9, + + // The following do not require providing an object + + /// Object mesh (under the mouse cursor) feature type flag + FEATURE_HOVER_OBJECT_MESH = 10, + + /// Object feature (under the mouse cursor) feature type flag + FEATURE_HOVER_OBJECT = 11 }; /// Spray gun simulation: @@ -1660,8 +1721,10 @@ class ROBODK RoboDK { }; private: - QTcpSocket *_COM; - QTcpSocket *_COM_EVT; + QAbstractSocket *_COM; + bool _USE_EXCPETIONS; + bool _OWN_SOCKET; + QAbstractSocket *_COM_EVT; QString _IP; int _PORT; int _TIMEOUT; @@ -1678,25 +1741,27 @@ class ROBODK RoboDK { bool _check_connection(); bool _check_status(); - bool _waitline(QTcpSocket *com = nullptr); - QString _recv_Line(QTcpSocket *com = nullptr);//QString &string); - bool _send_Line(const QString &string,QTcpSocket *com = nullptr); - int _recv_Int(QTcpSocket *com = nullptr);//qint32 &value); - bool _send_Int(const qint32 value,QTcpSocket *com = nullptr); - Item _recv_Item(QTcpSocket *com = nullptr);//Item *item); + bool _waitline(QAbstractSocket *com = nullptr); + QString _recv_Line(QAbstractSocket *com = nullptr);//QString &string); + bool _send_Line(const QString &string,QAbstractSocket *com = nullptr); + int _recv_Int(QAbstractSocket *com = nullptr);//qint32 &value); + bool _send_Int(const qint32 value,QAbstractSocket *com = nullptr); + Item _recv_Item(QAbstractSocket *com = nullptr);//Item *item); bool _send_Item(const Item *item); bool _send_Item(const Item &item); - Mat _recv_Pose(QTcpSocket *com = nullptr);//Mat &pose); + Mat _recv_Pose(QAbstractSocket *com = nullptr);//Mat &pose); bool _send_Pose(const Mat &pose); bool _recv_XYZ(tXYZ pos); bool _send_XYZ(const tXYZ pos); - bool _recv_Array(double *values, int *psize=nullptr,QTcpSocket *com = nullptr); + bool _recv_Array(double *values, int *psize=nullptr,QAbstractSocket *com = nullptr); bool _send_Array(const double *values, int nvalues); bool _recv_Array(tJoints *jnts); bool _send_Array(const tJoints *jnts); bool _send_Array(const Mat *mat); bool _recv_Matrix2D(tMatrix2D **mat); bool _send_Matrix2D(tMatrix2D *mat); + bool _send_Bytes(const QByteArray &data, QAbstractSocket *com = nullptr); + QByteArray _recv_Bytes(QAbstractSocket *com = nullptr); void _moveX(const Item *target, const tJoints *joints, const Mat *mat_target, const Item *itemrobot, int movetype, bool blocking); @@ -1932,6 +1997,25 @@ class ROBODK Item { /// scale to apply as [scale_x, scale_y, scale_z] void Scale(double scale_xyz[3]); + + /// + /// Retrieves the point under the mouse cursor, a curve or the 3D points of an object. + /// The points are provided in [XYZijk] format in relative coordinates. + /// The XYZ are the local point coordinate and ijk is the normal of the surface. + /// + /// The type of geometry (FEATURE_SURFACE, FEATURE_POINT, ...). Set to FEATURE_SURFACE and if not point or curve was selected, the name of the geometry will be 'point on surface'. + /// The internal ID to retrieve the right geometry from the object (use SelectedFeature). + /// GetPointsResult object. + GetPointsResult GetPoints(int featureType = RoboDK::FEATURE_SURFACE, int featureId = 0); + + /// + /// Retrieve the currently selected feature for this object. + /// + /// The type of geometry, FEATURE_SURFACE, FEATURE_POINT, ... + /// The internal ID to retrieve the raw geometry (use GetPoints) + /// True if the object is selected + bool SelectedFeature(int &featureType, int &featureId); + /// /// Update the robot milling path input and parameters. Parameter input can be an NC file (G-code or APT file) or an object item in RoboDK. A curve or a point follow project will be automatically set up for a robot manufacturing project. ///
Tip: Use getLink() and setLink() to get/set the robot tool, reference frame, robot and program linked to the project. @@ -2429,6 +2513,19 @@ class ROBODK Item { /// QString setParam(const QString ¶m, const QString &value); + /// + /// Set a specific binary item parameter. + /// + /// item parameter + /// binary data + void setParam(const QString ¶m, const QByteArray &value); + + /// + /// Get a specific binary item parameter. + /// + /// item parameter + QByteArray getParam(const QString ¶m); + /// /// Disconnect from the RoboDK API. This flushes any pending program generation. /// @@ -2452,6 +2549,17 @@ class ROBODK Item { }; +/// \brief The GetPointsResult method represents the results of executing the GetPoints function. +struct GetPointsResult +{ + Item item; + int featureType; + int featureId; + QString featureName; + tMatrix2D* points; +}; + + /// Translation matrix class: Mat::transl. ROBODK Mat transl(double x, double y, double z); diff --git a/PluginRealTime/pluginexample.cpp b/PluginRealTime/pluginexample.cpp index 0294ce4..0defd54 100644 --- a/PluginRealTime/pluginexample.cpp +++ b/PluginRealTime/pluginexample.cpp @@ -218,7 +218,7 @@ void PluginExample::callback_information(){ // Perform some timing tests using the RoboDK API RDK->ShowMessage("Starting timing tests", false); - QString text_message_html("Plugin Timing Tests Summary on " + QDateTime::currentDateTime().toString(Qt::SystemLocaleLongDate) + ":
"); + QString text_message_html("Plugin Timing Tests Summary on " + QDateTime::currentDateTime().toString() + ":
"); int ntests=10000; //Item robot = RDK->getItem("", IItem::ITEM_TYPE_ROBOT); diff --git a/PluginRoboUI/roboui.cpp b/PluginRoboUI/roboui.cpp index 0323952..6b488f5 100644 --- a/PluginRoboUI/roboui.cpp +++ b/PluginRoboUI/roboui.cpp @@ -226,7 +226,12 @@ bool RoboUI::eventFilter(QObject* object, QEvent* event) if (object == _renderWindow && event->type() == QEvent::MouseMove) { QMouseEvent *mouseEvent = static_cast(event); +#if (QT_VERSION < QT_VERSION_CHECK(6, 0, 0)) QPointF pos = mouseEvent->localPos(); +#else + QPointF pos = mouseEvent->position(); +#endif + io.AddMousePosEvent(pos.x(), pos.y()); _rdk->Render(IRoboDK::RenderScreen); //ImGui::UpdateInputEvents() @@ -240,7 +245,12 @@ bool RoboUI::eventFilter(QObject* object, QEvent* event) event->type() == QEvent::MouseButtonRelease)) { QMouseEvent *mouseEvent = static_cast(event); +#if (QT_VERSION < QT_VERSION_CHECK(6, 0, 0)) QPointF pos = mouseEvent->localPos(); +#else + QPointF pos = mouseEvent->position(); +#endif + io.AddMousePosEvent(pos.x(), pos.y()); int button = -1; switch (mouseEvent->button()) From 6564221a975a7c84bacb19147327bf02dbf51ccf Mon Sep 17 00:00:00 2001 From: VDm Date: Mon, 12 May 2025 16:41:11 +0400 Subject: [PATCH 4/4] fix: restore support for Qt 5 --- Plugin-OPC-UA/pluginopcua.cpp | 3 ++- PluginBallbarTracker/PluginBallbarTracker.cpp | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/Plugin-OPC-UA/pluginopcua.cpp b/Plugin-OPC-UA/pluginopcua.cpp index 1d3d891..d10794b 100644 --- a/Plugin-OPC-UA/pluginopcua.cpp +++ b/Plugin-OPC-UA/pluginopcua.cpp @@ -17,6 +17,7 @@ #include #include #include +#include // only for Sleep() @@ -348,7 +349,7 @@ void PluginOPCUA::SaveSettings(){ qDebug() << "Saving OPC-UA plugin settings..."; QByteArray data; - QDataStream ds(&data, QDataStream::WriteOnly); + QDataStream ds(&data, QIODevice::WriteOnly); qint64 version = 2; ds << version; ds << Server->Port; diff --git a/PluginBallbarTracker/PluginBallbarTracker.cpp b/PluginBallbarTracker/PluginBallbarTracker.cpp index fc633bc..d8b61d0 100644 --- a/PluginBallbarTracker/PluginBallbarTracker.cpp +++ b/PluginBallbarTracker/PluginBallbarTracker.cpp @@ -329,7 +329,8 @@ void PluginBallbarTracker::update_ballbar_pose(){ double rho = 90.0 - qRadiansToDegrees(qAcos(pillar_2_tool_vec.z() / std::max(1e-6, r))); // Theta θ - double theta = 180 - qRadiansToDegrees(qAcos(pillar_2_tool_vec.x() / std::max(1e-6f, qSqrt(pillar_2_tool_vec.x() * pillar_2_tool_vec.x() + pillar_2_tool_vec.y() * pillar_2_tool_vec.y())))); + double xy_length = qSqrt(pillar_2_tool_vec.x() * pillar_2_tool_vec.x() + pillar_2_tool_vec.y() * pillar_2_tool_vec.y()); + double theta = 180 - qRadiansToDegrees(qAcos(pillar_2_tool_vec.x() / std::max(1e-6, xy_length))); if (pillar_2_tool_vec.y() > 0){ theta = -theta; }