博客

利用rqt_plot与Matlab分析ROS topic信息

  • Post author:
  • Post category:其他


本文使用Matlab 2013a.

后来发现matlab 工具箱可以直接处理rosbag了.

https://ww2.mathworks.cn/help/robotics/examples/work-with-rosbag-logfiles.html

最低版本Matlab R2015a, release note


https://ww2.mathworks.cn/help/robotics/release-notes.html?rntext=&startrelease=R2015a&endrelease=R2019a&groupby=release&sortby=descending&searchHighlight=

1. 背景

利用rqt_plot与Matlab分析KUKA youbot 的joint_states信息。因为在为youbot添加FollowJointTrajectory 的client之后，仿真环境中的动作与实际有出入，因此想看看机械臂的实际动作曲线。而在ROS中，内置的rqt_plot是可以提供绘图功能的，而后来发现rqt_plot的功能比较简单，随着数据量的增大，查看起来比较费劲。于是就有了后来的导入matlab中分析的部分。

ROS bag指令


https://blog.csdn.net/u010510350/article/details/72457758

2.rqt_plot绘图

照例，我们找到官网的相关部分，有个基础理解：

http://wiki.ros.org/rqt_plot

rosrun turtlesim turtlesim_node 

rqt_plot /turtle1/pose/x:y:theta
rqt_plot /turtle1/pose/x /turtle1/pose/y /turtle1/pose/theta


特别注意上面的第一句，它就是下面的简写，同一个topic下的直接冒号就好，不用输入那么多的文字。

在这里我为了避免每次要去动机械臂，将joint_states这个topic的信息已经保存为了bag文件，

录的动作是我上一篇文章，actionlib中的动作的一部分，大概是从home到伸直的一部分

。使用指令（参看《a gentle introduction to ROS》P135页） 

rosbag record -O youbot_action2.bag /joint_states

-O(是字母O, 并非数字0，它表示输出文件（output），后面跟的是你的输出文件的名字，我这里是youbot_action2.bag)，下面是我的bag的一些基本信息：

一共21.1s，包含了861条信息

我们查看一下数据内容

然后开启一个terminal，回放数据rosbag play youbot_action2.bag –clock，–clock表示使用simulate time，具体可以参考

http://wiki.ros.org/Clock

。再打开另外一个，开启rqt_plot，用命令 

 rqt_plot /joint_states/velocity[0]:velocity[1]:velocity[2]:velocity[3]:velocity[4]


因为，youbot机械臂是五个自由度，这里我们用标号0-4来引用每一个（用QwtPlot画出来的）

用MatPlot画出来的，可以看出他们刚好是左右颠倒的，我们的这个bag文件，有大概20s。右键按住鼠标上下或左右滑动进行缩放。

同理，我们也可以找出各个关节的一些信息： 

rqt_plot /joint_states/position[0]:position[1]:position[2]:position[3]:position[4]


得到的图像

图像很好看，但是实际分析起来是很头疼的，它只能是看个大概过程，了解基本曲线的动作和大概的规律，比如速度曲线的不平滑，关节位置曲线的大概变动范围（起始位置并不是0，依次是0.11，0.11，-0.11，0.11，0.12 rad）

3.Matlab绘图

其实，这个地方我也走了点弯路的，当时我用命令 

rostopic echo /joint_states


显示命令到terminal中，然后等到动作结束，用鼠标拷贝出来到txt文件，而且，最重要的问题是，由于滚屏，你只能拷贝部分运动信息，很快就舍弃这种做法了。

后来，就想起以前用命令行重定向将信息保存到了文本中，于是就很好的解决了数据保存问题。这里跟师弟学到了一个-p 参数

保存成plain text然后用–nostr去掉string信息. 

rostopic echo --nostr -p /joint_states > record2.txt

将txt文件拷贝出来到windows上，然后借助Matlab分析（这里主要是用到了seq，position和velocity）

