QModbus库使用，并作为ROS节点发布话题及程序CMakelist编写

2019-01-23 20:27发布

站内文章 / 前端开发

609 0

再贱就再见

女 | 书童

私信

在QT框架下，使用QModbus库实现了客户端程序对埃夫特机器人ER50C10A实时读取当前关节角度、笛卡尔坐标值等参数。

同时作为ROS节点发布到话题/joint_states中。

程序逻辑：

启动Modbus -- 连接--发送读取请求 --等待qt返回读取完毕信号触发槽函数读取 --读取并显示

其中，连接与发送请求等各部分需要写到不同的槽函数中，否则会报错modbus未连接。

程序代码：（截取部分）

主程序：初始化ROS节点、TOPIC、qt对象

int main(int argc, char *argv[])
{
	ros::init ( argc, argv, "test_modbus" );
	ros::NodeHandle n;
	ros::Publisher Cmd = n.advertise<sensor_msgs::JointState> ("/joint_states", 1024 );
	QApplication a(argc, argv);
	ModbusTcpClient w;
	w.publisher = &Cmd;
	w.show();
	return a.exec();
}

QModbus相关程序：

ModbusTcpClient::ModbusTcpClient(QWidget *parent) :
	QMainWindow(parent),
	ui(new Ui::ModbusTcpClient), m_holdingRegisters(20)
{
	ui->setupUi(this);
	modbusDevice = new QModbusTcpClient(this);
	ui->lineEdit->setText(QLatin1Literal("192.168.0.103:502"));
	connect(modbusDevice, &QModbusClient::errorOccurred, [this](QModbusDevice::Error) {statusBar()->showMessage(modbusDevice->errorString(), 5000);});
	connect(modbusDevice, &QModbusClient::stateChanged,this, &ModbusTcpClient::onStateChanged);
	connect(this, SIGNAL(readonce()),this,SLOT(on_readButton_clicked()));
}
ModbusTcpClient::~ModbusTcpClient()
{
	if (modbusDevice)
		modbusDevice->disconnectDevice();
	delete modbusDevice;
	delete ui;
}
void ModbusTcpClient::on_connectButton_clicked()
{
	emit Scan();
	if (!modbusDevice)
		return;
	statusBar()->clearMessage();

	if (modbusDevice->state() != QModbusDevice::ConnectedState) {
		const QUrl url = QUrl::fromUserInput(ui->lineEdit->text());
		modbusDevice->setConnectionParameter(QModbusDevice::NetworkPortParameter, url.port());
		modbusDevice->setConnectionParameter(QModbusDevice::NetworkAddressParameter, url.host());

		modbusDevice->setTimeout(1000);
		modbusDevice->setNumberOfRetries(3);
		if (!modbusDevice->connectDevice()) {
			statusBar()->showMessage(tr("Connect failed: ") + modbusDevice->errorString(), 5000);
		}
	}
	else {
		modbusDevice->disconnectDevice();
	}
}
void ModbusTcpClient::onStateChanged(int state)
{

	if (state == QModbusDevice::UnconnectedState)
		ui->connectButton->setText(tr("Connect"));
	else if (state == QModbusDevice::ConnectedState)
		ui->connectButton->setText(tr("Disconnect"));
}
void ModbusTcpClient::on_readButton_clicked()
{
	
		if (!modbusDevice)
		return;
		statusBar()->clearMessage();
		QModbusDataUnit readUnit(QModbusDataUnit::HoldingRegisters, 12, 12);
		if (auto *reply = modbusDevice->sendReadRequest(readUnit, 1)) {
			if (!reply->isFinished())
				connect(reply, &QModbusReply::finished, this, &ModbusTcpClient::readReady);
			else
				delete reply; // broadcast replies return immediately
		}
		else {
			statusBar()->showMessage(tr("Read error: ") + modbusDevice->errorString(), 5000);
		}
		
	
	}
