Commit Iniziale
This commit is contained in:
343
gui/roboglue_gui.cpp
Normal file
343
gui/roboglue_gui.cpp
Normal file
@@ -0,0 +1,343 @@
|
||||
#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 /////////////
|
||||
////////////////////////////////////////////////
|
||||
|
||||
|
||||
Reference in New Issue
Block a user