【ROS教程 003】ROS系统初体验

需要特别说明：ROS版本必须与Ubuntu系统版本匹配
实验环境：
ROS Fuerte
Ubuntu 12.04
在进行实际使用ROS开发之前，首先对ROS中的一些基础操作内容进行学习。使用TurtleSim进行练习，其中包括如何建立功能包、使用节点、使用参数服务和移动一个虚拟的机器人。
（1）查看ROS文件系统
使用rospack和rosstack来获取有关功能包、功能包集、路径和依赖性等信息。
查找turtlesim包的路径

$ rospack find turtlesim

获得功能包或功能包集下面的文件列表

$ rosls turtlesim

进入到某个文件夹，使用roscd命令

$ roscd turtlesim

(2) 创建工作空间
查看ROS正在使用的工作空间

$ echo $ROS_PACKAGE_PATH

我们将要创建的文件夹是在～/dev/rosbook/中，使用如下命令

$ cd ~
$ mkdir -p dev/rosbook

创建好文件夹后我们需要将新路径添加到ROS_PACKAGE_PATH，需要在～/.bashrc文件的末尾添加新的一行：

$ echo "export ROS_PACKAGE_PATH=~/dev/rosbook:${ROS_PACKAGE_PATH}" >> ~/.bashrc
$ . ~/.bashrc

这样我们就在ROS系统中创建和配置完成自己的新文件夹。
（3）创建ROS功能包
可以使用手动创建功能包，为了避免繁琐工作，推荐使用roscreate-pck命令行工具。
在rosbook文件夹下创建新的功能包

$ roscreate-pkg davebobo_tutorials std_msgs rospy roscpp

此功能包的格式包括功能包名称和依赖项；其中依赖项包括std_msgs rospy roscpp，如下命令行所示

$ roscreate-pkg [package_name] [depend1] [depend2] [depend3]

值得说明的是：

• std_msgs 包含了常见消息类型，表示基本数据类型和其他基本的消息构造，如多维数组
• rospy 一个ROS的纯Python客户端库。
• roscpp 使用C++实现的ROS的各种功能。程序员能够用这些接口快速完成与ROS的主题、服务和参数相关的开发工作。

正确执行以上命令后的效果：

我们可以使用rospack、roscd、rosls命令来获取新的功能包信息。
（4）编译ROS功能包
如果创建了一个功能包并且编写了一下代码，就需要编译功能包了。编译功能包时主要是代码的编译过程，为了编译功能包，可以使用命令rosmak

roscd davebobo_tutorials
rosmake davebobo _tutorial

几秒之后编译成功。
（5）使用ROS节点
节点都是可执行程序，位于packagename/bin目录中,我们使用turtlesim的功能包进行练习。
在开始之前，必须使用如下命令启动roscore

$ roscore

使用rosrun命令启动一个新的节点。

$ rosrun turtlesim turtlesim_node

我们可以看见出现一个新窗口，窗口中间出现一个小海龟。

这时再去查看节点列表就会看到出现一个新的节点。叫做/turtlesim。使用rosnode info Nodename查看节点的信息。

$ rosnode info /turtlesim
--------------------------------------------------------------------------------
Node [/turtlesim]
Publications:
 * /turtle1/color_sensor [turtlesim/Color]
 * /rosout [rosgraph_msgs/Log]
 * /turtle1/pose [turtlesim/Pose]

Subscriptions:
 * /turtle1/command_velocity [unknown type]

Services:
 * /turtle1/teleport_absolute
 * /turtlesim/get_loggers
 * /turtlesim/set_logger_level
 * /reset
 * /spawn
 * /clear
 * /turtle1/set_pen
 * /turtle1/teleport_relative
 * /kill

contacting node http://davebobo-virtual-machine:60313/ ...
Pid: 3770
Connections:
 * topic: /rosout
  * to: /rosout
  * direction: outbound
  * transport: TCPROS

以上信息中，包括发布者（及相应主题）、订阅者（及相应主题）和该节点具有的服务（srv）及它们各自唯一的名称。
（6）使用主题与节点交互
首先需要打开一个终端在里面运行roscore：

$ roscore

再打开一个终端，在里面运行一个turtlesim_node节点：

$ rosrun turtlesim turtlesim_node

打开另一个终端，在里面输入：

$ rosrun turtlesim turtle_teleop_key