void ModbusTcpClient::readReady()
{
	std::vector<double>	jointPos;
	jointPos.clear();
	auto reply = qobject_cast<QModbusReply *>(sender());
	if (!reply)
		return;
	if (reply->error() == QModbusDevice::NoError) {
		const QModbusDataUnit unit = reply->result();
		for (uint i = 0; i < unit.valueCount(); i++) {
			const QString entry = tr("Address: %1, Value: %2").arg(unit.startAddress())
				.arg(QString::number(unit.value(i),
					unit.registerType() <= QModbusDataUnit::Coils ? 10 : 16));
			);
			
		}
		ui->textBrowser->insertPlainText("机器人当前位姿：");
		for (int q = 0; q < 6; q++)
		{
			poses.clear();
			
			QString number1 = QString::number(unit.value(2 * q + 1), unit.registerType() <= QModbusDataUnit::Coils ? 10 : 16);
			
			if (number1.length() < 4)
			{
				poses.append(QString::number(0));
			}
			poses.append(QString::number(unit.value(2 * q + 1), unit.registerType() <= QModbusDataUnit::Coils ? 10 : 16));
			
			QString number2 = QString::number(unit.value(2 * q), unit.registerType() <= QModbusDataUnit::Coils ? 10 : 16);
			if (number2.length() < 4)
			{
				poses.append(QString::number(0));
			}
			poses.append(QString::number(unit.value(2 * q), unit.registerType() <= QModbusDataUnit::Coils ? 10 : 16));
			pose[q] = BytesToFloat(poses);
			qDebug() << poses;
			jointPos.push_back(pose[q]/180*3.1415926);
			ui->textBrowser->insertPlainText(QString::number(pose[q]));
			ui->textBrowser->insertPlainText(" ");
		}
		ui->textBrowser->insertPlainText("\n");
		ui->textBrowser->moveCursor(QTextCursor::End);
		jointState.header.stamp = ros::Time::now();
        jointState.name = j_names;
        jointState.position = jointPos;
		publisher->publish(jointState);
	}

	else if (reply->error() == QModbusDevice::ProtocolError) {
		statusBar()->showMessage(tr("Read response error: %1 (Mobus exception: 0x%2)").
			arg(reply->errorString()).
			arg(reply->rawResult().exceptionCode(), -1, 16), 5000);
	}
	else {
		statusBar()->showMessage(tr("Read response error: %1 (code: 0x%2)").
			arg(reply->errorString()).
			arg(reply->error(), -1, 16), 5000);
	}

	reply->deleteLater();
	emit readonce();
}
void ModbusTcpClient::Display()
{
	ui->textBrowser->insertPlainText(QDateTime::currentDateTime().toString("yyyy-MM-dd hh:mm:ss "));
	ui->textBrowser->insertPlainText("机器人当前位姿：");
	ui->textBrowser->moveCursor(QTextCursor::End);
}

CMakeList编写：

cmake_minimum_required(VERSION 2.8.3)
project(test_modbus)
add_compile_options(-std=c++11)

set(CMAKE_INCLUDE_CURRENT_DIR ON)
set(CMAKE_AUTOMOC ON)
set(CMAKE_AUTOUIC ON)
set(CMAKE_AUTORCC ON)
set(Qt5SerialBus_DIR "/home/xxx/Qt5.8.0/5.8/gcc_64/lib/cmake/Qt5SerialBus")


find_package(catkin REQUIRED roscpp std_msgs sensor_msgs)
find_package(Qt5Widgets REQUIRED)
find_package(Qt5SerialBus REQUIRED)


catkin_package()


include_directories(${catkin_INCLUDE_DIRS})
include_directories(${Qt5SerialBus_INCLUDE_DIRS})
add_definitions(${Qt5SerialBus_DEFINITIONS})
qt5_wrap_ui( UIC src/test_modbus.ui)

add_executable(testmodbus src/main.cpp src/test_modbus.cpp src/test_modbus.h src/test_modbus.ui) 
target_link_libraries(testmodbus ${catkin_LIBRARIES} Qt5::Widgets Qt5::SerialBus)