344 lines
13 KiB
C++
344 lines
13 KiB
C++
#include "roboglue_gui.h"
|
|
#include "ui_roboglue_gui.h"
|
|
|
|
RoboGlue_GUI::RoboGlue_GUI(RoboGlue_SHARED *mem) : m(mem) {
|
|
|
|
//////// SETUP LOGGER //////////
|
|
modlog = spdlog::stdout_logger_mt("RoboGlue_guilog");
|
|
spdlog::set_level(spdlog::level::debug);
|
|
modlog->info("GUI Started");
|
|
|
|
//////// SETUP USER INTERFACE ///////
|
|
ui=new Ui::RoboGlue_GUI;
|
|
ui->setupUi(this);
|
|
ui->frm_move->setStyleSheet("border-image: url(:/images/PiantaRobot_Alto.png) 0 0 0 0 stretch stretch");
|
|
|
|
//////// RESTORE UI STATE ////////////
|
|
ui->txt_roboIp->setText(QString(m->comSettings.connection.robotIp.c_str()));
|
|
ui->txt_robotPort->setText(QString::number(m->comSettings.connection.robotPort));
|
|
ui->chk_autoConnect->setChecked(m->comSettings.connection.autoConnect);
|
|
|
|
//////// ENABLE HOVERING EVENT OVER DEFINED AREA //////////////
|
|
ui->frm_move->installEventFilter(this);
|
|
|
|
//////// CONNECT SIGNALS & SLOTS ///////
|
|
connect(this, SIGNAL(destroyed()),
|
|
this, SLOT(deleteLater()), Qt::ConnectionType::QueuedConnection);
|
|
connect(this, SIGNAL(pad_hoverEvent(QEvent *)),
|
|
this, SLOT(on_pad_hoverEvent(QEvent *)));
|
|
|
|
/////// READ NAME DEFINITION JSON FILE //////////
|
|
try {
|
|
QFile fp(QString(m->guiSettings.namePath.c_str()));
|
|
fp.open(QFile::ReadOnly | QFile::Text);
|
|
QByteArray rawdata= fp.readAll();
|
|
std::string strdata= rawdata.toStdString();
|
|
nameDefinitions = json::parse(strdata.c_str());
|
|
fp.close();
|
|
} catch (json::exception e){
|
|
modlog->error("Unable to parse name definitions:\n\t{}",e.what());
|
|
}
|
|
}
|
|
|
|
RoboGlue_GUI::~RoboGlue_GUI() {
|
|
spdlog::drop("RoboGlue_guilog");
|
|
delete ui;
|
|
}
|
|
|
|
bool RoboGlue_GUI::eventFilter(QObject *obj, QEvent *event){
|
|
if(obj == static_cast<QObject *>(ui->frm_move))
|
|
if(event->type()==event->MouseMove || event->type()==event->MouseButtonPress
|
|
|| event->type()==event->KeyPress){
|
|
event->accept();
|
|
emit pad_hoverEvent(event);
|
|
}
|
|
return QWidget::eventFilter(obj, event);
|
|
}
|
|
|
|
QVariantMap RoboGlue_GUI::getLockAxes() {
|
|
QVariantMap param;
|
|
param["lx"]=ui->chk_lockX->isChecked();
|
|
param["ly"]=ui->chK_lockY->isChecked();
|
|
param["lz"]=ui->chk_lockZ->isChecked();
|
|
param["lrx"]=ui->chk_lockRX->isChecked();
|
|
param["lry"]=ui->chk_lockRY->isChecked();
|
|
param["lrz"]=ui->chk_lockRZ->isChecked();
|
|
return param;
|
|
}
|
|
|
|
void RoboGlue_GUI::enableLockAxes() {
|
|
ui->chk_lockX->setEnabled(true);
|
|
ui->chK_lockY->setEnabled(true);
|
|
ui->chk_lockZ->setEnabled(true);
|
|
ui->chk_lockRX->setEnabled(true);
|
|
ui->chk_lockRY->setEnabled(true);
|
|
ui->chk_lockRZ->setEnabled(true);
|
|
}
|
|
|
|
void RoboGlue_GUI::disableLockAxes(){
|
|
ui->chk_lockX->setEnabled(false);
|
|
ui->chK_lockY->setEnabled(false);
|
|
ui->chk_lockZ->setEnabled(false);
|
|
ui->chk_lockRX->setEnabled(false);
|
|
ui->chk_lockRY->setEnabled(false);
|
|
ui->chk_lockRZ->setEnabled(false);
|
|
}
|
|
|
|
////////////////////////////////////////////////
|
|
////////////INTERNAL PRIVATE SLOTS//////////////
|
|
////////////////////////////////////////////////
|
|
void RoboGlue_GUI::on_commonStatusChange() {
|
|
modlog->trace("on_commonStatusChange Received");
|
|
}
|
|
|
|
void RoboGlue_GUI::on_pad_hoverEvent(QEvent* e) {
|
|
QMouseEvent *mouse = static_cast<QMouseEvent *>(e);
|
|
QKeyEvent *key = static_cast<QKeyEvent *>(e);
|
|
static float zpos=0.5;
|
|
static bool trackEnable = false;
|
|
static QPointF lastPos;
|
|
QPointF p;
|
|
float limit = m->comSettings.kine.maxReach;
|
|
|
|
modlog->trace("event: {}",e->type());
|
|
if(e->type() == e->MouseMove){
|
|
QPointF pos = mouse->localPos();
|
|
qreal x = boost::algorithm::clamp(pos.x(),0,ui->frm_move->size().width());
|
|
qreal y = boost::algorithm::clamp(pos.y(),0,ui->frm_move->size().height());
|
|
p = QPointF(x,y);
|
|
} else if(e->type() == e->MouseButtonPress &&
|
|
(m->commonStatus.isRecording || m->commonStatus.isRealtime)) {
|
|
modlog->trace("mousebuttonevent {}", mouse->button());
|
|
if(!trackEnable){
|
|
trackEnable=true;
|
|
//mouse->setLocalPos(lastPos);
|
|
ui->frm_move->setFocus();
|
|
if (ui->btn_record->isEnabled()) ui->btn_record->setEnabled(false);
|
|
if (ui->btn_realtime->isEnabled()) ui->btn_realtime->setEnabled(false);
|
|
} else if(trackEnable){
|
|
trackEnable=false;
|
|
lastPos=mouse->pos();
|
|
if (m->commonStatus.isRecording) ui->btn_record->setEnabled(true);
|
|
if (m->commonStatus.isRealtime) ui->btn_realtime->setEnabled(true);
|
|
}
|
|
} else if (e->type()==e->KeyPress) {
|
|
modlog->trace("keyevent {}", key->key());
|
|
if(key->key() == 16777235)
|
|
if (zpos < limit) zpos=zpos+0.1;
|
|
if(key->key() == 16777237)
|
|
if (zpos > 0) zpos=zpos-0.1;
|
|
}
|
|
|
|
if(trackEnable && (m->robotVariables.at("RDY") == 1)) {
|
|
QTransform t=QTransform::fromTranslate(-ui->frm_move->size().width()/2,
|
|
-ui->frm_move->size().height()/2);
|
|
t = t * QTransform::fromScale(-limit/t.dx(),
|
|
-limit/t.dy());
|
|
QPointF q = t.map(p);
|
|
ui->lbl_posX->setText(QString("X: %1m").arg(q.x(),6));
|
|
ui->lbl_posY->setText(QString("Y: %1m").arg(-q.y(),6));
|
|
ui->lbl_posZ->setText(QString("Z: %1m").arg(zpos,6));
|
|
|
|
// FIXME
|
|
///////////////////////////////////////////////////////
|
|
////////////////// TEMPORARY SEND COORDINATES ////////
|
|
////////////////// TO ROBOT /////////////////////////
|
|
/// (TO BE IMPLEMENTED IN MAIN FOR KINEMATICKS CHECK)
|
|
if (q.x()>-limit && q.x()<limit && q.y()>-limit && q.y()<limit && zpos > 0 && zpos < limit){
|
|
QList<double> sp = {q.x(),-q.y(),zpos,1.57,-2.68,0};
|
|
emit sendROScoordinates(sp);
|
|
}
|
|
}
|
|
}
|
|
|
|
void RoboGlue_GUI::on_btn_sendCommand_clicked(){
|
|
modlog->debug("robotSendCommand Requested");
|
|
emit robotSendURscript(ui->txt_commandToSend->text());
|
|
}
|
|
|
|
|
|
void RoboGlue_GUI::on_btn_robotConnect_clicked() {
|
|
modlog->debug("robotConnect Requested");
|
|
emit robotConnect(ui->txt_roboIp->text(),
|
|
ui->txt_robotPort->text().toUInt(),
|
|
5); // default retry attempt to 5
|
|
}
|
|
|
|
void RoboGlue_GUI::on_btn_robotDisconnect_clicked() {
|
|
modlog->debug("robotDisconnect Requested");
|
|
emit robotDisconnect();
|
|
}
|
|
|
|
void RoboGlue_GUI::on_chk_autoConnect_clicked(bool checked) {
|
|
m->comSettings.connection.autoConnect = checked;
|
|
m->settings->beginGroup("robot");
|
|
m->settings->beginGroup("connection");
|
|
m->settings->setValue("autoconnect", checked);
|
|
m->settings->endGroup();
|
|
m->settings->endGroup();
|
|
}
|
|
|
|
void RoboGlue_GUI::on_btn_record_clicked() {
|
|
static bool recording = false;
|
|
QVariantMap param;
|
|
QVariantMap metadata;
|
|
modlog->debug("robotTrajectory Record");
|
|
param.insert("action", "");
|
|
if (!recording) {
|
|
m->commonStatus.isRecording = true;
|
|
recording = true;
|
|
param["action"] = "start";
|
|
param["lock"] = getLockAxes();
|
|
metadata["name"] = ui->txt_fileName->text();
|
|
metadata["code"] = QUuid::createUuid().toString();
|
|
metadata["ds"] = ui->spn_ds->value();
|
|
metadata["dt"] = ui->spn_dt->value();
|
|
param["metadata"] = metadata;
|
|
ui->btn_play->setEnabled(false);
|
|
ui->btn_realtime->setEnabled(false);
|
|
disableLockAxes();
|
|
} else {
|
|
m->commonStatus.isRecording = false;
|
|
recording = false;
|
|
param["action"]="stop";
|
|
ui->btn_play->setEnabled(true);
|
|
ui->btn_realtime->setEnabled(true);
|
|
enableLockAxes();
|
|
}
|
|
emit m->commonStatusChange();
|
|
emit sendROScommand("RECORD", param);
|
|
}
|
|
|
|
void RoboGlue_GUI::on_btn_play_clicked() {
|
|
static bool playing = false;
|
|
QVariantMap param;
|
|
QVariantMap metadata;
|
|
modlog->debug("robotTrajectory Replay");
|
|
if (!playing) {
|
|
m->commonStatus.isPlaying = true;
|
|
playing = true;
|
|
metadata["name"]=ui->txt_fileName->text();
|
|
param["action"]="start";
|
|
param["metadata"] = metadata;
|
|
param["lock"] = getLockAxes();
|
|
ui->btn_record->setEnabled(false);
|
|
ui->btn_realtime->setEnabled(false);
|
|
ui->btn_play->setText("Stop");
|
|
ui->txt_fileName->setEnabled(false);
|
|
disableLockAxes();
|
|
} else {
|
|
m->commonStatus.isPlaying = false;
|
|
playing = false;
|
|
param["action"] = "stop";
|
|
ui->btn_record->setEnabled(true);
|
|
ui->btn_realtime->setEnabled(true);
|
|
ui->btn_play->setText("Play");
|
|
ui->txt_fileName->setEnabled(true);
|
|
enableLockAxes();
|
|
}
|
|
emit m->commonStatusChange();
|
|
emit sendROScommand("PLAY", param);
|
|
}
|
|
|
|
void RoboGlue_GUI::on_btn_open_clicked() {
|
|
static bool fileOpen = false;
|
|
QVariantMap param;
|
|
QVariantMap metadata;
|
|
modlog->debug("robotTragectory Open");
|
|
if(!fileOpen){
|
|
m->commonStatus.isFileOpen = true;
|
|
fileOpen = true;
|
|
metadata["name"] = ui->txt_fileName->text();
|
|
metadata["plot"] = true;
|
|
param["action"] = "open";
|
|
param["metadata"] = metadata;
|
|
ui->btn_record->setEnabled(false);
|
|
ui->btn_realtime->setEnabled(false);
|
|
ui->btn_open->setText("Close");
|
|
ui->txt_fileName->setEnabled(false);
|
|
} else {
|
|
m->commonStatus.isFileOpen = false;
|
|
fileOpen = false;
|
|
param["action"] = "close";
|
|
ui->btn_record->setEnabled(true);
|
|
ui->btn_realtime->setEnabled(true);
|
|
ui->btn_open->setText("Open");
|
|
ui->txt_fileName->setEnabled(true);
|
|
}
|
|
emit m->commonStatusChange();
|
|
emit sendROScommand("OPEN", param);
|
|
}
|
|
|
|
void RoboGlue_GUI::on_btn_realtime_clicked() {
|
|
static bool realtime = false;
|
|
QVariantMap param;
|
|
QVariantMap lock;
|
|
modlog->debug("robotTrajectory Realtime");
|
|
if (!realtime){
|
|
realtime = true;
|
|
m->commonStatus.isRealtime = true;
|
|
param["action"] = "start";
|
|
param["lock"] = getLockAxes();
|
|
if (ui->rad_pos->isChecked()) param["mode"] = "pos";
|
|
else if (ui->rad_vel->isChecked()) param["mode"] = "vel";
|
|
ui->btn_play->setEnabled(false);
|
|
ui->btn_record->setEnabled(false);
|
|
disableLockAxes();
|
|
} else {
|
|
m->commonStatus.isRealtime = false;
|
|
realtime = false;
|
|
param["action"]="stop";
|
|
ui->btn_play->setEnabled(true);
|
|
ui->btn_record->setEnabled(true);
|
|
enableLockAxes();
|
|
}
|
|
emit m->commonStatusChange();
|
|
emit sendROScommand("REALTIME", param);
|
|
}
|
|
////////////////////////////////////////////////
|
|
////////END INTERNAL PRIVATE SLOTS//////////////
|
|
////////////////////////////////////////////////
|
|
|
|
////////////////////////////////////////////////
|
|
////////////EXTERNAL PUBLIC SLOTS///////////////
|
|
////////////////////////////////////////////////
|
|
void RoboGlue_GUI::on_newRobotData() {
|
|
m->mutex.lock();
|
|
robotModeData_t mode = m->robotData.robotMode;
|
|
cartesianInfo_t cart = m->robotData.cartesianInfo;
|
|
jointData_t joint = m->robotData.jointData;
|
|
m->mutex.unlock();
|
|
|
|
//////////// mode info //////////
|
|
try {
|
|
ui->lbl_connected->setText(QString::number(mode.isRealRobotConnected));
|
|
ui->lbl_robotMode->setText(QString(nameDefinitions["robotModes"][std::to_string(mode.robotMode).c_str()].get<std::string>().c_str()));
|
|
ui->lbl_powerOn->setText(QString::number(mode.isRobotPowerOn));
|
|
ui->lbl_emeStop->setText(QString::number(mode.isEmergencyStopped));
|
|
ui->lbl_proStop->setText(QString::number(mode.isProtectiveStopped));
|
|
ui->lbl_programRunning->setText(QString::number(mode.isProgramRunning));
|
|
ui->lbl_controlMode->setText(QString(nameDefinitions["controlModes"][std::to_string(mode.controlMode).c_str()].get<std::string>().c_str()));
|
|
} catch (json::exception e){
|
|
modlog->error("Unable to find name: \n{}",e.what());
|
|
}
|
|
//////////// cartesian info //////////
|
|
ui->lbl_cartX->setText("X: "+QString::number(cart.cartPosition.X*1000)+QString(" mm"));
|
|
ui->lbl_cartY->setText("Y: "+QString::number(cart.cartPosition.Y*1000)+QString(" mm"));
|
|
ui->lbl_cartZ->setText("Z: "+QString::number(cart.cartPosition.Z*1000)+QString(" mm"));
|
|
ui->lbl_cartRX->setText("Rx: "+QString::number(cart.cartPosition.RX,'g',3)+QString(" rad"));
|
|
ui->lbl_cartRY->setText("Ry: "+QString::number(cart.cartPosition.RY,'g',3)+QString(" rad"));
|
|
ui->lbl_cartRZ->setText("Rz: "+QString::number(cart.cartPosition.RZ,'g',3)+QString(" rad"));
|
|
//////////// joint info //////////
|
|
ui->lbl_J1->setText("Base: " + QString::number(joint.jointParam[0].actual)+" rad");
|
|
ui->lbl_J2->setText("Shoulder: " + QString::number(joint.jointParam[1].actual)+" rad");
|
|
ui->lbl_J3->setText("Elbow: " + QString::number(joint.jointParam[2].actual)+" rad");
|
|
ui->lbl_J4->setText("Wrist1: " + QString::number(joint.jointParam[3].actual)+" rad");
|
|
ui->lbl_J5->setText("Wrist2: " + QString::number(joint.jointParam[4].actual)+" rad");
|
|
ui->lbl_J6->setText("Wrist3: " + QString::number(joint.jointParam[5].actual)+" rad");
|
|
}
|
|
////////////////////////////////////////////////
|
|
//////// END EXTERNAL PUBLIC SLOTS /////////////
|
|
////////////////////////////////////////////////
|
|
|
|
|