有同学可能就会问了，明明文件中有时间信息，人家还精确到了ns，干嘛不用呢，这里有个小插曲，当时用matlab读取的时候，由于linux系统中的时间戳信息是从1970年的秒数开始计算的，数据长度很大，导致Matlab处理的时候直接用科学计数法来统计，后面的几位数都直接给过滤掉了，因而导致画出来的图明显不符合逻辑，一个时间点对应多个关节值。

因此，由于每个数据点都有自己的标号，我就简化为关节信息和点序号的对应信息了。


4. 总结

这里只是把自己的一些过程展示出来，抛砖引玉，具体的数据分析，后面还有待进一步深入。我还没有找其他的机械臂对比一下速度曲线，youbot的跟我预想的不太一样，也可能是由于参数信息的设置导致的，后面我会继续深入下去。

另外，借助于ROS本身的rqt_plot，我们看到数据图像，与matlab基本相同，对于，只是想看看大概规律来说，功能也够用了。

————————————————————————

2016.03更新

———————————————————————— 

%%
%Analysis for youbot joint states created by torque control service.
%%
clear;
clc;
clf;

filename = 'ik_service.txt';
startRow = 2;
 ALL_LINES = 1296 ; 
  endRow = ALL_LINES + 1; % all line +1
INTERVAL = 4;

 ikserviceori = importfile(filename, startRow, endRow);

t_index_colume = 1: INTERVAL : ALL_LINES;
t_value = ikserviceori(t_index_colume, 1) ;

q_start_line = 2;
j1_rank =q_start_line: INTERVAL: ALL_LINES;
j1_pos_index =  ikserviceori ( j1_rank, 5) ;

j2_rank = q_start_line: INTERVAL: ALL_LINES;
j2_pos_index =  ikserviceori ( j2_rank, 4) ;

j3_rank = q_start_line: INTERVAL: ALL_LINES;
j3_pos_index =  ikserviceori ( j3_rank, 3) ;
j4_rank = q_start_line: INTERVAL: ALL_LINES;
j4_pos_index =  ikserviceori ( j4_rank, 2) ;

j5_rank = q_start_line: INTERVAL: ALL_LINES;
j5_pos_index =  ikserviceori ( j5_rank, 1) ;

subplot(3,1,1);  xlabel('Time'); ylabel('q');
% grid on;

plot(t_value, j1_pos_index);
hold on;

plot(t_value, j2_pos_index);
hold on;

plot(t_value, j3_pos_index);
hold on;

plot(t_value, j4_pos_index);
hold on;

plot(t_value, j5_pos_index);
hold on;
legend('J1','J2','J3', 'J4','J5','location','northeast');
%%
qd_start_line = 3;
j1_qd_rank = qd_start_line: INTERVAL: ALL_LINES;
j1_qd_index =  ikserviceori ( j1_qd_rank, 5) ;

j2_qd_rank = qd_start_line: INTERVAL: ALL_LINES;
j2_qd_index =  ikserviceori ( j2_qd_rank, 4) ;

j3_qd_rank = qd_start_line: INTERVAL: ALL_LINES;
j3_qd_index =  ikserviceori ( j3_qd_rank, 3) ;

j4_qd_rank = qd_start_line: INTERVAL: ALL_LINES;
j4_qd_index =  ikserviceori ( j4_qd_rank, 2) ;

j5_qd_rank = qd_start_line: INTERVAL: ALL_LINES;
j5_qd_index =  ikserviceori ( j5_qd_rank, 1) ;

subplot(3,1,2);  xlabel('Time'); ylabel('qd');
% grid on;
plot(t_value, j1_qd_index);
hold on;

plot(t_value, j2_qd_index);
hold on;

plot(t_value, j3_qd_index);
hold on;

plot(t_value, j4_qd_index);
hold on;

plot(t_value, j5_qd_index);

legend('J1_qd','J2_qd','J3_qd', 'J4_qd','J5_qd','location','northeast');
%%
qdd_start_line = 4;
j1_qdd_rank = qdd_start_line: INTERVAL: ALL_LINES;
j1_qdd_index =  ikserviceori ( j1_qdd_rank, 5) ;

