Home » ROS与Qt5人机交互 » ROS与Qt5人机交互界面开发-实现机器人速度仪表盘

ROS与Qt5人机交互界面开发-实现机器人速度仪表盘

ROS与Qt5人机交互界面开发-实现机器人速度仪表盘

说明:

  • 介绍如何实现实现机器人速度仪表盘

步骤:

  • 实现效果:

请输入图片描述

  • 原理主要就是订阅机器人的odom话题,然后利用信号与槽更新ui仪表盘控件的信息
  • 第一步:制作仪表盘控件
  • 由于qt标准控件没有仪表盘,因此我们需要自己手动绘制
  • 可以直接使用这个绘制文件
  • CCtrlDashBoard.h内容如下:
#ifndef CCTRLDASHBOARD_H
#define CCTRLDASHBOARD_H

#include <QWidget>

class CCtrlDashBoard : public QWidget
{
    Q_OBJECT
public:
    enum StyleType {
        ST_DEFAULT=0,
        ST_ARCBAR
    };
    explicit CCtrlDashBoard(QWidget *parent = nullptr, StyleType type=ST_DEFAULT);

    void setValue(qreal value){
        m_DashValue = value;
        update();
    }
    void setBackGroundColor(QColor color){
        m_BgColor=color;
        update();
    }
    void setFrontColor(QColor color){
        m_FrontColor=color;
        update();
    }
    void setBorderColor(QColor color){
        m_BorderColor=color;
        update();
    }
    void setUnitString(QString str){
        m_StrUnit=str;
        update();
    }

    void drawBackGround(QPainter *painter, qreal hlafWidth);
    void drawScaleDials(QPainter *painter, qreal hlafWidth);
    void drawIndicator(QPainter *painter, qreal hlafWidth);
    void drawIndicatorBar(QPainter *painter, qreal hlafWidth);
signals:

public slots:
protected:
    virtual void paintEvent(QPaintEvent * event);

private:
    int m_StartAngle;
    int m_EndAngle;
    int m_StyleType;

    qreal m_LineLength;
    qreal m_DashValue;
    qreal m_MaxValue;
    qreal m_MinValue;
    qreal m_DashNum;

    QColor m_BgColor;
    QColor m_FrontColor;
    QColor m_BorderColor;
    QString m_StrUnit;

    qreal m_MaxBorderRadius;
    qreal m_MinBorderRadius;
    qreal m_DialsRadius;
};

#endif // CCTRLDASHBOARD_H
  • CCtrlDashBoard.cpp文件内容如下:
#include "../include/cyrobot_monitor/CCtrlDashBoard.h"
#include <QPainter>
#include <QDebug>
#include <qmath.h>

CCtrlDashBoard::CCtrlDashBoard(QWidget *parent, StyleType type) :
    QWidget(parent),
    m_StyleType(type)
{
    m_BorderColor = QColor(60,60,60);
    m_BgColor = QColor(160,160,160);
    m_FrontColor = Qt::white;

    m_DashValue= 0;
    m_MinValue = 0;
    m_MaxValue = 100;
    m_DashNum = 1;
    m_LineLength=0;

    m_StartAngle=90;  //510
    m_EndAngle=0;   //120
    update();
}

