Trying to control Robot from Joypad GUI in ROS
-
So what I want to do is to control my turtlebot in gazebo using the output from the joypad GUI that I made. But I am don't know how can I get the x and y coordinates of my joypad knob and somehow pass it to cmd_vel topic for direction and velocity. I have attached my code below:
joypad.h:#pragma once #include <QWidget> #include <ros/ros.h> class QPropertyAnimation; class QParallelAnimationGroup; namespace Ui { class joypad; } class JoyPad : public QWidget { Q_OBJECT Q_PROPERTY(float x READ x WRITE setX NOTIFY xChanged) Q_PROPERTY(float y READ y WRITE setY NOTIFY yChanged) public: explicit JoyPad(QWidget *parent = Q_NULLPTR, Qt::WindowFlags f = Qt::WindowFlags()); float x() const; float y() const; signals: void xChanged(float value); void yChanged(float value); public slots: void setX(float value); void setY(float value); // Add or remove the knob return animations in x or y- direction. void removeXAnimation(); void addXAnimation(); void removeYAnimation(); void addYAnimation(); /* Set the alignment of the quadratic content if the widgets geometry isn quadratic. * Flags can be combined eg. setAlignment(Qt::AlignLeft | Qt::AlignBottom); */ void setAlignment(Qt::Alignment f); private: Ui::joypad *ui; void resizeEvent(QResizeEvent *event) override; virtual void paintEvent(QPaintEvent *event) override; void mousePressEvent(QMouseEvent *event) override; void mouseReleaseEvent(QMouseEvent *event) override; void mouseMoveEvent(QMouseEvent *event) override; float m_x; float m_y; QParallelAnimationGroup *m_returnAnimation; QPropertyAnimation *m_xAnimation; QPropertyAnimation *m_yAnimation; QRectF m_bounds; QRectF m_knopBounds; QPoint m_lastPos; bool knopPressed; Qt::Alignment m_alignment; };joypad.cpp
#include "joypad.h" #include "ui_joypad.h" #include <QPainter> #include <QParallelAnimationGroup> #include <QPropertyAnimation> #include <QMouseEvent> #include <math.h> #include <QDebug> #include <QPushButton> #include <QApplication> template<typename T> T constrain(T Value, T Min, T Max) { return (Value < Min)? Min : (Value > Max)? Max : Value; } JoyPad::JoyPad(QWidget *parent, Qt::WindowFlags f) : QWidget(parent, f), m_x(0), m_y(0), m_returnAnimation(new QParallelAnimationGroup(this)), m_xAnimation(new QPropertyAnimation(this, "x")), m_yAnimation(new QPropertyAnimation(this, "y")), m_alignment(Qt::AlignCenter) { m_xAnimation->setEndValue(0.f); m_xAnimation->setDuration(400); m_xAnimation->setEasingCurve(QEasingCurve::OutSine); m_yAnimation->setEndValue(0.f); m_yAnimation->setDuration(400); m_yAnimation->setEasingCurve(QEasingCurve::OutSine); m_returnAnimation->addAnimation(m_xAnimation); m_returnAnimation->addAnimation(m_yAnimation); } /** * @brief JoyPad::x * @return */ float JoyPad::x() const { return m_x; } /** * @brief JoyPad::y * @return */ float JoyPad::y() const { return m_y; } /** * @brief JoyPad::setX * @param value of x axis from -1 to 1 */ void JoyPad::setX(float value) { m_x = constrain(value, -1.f, 1.f); qreal radius = ( m_bounds.width() - m_knopBounds.width() ) / 2; m_knopBounds.moveCenter(QPointF(m_bounds.center().x() + m_x * radius, m_knopBounds.center().y())); update(); emit xChanged(m_x); } /** * @brief JoyPad::setY * @param value of y axis from -1 to 1 */ void JoyPad::setY(float value) { m_y = constrain(value, -1.f, 1.f); qreal radius = ( m_bounds.width() - m_knopBounds.width() ) / 2; m_knopBounds.moveCenter(QPointF(m_knopBounds.center().x(), m_bounds.center().y() + m_y * radius)); update(); emit yChanged(m_y); } void JoyPad::removeXAnimation() { // return if the animation is already removed if (m_xAnimation->parent() != m_returnAnimation) return; m_returnAnimation->removeAnimation(m_xAnimation); // take ownership of the animation (parent is 0 after removeAnimation()) m_xAnimation->setParent(this); } void JoyPad::addXAnimation() { // abort if the animation is already added if (m_xAnimation->parent() == m_returnAnimation) return; m_returnAnimation->addAnimation(m_xAnimation); } void JoyPad::removeYAnimation() { if (m_yAnimation->parent() != m_returnAnimation) return; m_returnAnimation->removeAnimation(m_yAnimation); m_yAnimation->setParent(this); } void JoyPad::addYAnimation() { if (m_yAnimation->parent() == m_returnAnimation) return; m_returnAnimation->addAnimation(m_yAnimation); } void JoyPad::setAlignment(Qt::Alignment f) { m_alignment = f; } /** * @brief JoyPad::resizeEvent * @param event * * calculates a square bounding rect for the background and the knob */ void JoyPad::resizeEvent(QResizeEvent *event) { Q_UNUSED(event) //change here for radius float size = qMin(rect().width()/2, rect().height()/2); QPointF topleft; if (m_alignment.testFlag(Qt::AlignTop)) { topleft.setY(0); } else if (m_alignment.testFlag(Qt::AlignVCenter)) { topleft.setY( ((height()-size)/2) ); } else if(m_alignment.testFlag(Qt::AlignBottom)) { topleft.setY( height()-size); } if (m_alignment.testFlag(Qt::AlignLeft)) { topleft.setX(0); } else if(m_alignment.testFlag(Qt::AlignHCenter)) { topleft.setX( (width()-size)/2 ); } else if(m_alignment.testFlag(Qt::AlignRight)) { topleft.setX( width()-size); } m_bounds = QRectF(topleft, QSize(size, size)); qDebug() << m_bounds; m_knopBounds.setWidth(size * 0.3); m_knopBounds.setHeight(size*0.3); // adjust knob position qreal radius = ( m_bounds.width() - m_knopBounds.width() ) / 2; m_knopBounds.moveCenter(QPointF(m_bounds.center().x() + m_x * radius, m_bounds.center().y() - m_y * radius)); } /** * @brief JoyPad::paintEvent * @param event */ void JoyPad::paintEvent(QPaintEvent *event) { Q_UNUSED(event) QPainter painter(this); painter.setRenderHint(QPainter::Antialiasing); painter.setRenderHint(QPainter::HighQualityAntialiasing); // draw background QRadialGradient gradient(m_bounds.center(), m_bounds.width()/2, m_bounds.center()); gradient.setFocalRadius(m_bounds.width()*0.3); gradient.setCenterRadius(m_bounds.width()*0.7); gradient.setColorAt(0, Qt::lightGray); gradient.setColorAt(1, Qt::lightGray); painter.setPen(QPen(QBrush(Qt::gray), m_bounds.width()* 0.005)); painter.setBrush(QBrush(gradient)); painter.drawEllipse(m_bounds); // draw crosshair // painter.setPen(QPen(QBrush(Qt::gray), m_bounds.width()* 0.005)); // painter.drawLine(QPointF(m_bounds.left(), m_bounds.center().y()), QPointF(m_bounds.center().x() - m_bounds.width()*0.35, m_bounds.center().y())); // painter.drawLine(QPointF(m_bounds.center().x() + m_bounds.width()*0.35, m_bounds.center().y()), QPointF(m_bounds.right(), m_bounds.center().y())); // painter.drawLine(QPointF(m_bounds.center().x(), m_bounds.top()), QPointF(m_bounds.center().x(), m_bounds.center().y() - m_bounds.width()*0.35)); // painter.drawLine(QPointF(m_bounds.center().x(), m_bounds.center().y() + m_bounds.width()*0.35), QPointF(m_bounds.center().x(), m_bounds.bottom())); // draw knob if (!this->isEnabled()) return; gradient = QRadialGradient(m_knopBounds.center(), m_knopBounds.width()/2, m_knopBounds.center()); gradient.setColorAt(0, Qt::gray); gradient.setColorAt(1, Qt::darkGray); gradient.setFocalRadius(m_knopBounds.width()*0.2); gradient.setCenterRadius(m_knopBounds.width()*0.5); painter.setPen(QPen(QBrush(Qt::darkGray), m_bounds.width()*0.005)); painter.setBrush(QBrush(gradient)); painter.drawEllipse(m_knopBounds); } /** * @brief JoyPad::mousePressEvent * @param event */ void JoyPad::mousePressEvent(QMouseEvent *event) { if (m_knopBounds.contains(event->pos())) { m_returnAnimation->stop(); m_lastPos = event->pos(); knopPressed = true; } } /** * @brief JoyPad::mouseReleaseEvent * @param event */ void JoyPad::mouseReleaseEvent(QMouseEvent *event) { Q_UNUSED(event) knopPressed = false; m_returnAnimation->start(); } /** * @brief JoyPad::mouseMoveEvent * @param event */ void JoyPad::mouseMoveEvent(QMouseEvent *event) { if (!knopPressed) return; // moved distance QPointF dPos = event->pos() - m_lastPos; // change the distance sligthly to guarantee overlaping knop and pointer dPos += 0.05 * (event->pos() - m_knopBounds.center()); QPointF fromCenterToKnop = m_knopBounds.center() + dPos - m_bounds.center(); qreal radius = ( m_bounds.width() - m_knopBounds.width() ) / 2; fromCenterToKnop.setX(constrain(fromCenterToKnop.x(), -radius, radius)); fromCenterToKnop.setY(constrain(fromCenterToKnop.y(), -radius, radius)); m_knopBounds.moveCenter(fromCenterToKnop + m_bounds.center()); m_lastPos = event->pos(); update(); if (radius == 0) return; float x = ( m_knopBounds.center().x() - m_bounds.center().x() ) / radius; float y = ( m_knopBounds.center().y() - m_bounds.center().y() ) / radius; if (m_x !=x) { m_x = x; emit xChanged(m_x); } if (m_y !=y) { m_y = y; emit yChanged(m_y); } }main.cpp
#include "joypad.h" #include "mainwindow.h" #include <QApplication> #include "geometry_msgs/Twist.h" int main(int argc, char *argv[]) { ros::init(argc, argv, "joypad"); QApplication a(argc, argv); ros::NodeHandle nh; // Init cmd_vel publisher ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1); // Create Twist message geometry_msgs::Twist twist; mainwindow w; // set the window title as the node name w.setWindowTitle(QString::fromStdString( ros::this_node::getName())); w.show(); return a.exec(); }mainwindow.h
#pragma once #include <QWidget> namespace Ui { class mainwindow; } class mainwindow : public QWidget { Q_OBJECT public: explicit mainwindow(QWidget *parent = nullptr); ~mainwindow(); float X; float Y; float speed; float turn; private: Ui::mainwindow *ui; };mainwindow.cpp
#include "mainwindow.h" #include "ui_mainwindow.h" #include <ros/ros.h> #include <QDebug> #include <geometry_msgs/Twist.h> #include <cmath> mainwindow::mainwindow(QWidget *parent) : QWidget(parent), ui(new Ui::mainwindow) { ui->setupUi(this); X=0.0; Y=0.0; float speed = sqrt(X*X+Y*Y); float turn = atan(X/Y); qDebug() << "speed " << speed; qDebug() << "turn: " << turn; connect(ui->widget, &JoyPad::xChanged, this, [this](float x){ X=x; Y=ui->widget->y(); //qDebug() << "x: " << X << " y: " << Y; }); qDebug() << "x: " << X << " y: " << Y; connect(ui->widget, &JoyPad::yChanged, this, [this](float y){ X=ui->widget->x(); Y=y; //qDebug() << "x: " << X << " y: " << Y; }); qDebug() << "x: " << X << " y: " << Y; } mainwindow::~mainwindow() { delete ui; }
dynamically changing X and Y when qDebug is inside connect:
not changing (static) X and Y and also when qDebug written is outside connect:
So how will I know that my X and Y are changing as I move the joypad knob when I want to use them outside the connect()
-
Hi and welcome to devnet,
Can you explain what your code does ?
From a quick look you did not yet connect your UI with the ROS part.
On a side note, please put your code between coding tags (the </> button or put three backticks before and after your code) to make it readable.
-
Hi and welcome to devnet,
Can you explain what your code does ?
From a quick look you did not yet connect your UI with the ROS part.
On a side note, please put your code between coding tags (the </> button or put three backticks before and after your code) to make it readable.
-
From the looks of it, you just initialized ROS but you are not sending any message to it.
-
@SGaist That's where I am stuck.....for sending message i need to get the knob coordinates when i move it to get speed and direction and then publish it to cmd_vel topic how should I do that. I have updated the code now I am able to get the coordinates of my knob in the terminal as I move it but when I remove the qDebug statements out of the connect I don't get dynamically changing X and Y as I was getting when I had qDebug in connect so how will I know if my X and Y variables are still changing when I move the knob when I use them outside of my connect statement.
-
You have the values in the lambda so you should do the message sending there.
Note that if you search for "ROS Qt" you'll find several tutorial on how to integrate Qt in a ROS application.