在运行完这条命令后，在这个终端下，按键盘上的方向键可以看到，之前我们运行的乌龟开始移动，:-)，很有意思吧。
urtlesim_node和turtle_teleop_key通过ROS的主题来相互通信。turtle_teleop_key把用户按下的键发布到主题上，turtlesim_node也订阅了同一个主题用来接收用户按下的键，并做出相应的动作。我们有这个例子可以体会到主题的作用，更加深刻的认识到了节点的概念。
可以使用以下命令查看/teleop_turtle和/turtlesim节点的信息。

$ rosnode info /teleop_turtle
$ rosnode info /turtlesim

使用rostopic pub [topic] [msg_type] [args]命令直接发布主题。

$ rostopic pub -1 /turtle1/command_velocity turtlesim/Velocity -- 1 1

可以看到小海龟在做曲线运动。
（7）使用服务
同上，先运行roscore并启动turtlesim节点。使用以下命令列出在turtlesim节点运行时系统提供的服务。

$ rosservice list

使用/spawn服务在不同的方向和不同的位置创建另一只小海龟。

$ rosservice call /spawn 3 3 0.2 "new_turtle"

（8）使用参数服务器
参数服务器用于存储所有节点均可以访问的共享数据。ROS中用于管理参数服务器的工具称为rosparam,查看被所有节点所使用的服务器参数:

$ rosparam list

可以使用get参数读取某个值，也可以使用set参数设定一个新的值。rosparam的另外一个特性是参数转存。通过该参数可以保存或加载参数服务器的内容。
使用rosparam dump [file_name]来保存参数服务器。

$ rosparam dump save.yaml

使用rosparam load [file_name] [namesapce]像参数服务器加载一个新的数据文件。

$ rosparam load save.yaml namespace

（9）创建节点和编译节点
本节使用一个实例进行学习，一个发布一些数据，另一个接收这些数据，两个节点之间使用最基本的工作进行通信。进入如下文件位置：

$ roscd davebobo_tutorials/src/

创建两个文件并分别命名为exaple1_a.cpp和example_b.cpp， exaple1_a.cpp文件将会发送带有节点名称的数据， exaple1_b.cpp文件会把这些数据显示在shell窗口中。
exaple1_a.cpp中的代码如下：

#include "ros/ros.h"
#include "std_msgs/String.h"
#include <sstream>

int main(int argc,char **argv)
{
     ros::init(argc,argv,"example1_a");
     ros::NodeHandle n;
     ros::Publisher chatter_pub = n.advertise<std_msgs::String>("message",1000);
     ros::Rate loop_rate(10);
while(ros::ok())
{
std_msgs::String msg;
std::stringstream ss;
ss << "I am the example1_a node";
msg.data = ss.str();
chatter_pub.publish(msg);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}

exaple1_b.cpp中的代码如下：

#include "ros/ros.h"
#include "std_msgs/String.h"

void chatterCallback(const std_msgs::String::ConstPtr&msg)
{
     ROS_INFO("I heard: [%s]",msg->data.c_str());
}

int main(int argc,char **argv)
{
ros::init(argc,argv,"example1_b");
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe("message",100,chatterCallback);
ros::spin();
return 0;
}

编写好程序后，当使用davebobo_tutorials功能包时，需要自行编辑CMakeLists.txt文件。使用自己喜欢的编辑器或直接使用rosed工具将会在VIM编辑器下打开这个文件，并将以下命令行复制到文件的末尾处：

rosbuild_add_executable(example1_a src/example1_a.cpp)
rosbuild_add_executable(example1_b src/example1_b.cpp)

启动ROS，然后在不同的shell窗口下分别运行以下命令

$ rosrun davebobo_tutorials example1_a
$ rosrun davebobo_tutorials example1_b

可以发现在example1_b运行的shell窗口可以打印出INFO消息。
（10）创建并使用msg和srv文件
① 服务
首先，在davebobo_tutorials功能包下创建msg文件夹，并在其中创建一个新的文件davebobo_msg1.msg。在文件中增加以下行：

int32 A
int32 B
int32 C

接着编辑CMakeList.txt从rosbuild_genmsg()这一行中删除#，然后使用rosmake命令编译功能包。

$ rosmake dvaebobo_tutorials

为检查编译是否正确，使用rosmsg命令，如果可以看到与davebobo_msg1.msg文件中相同的内容说明编译正确。

$ rosmsg show davebobo_tutorials/davebobo_msg1

现在创建一个srv文件，同样在dvaebobo_tutorials文件夹下创建一个名为srv的文件，并新建davebobo_srv1.srv.在文件中增加以下行

int32 A
int32 B
int32 C
---
int32 sum

编辑 CMakeList.txt从rosbuild_gensrv()这一行中删除#，重新编译功能包并使用rossrv工具来检查。

$ rossrv show davebobo_tutorials/davebobo_srv1.srv

上面我们在ROS中创建一个服务，接着我们需要两个节点，一个服务器和一个客户端。在davebobo_tutorials功能包中新建两个节点分别命名为example2_a.cpp和 example2_b.cpp.
example2_a.cpp中的代码：

#include "ros/ros.h"
#include "davebobo_tutorials/davebobo_srv1.h"

bool add(davebobo_tutorials::davebobo_srv1::Request &req,
  davebobo_tutorials::davebobo_srv1::Response &res)
{
  res.sum = req.A + req.B + req.C;
  ROS_INFO("request: A=%ld, B=%ld C=%ld", (int)req.A, (int)req.B, (int)req.C);
  ROS_INFO("sending back response: [%ld]", (int)res.sum);
  return true;
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "add_3_ints_server");
  ros::NodeHandle n;

  ros::ServiceServer service = n.advertiseService("add_3_ints", add);
  ROS_INFO("Ready to add 3 ints.");
  ros::spin();

  return 0;
}