void CCtrlDashBoard::drawBackGround(QPainter *painter, qreal hlafWidth)
{
    m_MaxBorderRadius = ((hlafWidth*5)/6);
    qreal startX=hlafWidth-m_MaxBorderRadius;
    painter->save();
    painter->setPen(m_BorderColor);
    painter->setBrush(m_BorderColor);

    QPainterPath bigCircle;
    bigCircle.addEllipse(startX, startX, (m_MaxBorderRadius*2), (m_MaxBorderRadius*2));

    m_MinBorderRadius = m_MaxBorderRadius-20;
    startX=hlafWidth-m_MinBorderRadius;
    QPainterPath smallCircle;
    smallCircle.addEllipse(startX, startX, (m_MinBorderRadius*2), (m_MinBorderRadius*2));

    QPainterPath path = bigCircle - smallCircle;
    painter->drawPath(path);
    painter->restore();

    painter->save();
    painter->setBrush(m_BgColor);
    painter->drawEllipse(startX,startX,(m_MinBorderRadius*2), (m_MinBorderRadius*2));
    painter->drawText(startX+90,startX+170,"CM/S");
    painter->restore();
    m_DialsRadius = m_MinBorderRadius-10;
    if(m_DialsRadius < 0){
        m_DialsRadius=2;
    }
}
void CCtrlDashBoard::drawScaleDials(QPainter *painter, qreal hlafWidth)
{
    qreal tSteps = (m_MaxValue - m_MinValue)/m_DashNum; //相乘后的值是分的份数
    qreal angleStep = 1.0*(360.0-m_StartAngle - m_EndAngle) / tSteps; //每一个份数的角度

    painter->save();
    QPen pen ;
    pen.setColor(m_FrontColor); //推荐使用第二种方式
    painter->translate(hlafWidth,hlafWidth);
    painter->rotate(m_StartAngle);
    m_LineLength = (hlafWidth/16);
    qreal lineStart = m_DialsRadius-4*m_LineLength-m_LineLength;
    qreal lineSmStart = m_DialsRadius-4*m_LineLength-m_LineLength/2;
    qreal lineEnd = m_DialsRadius-4*m_LineLength;
    for (int i = 0; i <= tSteps; i++)
    {
        if (i % 10 == 0)//整数刻度显示加粗
        {
            pen.setWidth(2); //设置线宽
            painter->setPen(pen); //使用面向对象的思想,把画笔关联上画家。通过画家画出来
            painter->drawLine(lineStart,lineStart, lineEnd, lineEnd); //两个参数应该是两个坐标值
            //painter->drawText(lineEnd+6,lineEnd+6, tr("%1").arg(m_MinValue+i));
        }
        else
        {
            pen.setWidth(1);
            painter->setPen(pen);
            painter->drawLine(lineSmStart, lineSmStart, lineEnd, lineEnd); //两个参数应该是两个坐标值
        }
        painter->rotate(angleStep);
    }
    painter->restore();

    painter->save();
    painter->setPen(pen);
    //m_startAngle是起始角度,m_endAngle是结束角度,m_scaleMajor在一个量程中分成的刻度数
    qreal startRad = ( 315-m_StartAngle) * (3.14 / 180);
    qreal deltaRad = (360-m_StartAngle - m_EndAngle) * (3.14 / 180) / tSteps;
    qreal sina,cosa;
    qreal x, y;
    QFontMetricsF fm(this->font());
    double w, h, tmpVal;
    QString str;
    painter->translate(hlafWidth,hlafWidth);
    lineEnd = m_MinBorderRadius-8;
    for (int i = 0; i <= tSteps; i++)
    {
        if (i % 10 == 0)//整数刻度显示加粗
        {
            sina = qSin(startRad - i * deltaRad);
            cosa = cos(startRad - i * deltaRad);

            tmpVal = 1.0 * i *((m_MaxValue - m_MinValue) / tSteps) + m_MinValue;
            str = QString( "%1" ).arg(tmpVal);  //%1作为占位符   arg()函数比起 sprintf()来是类型安全的
            w = fm.size(Qt::TextSingleLine,str).width();
            h = fm.size(Qt::TextSingleLine,str).height();
            x = lineEnd * cosa - w / 2;
            y = -lineEnd * sina + h / 4;
            painter->drawText(x, y, str); //函数的前两个参数是显示的坐标位置,后一个是显示的内容,是字符类型""
        }
    }
    painter->restore();
}

