通过 rqt_plot 完成ATI F/T 力和力矩传感器六轴数据可视化

基于该篇博客https://blog.csdn.net/qq_34935373/article/details/103481401，实现了通过ROS节点的方式启动ATI sensor，完成配置归零和设置软件矢量偏移，并且获取六轴数据，实时的打印在终端上。但是由于数据的刷新非常迅速，人眼很难捕捉到所有信息，所以想着能不能通过python库或者ROS插件完成数据的可视化。

偶然看到了ATI结合UR的自动装配视频，感觉这个界面很nice，官网找了半天却也没找到。

 之前的java版的可视化界面虽然也不错，但是却很难集成在ROS当中，于是我尝试了pyqtgraph这个python库。

1.PyQtGraph的安装：

$ pip install pyqtgraph

2.PyQtGraph样例：

$ ipython3
$ import pyqtgraph.examples
$ pyqtgraph.examples.run()

3.PyQtGraph测试：

会显示一个界面，提供了各种demo和代码。复制修改成自己的即可。

复制上述代码，稍作修改运行（一共三个update函数，主要就修改了第一个，看看效果），效果如下：

# -*- coding: utf-8 -*-

import pyqtgraph as pg
from pyqtgraph.Qt import QtCore, QtGui
import numpy as np


win = pg.GraphicsWindow()
win.setWindowTitle('ATI F/T sensor data')
label = pg.LabelItem(justify='right')
win.addItem(label)


# 1) Simplest approach -- update data in the array such that plot appears to scroll
#    In these examples, the array size is fixed.
win.nextRow()
p1 = win.addPlot()
p2 = win.addPlot()
data1 = np.random.normal(size=300)
p1.setRange(yRange=[-100, 100])
p2.setRange(yRange=[-100, 100])
curve1 = p1.plot(data1, pen="r")
curve2 = p2.plot(data1, pen="g")
ptr1 = 0

# 水平示意线,设置成true就可以拖动
Line1 = pg.InfiniteLine(angle=0, movable=False)
Line2 = pg.InfiniteLine(angle=0, movable=False)
p1.addItem(Line1, ignoreBounds=True)
p2.addItem(Line2, ignoreBounds=True)

hLine1 = pg.InfiniteLine(angle=0, movable=False)
hLine2 = pg.InfiniteLine(angle=0, movable=False)
p1.addItem(hLine1, ignoreBounds=True)
p2.addItem(hLine2, ignoreBounds=True)
vb1 = p1.vb
vb2 = p2.vb


def mouseMoved(evt):
    pos = evt[0]
    if p1.sceneBoundingRect().contains(pos):
        mousePoint = vb1.mapSceneToView(pos)
        label.setText("<span style='color: red'>y1=%0.1f</span>" % mousePoint.y())
        hLine1.setPos(mousePoint.y())
    if p2.sceneBoundingRect().contains(pos):
        mousePoint = vb2.mapSceneToView(pos)
        label.setText("<span style='color: green'>y1=%0.1f</span>" % mousePoint.y())
        hLine2.setPos(mousePoint.y())


def update1():
    global data1, curve1, ptr1
    data1[:-1] = data1[1:]  # shift data in the array one sample left (see also: np.roll)
    data1[-1] = np.random.normal()
    curve1.setData(data1)

    ptr1 += 1
    curve2.setData(data1)
    curve2.setPos(ptr1, 0)


# 1) Simplest approach -- update data in the array such that plot appears to scroll
#    In these examples, the array size is fixed.
win.nextRow()
p3 = win.addPlot()
p4 = win.addPlot()
data3 = np.random.normal(size=300)
curve3 = p3.plot(data3)
curve4 = p4.plot(data3)
ptr3 = 0


def update2():
    global data3, curve3, ptr3
    data3[:-1] = data3[1:]  # shift data in the array one sample left
    # (see also: np.roll)
    data3[-1] = np.random.normal()
    curve3.setData(data3)

    ptr3 += 1
    curve4.setData(data3)
    curve4.setPos(ptr3, 0)


