ROS与Matlab语言入门教程-TurtleBot的通信

本例程介绍TurtleBot平台和MATLAB用户与TurtleBot的交互方式。特别地，本例程的代码将演示如何发布消息（例如速度消息）到TurtleBot，以及如何订阅TurtleBot发布到话题的消息（例如里程计消息）。
为使本例程正常工作，必须运行TurtleBot。
预备知识：4.2 开始使用Gazebo和仿真的TurtleBot，或者4.1开始使用实际的TurtleBot。

连接到TurtleBot

TurtleBot必须运行。如果用户使用真实的TurtleBot，并且根据4.1开始使用实际的TurtleBot中硬件设置步骤，机器人就已经运行。如果用户使用仿真的TurtleBot并且根据4.2 开始使用Gazebo和仿真的TurtleBot中的步骤设置，从桌面启动其中一个Gazebo环境（例如，Gazebo TurtleBot World）。

如果MATLAB软件运行在宿主计算机上，运行如下的命令。用TurtleBot的IP地址替换示例IP地址（192.168.1.1），下面的命令初始化Gazebo并连接到TurtleBot。

ipaddress = '192.168.1.1'
rosinit(ipaddress)

如果用户用于连接到TurtleBot的网络不是默认的网络适配器，用户可以手动指定用于连接到机器人的适配器的IP地址。用户使用无线网络可能发生这种情况，但是仍然还有激活的以太网连接。用TurtleBot的IP地址替换IP_OF_TURTLEBOT，用于连接到机器人的宿主适配器的IP地址替换IP_OF_HOST_COMPUTER。

rosinit('IP_OF_TURTLEBOT','NodeHost','IP_OF_HOST_COMPUTER');

输入如下的命令显示所有可用的ROS话题：

rostopic list

如果用户不能看到任务话题，说明网络设置失败。参考本文档的开头部分的网络设置步骤。如果用户使用Gazebo，列表如下图所示。注意，下图只显示了部分话题。

移动机器人并播放声音

用户可以通过发布消息到“/mobile_base/commands/velocity”话题，控制TurtleBot的运动。消息必须是“geometry_msgs/Twist”类型，且包含数据说明需要的线性和角度速度。TurtleBot的运动可以通过两个不同的值控制：X轴方向的线性速度，控制前进和后退；Z轴方向的角速度，控制机器人基座的旋转速度。设置“velocity”变量用于TurtleBot的简单运动。

velocity = 0.1; % meters per second

为“/mobile_base/commands/velocity”话题创建发布器，相关的消息包含速度数值。

robot = rospublisher('/mobile_base/commands/velocity')
velmsg = rosmessage(robot)
%* Set the forward velocity (along the X-axis) of the robot based on the
% |velocity| variable and publish the command to the robot. The TurtleBot
% will move forward a small distance and then come to a stop.
%
velmsg.Linear.X = velocity;
send(robot,velmsg);

安全起见，如果TurtleBot一直从“/mobile_base/commands/velocity”话题接收到速度数据，它才会一直前进。
执行下述命令，查看速度话题发布的消息类型：

rostopic type /mobile_base/commands/velocity

话题要求“geometry_msgs/Twist”类型的消息，这正是上面创建的“velmsg”的类型。

使用“rostopic info TOPICNAME”命令，查看那些节点发布、订阅指定的话题。下面的指令列出了速度话题的发布器和订阅器，MATLAB被列出作为其中一个发布器。

rostopic info /mobile_base/commands/velocity

可选：如果用户使用真实的TurtleBot，还可以发送声音指令给它。

soundpub = rospublisher('/mobile_base/commands/sound')
soundmsg = rosmessage(rostype.kobuki_msgs_Sound);
soundmsg.Value = 6; % Any number 0-6
send(soundpub,soundmsg);

接收机器人的位置和方向

TurtleBot使用“/odom”话题发布它的当前位置和方向（位姿）。由于TurtleBot没有装配GPS系统，当前的位姿是相对于机器人初始位姿。为里程计消息创建订阅器：

odom = rossubscriber('/odom')

等待订阅器返回数据，然后提取数据，在分配到x,y和z变量。

odomdata = receive(odom,3);
pose = odomdata.Pose.Pose;
x = pose.Position.X;
y = pose.Position.Y;
z = pose.Position.Z;

注意，如果用户看到错误，通常是“receive”指令时间溢出，确保里程计被发布和网络设置正确。
显示x,y和z的值

[x,y,z]

TurtleBot的方向以四元数的形式存储在“pose.Orientation”变量。使用“quat2eul”转换到欧拉角的更简便表示方式。执行下述命令，可以显示当前机器人的方向角“theta”(°)。

quat = pose.Orientation;
angles = quat2eul([quat.W quat.X quat.Y quat.Z]);
theta = deg2rad(angles(1))

接收图像数据

确保Kinect相机正在运行，如果用户再次运行“rostopic list”，可以看到许多以“/camera”开头的话题被列出来，真实TurtleBot硬件，用户可以发现如下话题：

/camera/rgb/image_color/compressed

为订阅该话题，使用下面的命令：

imsub = rossubscriber('/camera/rgb/image_color/compressed');

如果用户使用Gazebo，话题列表是不一样的，则应使用下面的话题。

/camera/rgb/image_raw

为订阅该话题，使用下面的命令：

imsub = rossubscriber('/camera/rgb/image_raw');

在订阅图像话题之后，等待数据然后用“imshow”显示。

img = receive(imsub);
imshow(readImage(img));

在Gazebo中，图像类似如下所示：

Kinect相机获得的真实环境中的图像类似如下所示。

使用如下的“while”循环，显示持续Kinect相机更新的图像。

tic;
while toc < 20
img = receive(imsub);
imshow(readImage(img))
end

断开与机器人的连接

当用户完成工作后，清楚工作空间中的发布器、订阅器和其它有关ROS的对象是个良好的习惯。

clear

建议当完成ROS网络有关的工作之后，使用“rosshutdown”指令关闭全局节点和断开与Gazebo的连接。

rosshutdown