void CCtrlDashBoard::drawIndicator(QPainter *painter, qreal hlafWidth)
{
    QPolygon pts;
    pts.setPoints(3, -8,0, 8,0, 0,(int)m_DialsRadius-20); /* (-2,0)/(2,0)/(0,60) *///第一个参数是 ,坐标的个数。后边的是坐标
    painter->save();
    painter->translate(hlafWidth, hlafWidth);
    painter->rotate(m_StartAngle-45);

    if(m_DashValue>0){
        qreal tSteps = (m_MaxValue - m_MinValue)/m_DashNum; //相乘后的值是分的份数
        qreal angleStep = 1.0*(360.0-m_StartAngle - m_EndAngle) / tSteps; //每一个份数的角度
        double degRotate = angleStep*tSteps*(m_DashValue/100.0)+angleStep;

        //画指针
        //qDebug() <<"degRotate =="<<degRotate<<"m_DashValue =="<<m_DashValue<<m_StartAngle<<m_DialsRadius<<hlafWidth;
        if(m_DashValue == 99){
            painter->rotate(m_EndAngle-m_StartAngle);  //顺时针旋转坐标系统
        }else
        {
            painter->rotate(degRotate);  //顺时针旋转坐标系统
        }

    }
    QRadialGradient haloGradient(0, 0, hlafWidth/2, 0, 0);  //辐射渐变
    haloGradient.setColorAt(0, QColor(255,69,0));
    haloGradient.setColorAt(1, QColor(255,0,0));
    painter->setPen(Qt::white); //定义线条文本颜色  设置线条的颜色
    painter->setBrush(haloGradient);//刷子定义形状如何填满 填充后的颜色
    painter->drawConvexPolygon(pts); //这是个重载函数,绘制多边形。
    painter->restore();

    //画中心点
    QColor niceBlue(150, 150, 200);
    QConicalGradient coneGradient(0, 0, -90.0);  //角度渐变
    coneGradient.setColorAt(0.0, Qt::darkGray);
    coneGradient.setColorAt(0.2, niceBlue);
    coneGradient.setColorAt(0.5, Qt::white);
    coneGradient.setColorAt(1.0, Qt::darkGray);
    painter->save();
    painter->translate(hlafWidth,hlafWidth);
    painter->setPen(Qt::NoPen);  //没有线,填满没有边界
    painter->setBrush(coneGradient);
    painter->drawEllipse(-10, -10, 20, 20);
    painter->restore();
}

void CCtrlDashBoard::drawIndicatorBar(QPainter *painter, qreal hlafWidth)
{
    // 渐变色
    painter->save();
    painter->translate(hlafWidth,hlafWidth);
    qreal lineEnd = m_DialsRadius-3*m_LineLength;
    QRadialGradient gradient(0, 0, lineEnd);
    gradient.setColorAt(0, Qt::white);
    gradient.setColorAt(1.0, Qt::blue);
    painter->setBrush(gradient);

    // << 1(左移1位)相当于radius*2 即:150*2=300
    //QRectF(-150, -150, 300, 300)
    QRectF rect(-lineEnd, -lineEnd, lineEnd*2, lineEnd*2);
    QPainterPath path;
    path.arcTo(rect, m_StartAngle, 270);

    // QRectF(-120, -120, 240, 240)
    QPainterPath subPath;
    subPath.addEllipse(rect.adjusted(10, 10, -10, -10));

    // path为扇形 subPath为椭圆
    path -= subPath;
    painter->setPen(Qt::NoPen);
    painter->rotate(m_StartAngle+45);
    painter->drawPath(path);
    painter->restore();


    qreal degRotate=0.1;
    if(m_DashValue>0){
        qreal tSteps = (m_MaxValue - m_MinValue)/m_DashNum; //相乘后的值是分的份数
        qreal angleStep = 1.0*(360.0-m_StartAngle - m_EndAngle) / tSteps; //每一个份数的角度
        degRotate = angleStep*tSteps*(m_DashValue/100.0)+angleStep;

        //画指针
        //qDebug() <<"degRotate =="<<degRotate<<"m_DashValue =="<<m_DashValue<<m_StartAngle<<m_DialsRadius<<hlafWidth;
        /*if(m_DashValue == 99){
            painter->rotate(m_EndAngle-m_StartAngle);  //顺时针旋转坐标系统
        }else
        {
            painter->rotate(degRotate);  //顺时针旋转坐标系统
        }*/
    }

    painter->save();
    painter->translate(hlafWidth, hlafWidth);

    QRadialGradient ftGradient(0, 0, lineEnd);
    ftGradient.setColorAt(0, Qt::white);
    ftGradient.setColorAt(1.0, Qt::darkYellow);
    painter->setBrush(ftGradient);

    // << 1(左移1位)相当于radius*2 即:150*2=300
    //QRectF(-150, -150, 300, 300)
    QRectF ftRect(-lineEnd, -lineEnd, lineEnd*2, lineEnd*2);
    QPainterPath ftPath;
    ftPath.arcTo(ftRect, m_EndAngle-m_StartAngle, -degRotate);
    // path为扇形 subPath为椭圆
    ftPath -= subPath;
    painter->rotate(m_StartAngle-45);
    painter->drawPath(ftPath);
    painter->restore();

    QPolygon pts;
    int pointLength=lineEnd-12;
    pts.setPoints(3, -6,pointLength-10, 6,pointLength-10, 0,pointLength);
    painter->save();
    painter->translate(hlafWidth, hlafWidth);
    painter->rotate(m_StartAngle-45);

    if(m_DashValue == 99){
        painter->rotate(m_EndAngle-m_StartAngle);  //顺时针旋转坐标系统
    }else
    {
        painter->rotate(degRotate);  //顺时针旋转坐标系统
    }
    painter->setPen(Qt::white); //定义线条文本颜色  设置线条的颜色
    painter->setBrush(Qt::blue);//刷子定义形状如何填满 填充后的颜色
    painter->drawConvexPolygon(pts); //这是个重载函数,绘制多边形。
    painter->restore();
}

