Robotic System Toolbox 使用笔记

Matlab在2015a里引入了ROS工具箱。

官方文档：Robotic System Toolbox的官方文档

使用笔记：

1. 基础命令

rosinit：将当前matlab接入ROS的网络中，需在matlab命令窗口中运行rosinit命令，当前的matlab进程将作为一个ROS Node（称为matlab_node）在ROS master中注册。注意：若此前未开ROS master，则rosinit命令会先开启ROS master，后开启matlab_node。用rosshutdown命令可关闭matlab_node。
此工具箱提供的rostopic、rosmsg、rosnode、rosservice等命令与ROS中的类似。

2. 使用ROS Publisher和Subscriber交换信息

(1)robotics.ros.Node命令可以构造独立于matlab_node的新node。即使在matlab中使用rosshutdown关闭了matlab_node后，这些独立于matlab_node的node不会受影响。
(2)rospublisher和robotics.ros.Publisher均可用于发送消息。其中，使用robotics.Rate或rosrate可以以设定发送消息的频率，具体使用参考以特定频率发送消息。
(3)rossubscriber和robotics.ros.Subscriber均可用于订阅消息。其中，matlab提供两种接收消息的方法：receive和callback函数。
需要注意的是，凡是运行rospublisher、rossubscriber等函数时，均默认使用当前的matlab_node来发送或订阅消息；凡是用robotics.ros.Publisher、robotics.ros.Subscriber均使用指定的node来发送或订阅消息。

例子1：

segmentPub = rospublisher('/simple_msgs', 'std_msgs/Float64');
segmentMsg = rosmessage('std_msgs/Float64');
segmentMsg.Data   = 3.33; % 注意，matlab中消息的字段都是大写开头的
send(segmentPub,segmentMsg);
sub = rossubscriber('/simple_msgs');
pause(1) % Wait for message to update
sub.LatestMessage

例子2：

node1 = robotics.ros.Node('/test_node_1');
node2 = robotics.ros.Node('/test_node_2');
pub = robotics.ros.Publisher(node1,'/chatter','std_msgs/String');
sub = robotics.ros.Subscriber(node2,'/chatter','std_msgs/String');
msg = rosmessage('std_msgs/String');
msg.Data = 'Message from Node 1';
send(pub,msg) % Sent from node 1
pause(1) % Wait for message to update
sub.LatestMessage

Matlab的subscriber目前还存在着一些缺陷。
(1). 无法设置 Matlab 中的 ROS 订阅者的callback函数的运行频率。当用于接收数据时,几乎21%的数据将丢失。这不是由回调函数的运行频率太低引起的。实际上，订阅者的回调函数的运行频率很高。
(2). 大量消息的丢失是由于 matlab 中的callback函数很容易被挂起,因而 Ros 订阅者无法接收处理消息。Matlab 中的一个命令“drawnow”可以帮助解决这个问题。它可以更新图像并处理任何被挂起的回调函数。
(3). 这个工具箱的另一个缺陷是未提供在函数中接收实时的 ROS 消息的机制。ROS 消息是实时的,这意味着负责接收 ROS 消息的函数也应该被设计为实时在线的。否则,函数将立即完成,并会在 ROS 订阅者刚刚建立时终止。解决这个问题的方法是使用另一个数组来跟踪数据,使用 while 循环支持在线工作。此 while 循环有两部分:前台工作和后台工作。将已经接收到的数据写入另一个用于跟踪的数组中,这一工作在前台运行。而ROS 订阅者接收数据则在后台运行。只要确保 ROS 订阅者接收到的数据数量略大于复制到跟踪数组的数量,就可以保证一直在线接收。这个机制加上“drawnow”命令的效果很明显,数据的损失率降低到了1.2%。解决方法见例子3。

例子3：（实时接收信息）

function [Wren_loc] = subscribeUsingCallback

    rosshutdown;
    rosinit;

    % 许多在ROS的topic、message，在matlab中也有提供，例如baxter相关的topic和message类型，命名规则虽与ROS的有大小写的区别，但是可以互相通信
    wrenchPub = rospublisher('/robot/limb/right/endpoint_state', 'baxter_core_msgs/EndpointState');
    % 使用的callback函数可以在后面列出的函数wrenchCallback中看到
    wrenchHandle  = rossubscriber('/robot/limb/right/endpoint_state',@wrenchCallback, 'BufferSize', 1000);

    % subscriber的callback函数需要使用全局变量来保存接收到的信息
    global globalIndex;
    global Wrench;
    globalIndex     = 0;
    Wrench          = zeros(1000,7);

    % 跟踪数组，使得程序不会过早被退出，保证可以持续接收实时数据
    localIndex  = 0;
    Wren_loc    = zeros(1000,7);

    while(1)
        % （1）还没接收到任何数据时，等待输入
        while (localIndex==globalIndex)
            pause(2);
        end
        drawnow;

        % (2)当数据尚未接收完毕时,将接收到的数据拷入跟踪数组中
        while (localIndex1; %其中"1"可以根据需要调整
            if(currentIndex>globalIndex)
                currentIndex=globalIndex;
            end

            Wren_loc(localIndex+1:currentIndex,:)=Wrench(localIndex+1:currentIndex,:);

            localIndex  = currentIndex;
            drawnow;
        end
        drawnow;
        % (3)当数据已经接收完时，退出
        if(localIndex==globalIndex)
            break;
        end
    end
end

function wrenchCallback(~,message)
    global Wrench; global globalIndex;
    Tr = message.Header.Stamp;
    Ts = double(Tr.Sec)+double(Tr.Nsec)*10^-9;
    Fx = message.Wrench.Force.X;
    Fy = message.Wrench.Force.Y;
    Fz = message.Wrench.Force.Z;
    Mx = message.Wrench.Torque.X;
    My = message.Wrench.Torque.Y;
    Mz = message.Wrench.Torque.Z;

    if((Fx~=0)||(Fy~=0)||(Fz~=0)||(Mx~=0)||(My~=0)||(Mz~=0))
        globalIndex = globalIndex+1;
        Wrench(globalIndex,:) = [Ts Fx Fy Fz Mx My Mz];
    end
end

3. 消息类型：使用基础的消息类型，使用特殊的消息类型(如图像、点云等)，使用自定义的ROS消息类型。其中，自定义的消息类型需要附加安装一个ROS自定义消息支持包，安装方法如下：

(1)若matlab版本是R2015b, 在matlab命令窗口运行命令： roboticsSupportPackages 并跟随其指令进行安装。
(2)若matlab版本是R2016a及以上, 在matlab命令窗口运行命令： roboticsAddons 并跟随其指令进行安装。

并且需要有包含完整的消息定义的、按ROS规则编写的ROS包。生成方法：
(1) 指定生成的自定义消息的放置路径，例如： folderpath = ‘/home/drinkcor/MATLAB/custom_msg’
(2) 把ROS包拷贝到刚刚指定的路径的文件夹下
(3) 生成自定义的消息： >> rosgenmsg(folderpath)
(4) 把生成的自定义消息的放置路径添加到matlab的运行路径里。
至此，自定义的消息便可以使用。

4. 使用ROS service