j2_qdd_rank = qdd_start_line: INTERVAL: ALL_LINES;
j2_qdd_index =  ikserviceori ( j2_qdd_rank, 4) ;

j3_qdd_rank = qdd_start_line: INTERVAL: ALL_LINES;
j3_qdd_index =  ikserviceori ( j3_qdd_rank, 3) ;

j4_qdd_rank = qdd_start_line: INTERVAL: ALL_LINES;
j4_qdd_index =  ikserviceori ( j4_qdd_rank, 2) ;

j5_qdd_rank = qdd_start_line: INTERVAL: ALL_LINES;
j5_qdd_index =  ikserviceori ( j5_qdd_rank, 1) ;

subplot(3,1,3);  xlabel('Time'); ylabel('qdd');
% grid on;
plot(t_value, j1_qdd_index);
hold on;

plot(t_value, j2_qdd_index);
hold on;

plot(t_value, j3_qdd_index);
hold on;

plot(t_value, j4_qdd_index);
hold on;

plot(t_value, j5_qdd_index);

legend('J1_qdd','J2_qdd','J3_qdd', 'J4_qdd','J5_qdd','location','northeast');

==============================================================

2018.03更新

==============================================================

  • 统计文件行数：D:\Matlab_work\Dr_xu\t_vy_y.m 
filename = 'D:\jaco\Drxu\Drxu\2CartesianTorque.txt';
fid = load(filename,'-ascii');
% [m,n] = size(fid);
m= size(fid,1);

ALL_LINES = m;
tvyy = importfile_t_vy_y(filename, 1, ALL_LINES);

发现这种导入文件的方式有时候行，有时不行，还是建议直接利用matlab的import data生成script贴入到m文件中。

  • 设置坐标轴间距和数值 D:\Matlab_work\Dr_xu\j6v_xu.m 
plot(t_value,Jv6_value,'LineWidth',2);
hold on;
plot(t_value,Myv6_value,'LineWidth',2,'Color','r');

set(gca,'YLim',[10,40],'YTick',10:2:40);

xlabel('time(s)'); ylabel('Jv6(degree/s)');
legend('Jv6','Myv6','location','northeast');
  • 显示三维点 D:\Matlab_work\Dr_xu\analysis00000.m 
plot3(x_value,y_value,z_value,'-rs','LineWidth',1,'MarkerSize',2)% x_value中全是x的数据，可以是列向量
title('Target and real position');
legend('Target','real position','location','southeast');
  • 显示最大值 D:\Matlab_work\Dr_xu\path_compare.m 
max_error=max(delta)
n = find(delta == max(delta))
plot(VarName1(n,1), VarName2(n,1),'o');  
cell_string0{1} = ['X = ' num2str(VarName1(n,1))]; 
cell_string0{2} = ['Y = ' num2str(VarName2(n,1))];  
text(VarName1(n,1)-0.023, VarName2(n,1)-0.0025, cell_string0);

如果有多个最大值 D:\Matlab_work\Dr_xu\t_vy_y.m 

vy_index_max = find(vy_value == max(vy_value));

if(size(vy_index_max)==[1,1])
    
    vy_index_max = find(vy_value == max(vy_value));
else
    vy_index_max = vy_index_max(1,1);
end
  • 改变窗口位置 D:\Matlab_work\Dr_xu\j6v_xu.m 
hfigure_Jv51 =figure('Name','Jv51');
hfigure_Jv51 =figure;
set(hfigure_Jv51,'Position',[W/7,H/12,W/2,H/2])

==============2019 Matlab rosbag==================


https://ww2.mathworks.cn/help/robotics/examples/work-with-rosbag-logfiles.html

roscore

rosrun turtlesim turtlesim_node

rostopic type /turtle1/cmd_vel

geometry_msgs/Twist

rostopic type /turtle1/color_sensor

turtlesim/Color


版权声明：本文为yaked原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。