void CCtrlDashBoard::paintEvent(QPaintEvent * event)
{
    QPainter p(this);
    qreal width = qMin((this->width()>>1), (this->height()>>1));

    p.setRenderHints(QPainter::Antialiasing|QPainter::TextAntialiasing);

    drawBackGround(&p, width);

    drawScaleDials(&p, width);
    switch(m_StyleType)
    {
    case ST_DEFAULT:
        drawIndicator(&p, width);
        break;
    case ST_ARCBAR:
        drawIndicatorBar(&p, width);
        break;
    }
}
  • 向ui中添加仪表盘控件:
  • 需要在ui中添加一个widget(需要固定size),我这里为widget_speed_x,widget_speed_y
  • 添加代码:
m_DashBoard_x =new CCtrlDashBoard(ui.widget_speed_x);
m_DashBoard_x->setGeometry(ui.widget_speed_x->rect());
m_DashBoard_x->setValue(0);
m_DashBoard_y =new CCtrlDashBoard(ui.widget_speed_y);
m_DashBoard_y->setGeometry(ui.widget_speed_y->rect());
m_DashBoard_y->setValue(0);
  • 添加效果:

请输入图片描述

  • 关联仪表盘显示与机器人速度
  • 主要就是订阅odom话题:
  • 修改qnode.hpp文件
/**
 * @file /include/cyrobot_monitor/qnode.hpp
 *
 * @brief Communications central!
 *
 * @date February 2011
 **/
/*****************************************************************************
** Ifdefs
*****************************************************************************/

#ifndef cyrobot_monitor_QNODE_HPP_
#define cyrobot_monitor_QNODE_HPP_

/*****************************************************************************
** Includes
*****************************************************************************/

// To workaround boost/qt4 problems that won't be bugfixed. Refer to
//    https://bugreports.qt.io/browse/QTBUG-22829
#ifndef Q_MOC_RUN
#include <ros/ros.h>
#endif
#include <string>
#include <QThread>
#include <QStringListModel>
#include <nav_msgs/Odometry.h>
#include <std_msgs/Float64.h>
#include <std_msgs/Float32.h>
#include <geometry_msgs/Twist.h>
#include <map>
/*****************************************************************************
** Namespaces
*****************************************************************************/

namespace cyrobot_monitor {

/*****************************************************************************
** Class
*****************************************************************************/

class QNode : public QThread {
    Q_OBJECT
public:
    QNode(int argc, char** argv );
    virtual ~QNode();
    bool init();
    bool init(const std::string &master_url, const std::string &host_url);
    void move_base(char k,float speed_linear,float speed_trun);
    void run();

    /*********************
    ** Logging
    **********************/
    enum LogLevel {
             Debug,
             Info,
             Warn,
             Error,
             Fatal
     };

