通過 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