example2_b.cpp中的代码：

#include "ros/ros.h"
#include "davebobo_tutorials/davebobo_srv1.h"
#include <cstdlib>

int main(int argc, char **argv)
{
  ros::init(argc, argv, "add_3_ints_client");
  if (argc != 4)
  {
  ROS_INFO("usage: add_3_ints_client A B C ");
  return 1;
  }

  ros::NodeHandle n;
  ros::ServiceClient client = n.serviceClient<davebobo_tutorials::davebobo_srv1>("add_3_ints");
  davebobo_tutorials::davebobo_srv1 srv;
  srv.request.A = atoll(argv[1]);
  srv.request.B = atoll(argv[2]);
  srv.request.C = atoll(argv[3]);
  if (client.call(srv))
  {
  ROS_INFO("Sum: %ld", (long int)srv.response.sum);
  }
  else
  {
  ROS_ERROR("Failed to call service add_two_ints");
  return 1;
  }

  return 0;
}

为了编译节点，在CMakeList.txt文件中增加以下行：

rosbuild_add_executable(example2_a src/example2_a.cpp)
rosbuild_add_executable(example2_b src/example2_b.cpp)

执行编译命令：

$ rosmake davebobo_tutorials

分别在不同命令窗口启动节点：

$ rosrun davebobo_tutorials example2_a
$ rosrun davebobo_tutorials example2_b 1 2 3

example2_a窗口如下显示

example2_b窗口如下显示

② 消息
现在将使用自定义的msg文件来创建节点。创建example3_a.cpp和 example3_b.cpp文件，同时调用davebobo_msg1.msg
example3_a.cpp中代码

#include "ros/ros.h"
#include "davebobo_tutorials/davebobo_msg1.h"
#include <sstream>

int main(int argc, char **argv)
{
  ros::init(argc, argv, "example1_a");
  ros::NodeHandle n;
  ros::Publisher pub = n.advertise<davebobo_tutorials::davebobo_msg1>("message", 1000);
  ros::Rate loop_rate(10);
  while (ros::ok())
  {
  davebobo_tutorials::davebobo_msg1 msg;
  msg.A = 1;
  msg.B = 2;
  msg.C = 3;
  pub.publish(msg);
  ros::spinOnce();
  loop_rate.sleep();
  }
  return 0;
}

example3_b.cpp中代码

#include "ros/ros.h"
#include "davebobo_tutorials/davebobo_msg1.h"

void messageCallback(const davebobo_tutorials::davebobo_msg1::ConstPtr& msg)
{
  ROS_INFO("I heard: [%d] [%d] [%d]", msg->A, msg->B, msg->C);
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "example1_b");
  ros::NodeHandle n;
  ros::Subscriber sub = n.subscribe("message", 1000, messageCallback);
  ros::spin();
  return 0;
}

现在运行这两个节点会显示如下:

References:
[1]. Aaron Martinez Enrique Fern andez, ROS机器人程序设计[B], P14-42, 2014.
http://blog.csdn.net/xiaocainiaoshangxiao/article/details/13578493