    QStringListModel* loggingModel() { return &logging_model; }
    void log( const LogLevel &level, const std::string &msg);


Q_SIGNALS:
    void loggingUpdated();
    void rosShutdown();
    void speed_x(double x);
    void speed_y(double y);
    void power(float p);
    void Master_shutdown();
private:
    int init_argc;
    char** init_argv;
    ros::Publisher chatter_publisher;
    ros::Subscriber cmdVel_sub;
    ros::Subscriber chatter_subscriber;
    ros::Subscriber power_sub;
    ros::Publisher cmd_pub;
    QStringListModel logging_model;
    void speedCallback(const nav_msgs::Odometry::ConstPtr& msg);
    void powerCallback(const std_msgs::Float32& message_holder);
    void myCallback(const std_msgs::Float64& message_holder);
};

}  // namespace cyrobot_monitor

#endif /* cyrobot_monitor_QNODE_HPP_ */
  • 修改qnode.cpp
  • 发送信号与订阅话题:
/**
 * @file /src/qnode.cpp
 *
 * @brief Ros communication central!
 *
 * @date February 2011
 **/

/*****************************************************************************
** Includes
*****************************************************************************/

#include <ros/ros.h>
#include <ros/network.h>
#include <string>
#include <std_msgs/String.h>
#include <sstream>
#include "../include/cyrobot_monitor/qnode.hpp"
#include <QDebug>
/*****************************************************************************
** Namespaces
*****************************************************************************/

namespace cyrobot_monitor {

/*****************************************************************************
** Implementation
*****************************************************************************/

QNode::QNode(int argc, char** argv ) :
    init_argc(argc),
    init_argv(argv)
    {}

QNode::~QNode() {
    if(ros::isStarted()) {
      ros::shutdown(); // explicitly needed since we use ros::start();
      ros::waitForShutdown();
    }
    wait();
}

bool QNode::init() {
    ros::init(init_argc,init_argv,"cyrobot_monitor");
    if ( ! ros::master::check() ) {
        return false;
    }
    ros::start(); // explicitly needed since our nodehandle is going out of scope.
    ros::NodeHandle n;
    // Add your ros communications here.

     cmdVel_sub =n.subscribe<nav_msgs::Odometry>("raw_odom",1000,&QNode::speedCallback,this);
     power_sub=n.subscribe("power",1000,&QNode::powerCallback,this);
     //速度控制话题
     cmd_pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 1);
    start();
    return true;
}
void QNode::powerCallback(const std_msgs::Float32 &message_holder)
{

    emit power(message_holder.data);
}
void QNode::myCallback(const std_msgs::Float64 &message_holder)
{
    qDebug()<<message_holder.data<<endl;
}
//初始化的函数*********************************
bool QNode::init(const std::string &master_url, const std::string &host_url) {
    std::map<std::string,std::string> remappings;
    remappings["__master"] = master_url;
    remappings["__hostname"] = host_url;
    ros::init(remappings,"cyrobot_monitor");
    if ( ! ros::master::check() ) {
        return false;
    }
    ros::start(); // explicitly needed since our nodehandle is going out of scope.
    ros::NodeHandle n;


    //创建速度话题的订阅者
    cmdVel_sub =n.subscribe<nav_msgs::Odometry>("raw_odom",200,&QNode::speedCallback,this);
    power_sub=n.subscribe("power",1000,&QNode::powerCallback,this);
    //速度控制话题
    cmd_pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 1);
    start();
    return true;
}

