rqt_plot工具——ROS中查看變量時間趨勢線

rqt_plot工具——ROS中查看變量時間趨勢線

工欲善其事，必先利其器

本篇博文依舊是小白教程，最近在寫利用二次規劃方法優化機械臂關節軌跡的代碼。想要通過查看誤差變量的時間趨勢線看一下控制效果。百度了一下，有rqt_plot這個親民的小白包。

1.百度到了什麼？

https://blog.csdn.net/ktigerhero3/article/details/80927310
這個博主的博文講得比較清楚，就沒有繼續往下搜了。

2.然後看到了什麼？

注意，由於rqt_plot開啓了一個節點，所以要先運行roscore，把master運行起來。

$ roscore
$ rosrun rqt_plot rqt_plot

這個時候，我們會看到彈出了一個窗口，先不管他。知己知彼百戰不殆，姑且從宏觀的角度看一下這個節點的信息。

$ rostopic list

然後沒有彈出什麼特別的與rqt_plot相關的東西，也就是說，這個rqt_plot本身不會發布消息。

$ rosnode list

可以看到一個匿名發佈的節點，也就是說，可以任意開n個rqt_plot也是不影響的。
$ rosnode info rqt_gui_py_node_xxx
查看一下這個節點的信息，看到該節點沒有訂閱消息，提供了兩種服務，沒有發佈任何消息。

$ rosnode info /rqt_gui_py_node_23864 
--------------------------------------------------------------------------------
Node [/rqt_gui_py_node_23864]
Publications: 
 * /rosout [rosgraph_msgs/Log]

Subscriptions: None

Services: 
 * /rqt_gui_py_node_23864/get_loggers
 * /rqt_gui_py_node_23864/set_logger_level


contacting node http://dianyehuang-G7-7588:33125/ ...
Pid: 23864
Connections:
 * topic: /rosout
    * to: /rosout
    * direction: outbound
    * transport: TCPROS

3.怎麼用？

從功能上看，rqt_plot需要訂閱消息，接收這些消息然後按時間序列畫出曲線。在彈出的窗口中，我們可以在topic的文本框中填寫我們自己編寫的發佈的“topic”。接下來需要考慮的是，消息的類型應該是怎麼樣子的？由前面的博主的代碼中我們可以看到，可以是單個變量，也可以是向量。Vector3的類型是怎麼樣的？我們可以通過rosmsg show 來看一下：

$ rosmsg show geometry_msgs/Vector3
float64 x
float64 y
float64 z

可見，當我們自定義msg的時候，能夠對應上變量就可以了。

4. 測試代碼

自定義msg

rqtpltMsg,msg
float64 x
float64 y
int32 z

對應代碼如下：

#include <ros/ros.h>
#include "my_package/rqtpltMsg.h"
#include <iostream>
#include <math.h>

using namespace std;

int main(int argc, char** argv)
{
    ros::init(argc,argv, "test_rqtplt");
    ros::NodeHandle nh;
    ros::Publisher my_pub = nh.advertise<my_package::rqtpltMsg>("my_msg_plt",1);

    my_package::rqtpltMsg msg;
    ros::Rate loopRate(200);
    double t=0;
    while(ros::ok()){
        msg.x=0.1*sin(2*t);
        msg.y=0.2*cos(t);
        msg.z=0;
        my_pub.publish(msg);
        loopRate.sleep();
        t+=1.0/200.0;
    }
}