ROS Qt5 librviz人机交互界面开发二（实现机器人速度仪表盘）

一，实现效果：

原理主要就是订阅机器人的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话题：

创建订阅者：

ros::Subscriber cmdVel_sub;

回调函数：

 void speedCallback(const nav_msgs::Odometry::ConstPtr& msg);

信号：

Q_SIGNALS:
    void speed_x(double x);
    void speed_y(double y)

qnode.h

/**
* @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
发送信号与订阅话题：

cmdVel_sub =n.subscribe<nav_msgs::Odometry>("raw_odom",1000,&QNode::speedCallback,this);

话题的回调函数 在回调函数中发送信号：

//速度回调函数
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);
}

之后在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

本人原博客地址：

csdn