//速度回调函数
void QNode::speedCallback(const nav_msgs::Odometry::ConstPtr& msg)
{
    emit speed_x(msg->twist.twist.linear.x);
    emit speed_y(msg->twist.twist.linear.y);
}
void QNode::run() {
        int count=0;
        ros::Rate loop_rate(1);
        //当当前节点没有关闭时
        while ( ros::ok() ) {
            //调用消息处理回调函数
            ros::spinOnce();

            loop_rate.sleep();
        }
        //如果当前节点关闭
        Q_EMIT rosShutdown(); // used to signal the gui for a shutdown (useful to roslaunch)


}
//发布机器人速度控制
 void QNode::move_base(char k,float speed_linear,float speed_trun)
 {
     std::map<char, std::vector<float>> moveBindings
     {
       {'i', {1, 0, 0, 0}},
       {'o', {1, 0, 0, -1}},
       {'j', {0, 0, 0, 1}},
       {'l', {0, 0, 0, -1}},
       {'u', {1, 0, 0, 1}},
       {',', {-1, 0, 0, 0}},
       {'.', {-1, 0, 0, 1}},
       {'m', {-1, 0, 0, -1}},
       {'O', {1, -1, 0, 0}},
       {'I', {1, 0, 0, 0}},
       {'J', {0, 1, 0, 0}},
       {'L', {0, -1, 0, 0}},
       {'U', {1, 1, 0, 0}},
       {'<', {-1, 0, 0, 0}},
       {'>', {-1, -1, 0, 0}},
       {'M', {-1, 1, 0, 0}},
       {'t', {0, 0, 1, 0}},
       {'b', {0, 0, -1, 0}},
       {'k', {0, 0, 0, 0}},
       {'K', {0, 0, 0, 0}}
     };
     char key=k;
     //计算是往哪个方向
     float x = moveBindings[key][0];
     float y = moveBindings[key][3];
     float z = moveBindings[key][4];
     float th = moveBindings[key][3];
     //计算线速度和角速度
     float speed = speed_linear;
     float turn = speed_trun;
     // Update the Twist message
     geometry_msgs::Twist twist;
    twist.linear.x = x * speed;
    twist.linear.y = y * speed;
    twist.linear.z = z * speed;

    twist.angular.x = 0;
    twist.angular.y = 0;
    twist.angular.z = th * turn;

    // Publish it and resolve any remaining callbacks
    cmd_pub.publish(twist);
    ros::spinOnce();

 }
void QNode::log( const LogLevel &level, const std::string &msg) {
    logging_model.insertRows(logging_model.rowCount(),1);
    std::stringstream logging_model_msg;
    switch ( level ) {
        case(Debug) : {
                ROS_DEBUG_STREAM(msg);
                logging_model_msg << "[DEBUG] [" << ros::Time::now() << "]: " << msg;
                break;
        }
        case(Info) : {
                ROS_INFO_STREAM(msg);
                logging_model_msg << "[INFO] [" << ros::Time::now() << "]: " << msg;
                break;
        }
        case(Warn) : {
                ROS_WARN_STREAM(msg);
                logging_model_msg << "[INFO] [" << ros::Time::now() << "]: " << msg;
                break;
        }
        case(Error) : {
                ROS_ERROR_STREAM(msg);
                logging_model_msg << "[ERROR] [" << ros::Time::now() << "]: " << msg;
                break;
        }
        case(Fatal) : {
                ROS_FATAL_STREAM(msg);
                logging_model_msg << "[FATAL] [" << ros::Time::now() << "]: " << msg;
                break;
        }
    }
    QVariant new_row(QString(logging_model_msg.str().c_str()));
    logging_model.setData(logging_model.index(logging_model.rowCount()-1),new_row);
    Q_EMIT loggingUpdated(); // used to readjust the scrollbar
}

}  // namespace cyrobot_monitor
  • 之后在mainwindow链接信号:
connect(&qnode,SIGNAL(speed_x(double)),this,SLOT(slot_speed_x(double)));
connect(&qnode,SIGNAL(speed_y(double)),this,SLOT(slot_speed_y(double)));
  • 槽函数进行设定value:
void MainWindow::slot_speed_x(double x)
{
    if(x>=0) ui.label_dir_x->setText("正向");
    else ui.label_dir_x->setText("反向");

    m_DashBoard_x->setValue(abs(x*100));
}
void MainWindow::slot_speed_y(double x)
{
    if(x>=0) ui.label_dir_y->setText("正向");
    else ui.label_dir_y->setText("反向");
    m_DashBoard_y->setValue(abs(x*100));
}

纠错,疑问,交流: 请进入讨论区点击加入Q群

获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号


标签: ros与qt5人机交互界面开发