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));
}
- 完整的源码，可以查看github
获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号