# 1) Simplest approach -- update data in the array such that plot appears to scroll
#    In these examples, the array size is fixed.
win.nextRow()
p5 = win.addPlot()
p6 = win.addPlot()
data5 = np.random.normal(size=300)
curve5 = p5.plot(data5)
curve6 = p6.plot(data5)
ptr5 = 0


def update3():
    global data5, curve5, ptr5
    data5[:-1] = data5[1:]  # shift data in the array one sample left
    # (see also: np.roll)
    data5[-1] = np.random.normal()
    curve5.setData(data5)

    ptr5 += 1
    curve6.setData(data5)
    curve6.setPos(ptr5, 0)


# update all plots
def update():
    update1()
    update2()
    update3()


timer = pg.QtCore.QTimer()
timer.timeout.connect(update)
timer.start(50)
proxy = pg.SignalProxy(p1.scene().sigMouseMoved, rateLimit=60, slot=mouseMoved)

# Start Qt event loop unless running in interactive mode or using pyside.
if __name__ == '__main__':
    import sys
    if (sys.flags.interactive != 1) or not hasattr(QtCore, 'PYQT_VERSION'):
        QtGui.QApplication.instance().exec_()

---

---

看起来效果不错，但是在ROS通信的时候，数据在C++和python文件之间的传递显得很费事，读写文件的方式也非常麻烦，想要通过ROS话题发布的方式来通信，但是ROS中有没有集成pyqtgraph库，或报错缺少文件。所以接下来还是通过rqt_plot的方式来可视化数据。值得一提的是，rqt_plot中集成了pyqtgraph库接口，但是需要自己下载源代码编译。

1.发布话题查看：

首先代码写的很清楚了，发布的是geometry_msgs::WrenchStamped ftreadings;

void FTSensorPublisher::publishMeasurements()
    {
    // Recall that this has to be transformed using the stewart platform
    //tf_broadcaster_.sendTransform(tf::StampedTransform(nano_top_frame_, ros::Time::now(), "/world", "/nano_top_frame"));
    geometry_msgs::WrenchStamped ftreadings;
    float measurements[6];
    ftsensor_->getMeasurements(measurements);
 
    ftreadings.wrench.force.x = measurements[0];
    ftreadings.wrench.force.y = measurements[1];
    ftreadings.wrench.force.z = measurements[2];
    ftreadings.wrench.torque.x = measurements[3];
    ftreadings.wrench.torque.y = measurements[4];
    ftreadings.wrench.torque.z = measurements[5];
 
    ftreadings.header.stamp = ros::Time::now();
    ftreadings.header.frame_id = frame_ft_;
 
    pub_sensor_readings_.publish(ftreadings);
 
    ROS_INFO("Measured Force: %f %f %f Measured Torque: %f %f %f", measurements[0], measurements[1], measurements[2], measurements[3], measurements[4], measurements[5]);
}

运行ft_sensor节点，通过rostopic list查看。

2.修改launch文件：

在最后面加上了两个node pkg 用来启动两个rqt_plot，分别展示力和力矩，如果想要每个轴单独显示，多添加几个即可。

<launch>
  <arg name="ip" default="192.168.1.1"/>
  <arg name="frame" default="/ati_link"/>
  <arg name="respawn" default="true" />

  <node pkg="ati_sensor" name="ft_sensor" type="ft_sensor_node" respawn="$(arg respawn)" output="screen">
    <param name="ip" value="$(arg ip)" />
    <param name="frame" value="$(arg frame)" />
  </node>

  <node pkg="rqt_plot" name="rqt_plot_force" type="rqt_plot" args="/ft_sensor/data/wrench/force/x:y:z">
  </node>

  <node pkg="rqt_plot" name="rqt_plot_torque" type="rqt_plot" args="/ft_sensor/data/wrench/torque/x:y:z">
  </node>

</launch>

3.再次启动： 

连接好力矩传感器之后，再次运行launch文件

$ roslaunch ati_sensor ft_sensor.launch