ROS的学习与使用

2020-08-03

一、ROS简介

ros是机器人操作系统（Robot Operating System）的英文缩写。ROS是用于编写机器人软件程序的一种具有高度灵活性的软件架构。ROS的原型源自斯坦福大学的STanford Artificial Intelligence Robot (STAIR) 和 Personal Robotics (PR)项目。

ROS是什么

首先来看wiki上的解释：
ROS（Robot Operating System，下文简称“ROS”）是一个适用于机器人的开源的元操作系统。它提供了操作系统应有的服务，包括硬件抽象，底层设备控制，常用函数的实现，进程间消息传递，以及包管理。它也提供用于获取、编译、编写、和跨计算机运行代码所需的工具和库函数。ROS 的主要目标是为机器人研究和开发提供代码复用的支持。ROS是一个分布式的进程（也就是“节点”）框架，这些进程被封装在易于被分享和发布的程序包和功能包中。ROS也支持一种类似于代码储存库的联合系统，这个系统也可以实现工程的协作及发布。这个设计可以使一个工程的开发和实现从文件系统到用户接口完全独立决策（不受ROS限制）。同时，所有的工程都可以被ROS的基础工具整合在一起。
一句话来说：ROS是一整套包括通讯机制、开发工具、应用功能和生态系统的，目标是提高机器人研发过程中提高机器人软件复用率的解决方案。

ROS的历史

$\\ \\$

二、ROS的核心概念

1.节点与节点管理器

节点（Node）———— 执行单源

执行具体任务的进程，独立运行的可执行文件；
不同节点可使用不同的编程语言，可以运行在不同的主机；
节点在系统中的名称必须是唯一的
(总而言之，一个节点类似于Windows中一个exe文件，是一个最小的可执行单源,而不同的节点之间可以使用不同的 编程语言 这是需要我们注意的。)

节点管理器（Node Master）———— 控制中心

为节点提供命名和注册服务。
跟踪并且记录话题/服务的通信情况，辅助节点互相查找，建立链接。
提供参数服务器，节点使用此服务器存储和检索运行时的参数。

2.话题通信

话题（Topic）————异步通信机制

节点之间用来传输数据的重要总线
使用发布/订阅模型，数据又发布者传输到订阅者，同一个话题的发布者和订阅者可以不唯一。

消息（Message）————话题数据

具有一定的类型和数据结构。包括ROS提供的标准类型以及，用户自定义的类型；
使用编程语言无关的.msg文件来定义，编译过程中生成相应的代码文件。

3.服务（Service）————同步通信机制

使用客户端/服务端（Client/Server）模型，客户端发送请求数据，服务器完成后返回应答数据
使用编程语言无关的.srv文件定义请求和应答数据结构，编译过程中生成相应的代码文件.

话题与服务的区别：话题时单向传输，服务是双向的应答。

4. 参数(Parameter)————全局共享字典

可以通过网络访问的共享、多变量的字典
节点使用此服务器来存储和检索运行时参数
适合存储静态、非二进制的配置参数，不适合存储动态配置的数据

5. 文件系统

功能包(Package)

ROS软件中的基本单元，包含节点源代码，配置文件，数据定义灯

功能包清单(Package manifest)

记录功能包的基本信息，包含捉着信息，许可信息，依赖选项

元功能包(Meta Package)

组织多个用于统一目的的功能包

$\\ \\$

三、ROS中的命令行工具

rqt_graph:是一个利用图表的方式展示当前节点之间工作与通信状况的工具。
rosnode:ros中所有与节点相关的指令。
1. rosnode list: 展示当前ros环境中的节点列表。
2. rosnode info: 查看某一具体节点信息
rostopic:查看当前ros中与话题相关的信息。
1. rostopic list: 展示当前ros环境中的话题列表。
2. rostopic pub (-r): 在没有-r的情况下，rostopic pub只发布一次消息，存在r的话后面可以添加数字，那么则会按照相应的频率发布消息。
rosservice: 查看ROS中与服务有关的信息。
1. rosservice list: 展示当前ROS环境的服务列表。
2. rosservice call: 发布服务类型，同时会有返回数据。
rosbag: 能够记录消息与服务等类型的所有数据。

$\\ \\$

四、工作空间与工作包

1.工作空间

ROS中的工作空间也就是一个存放工程开发相关文件的文件夹。其中包含代码空间、编译空间、开发空间、安装空间四大版块。

src: 代码空间（Source Space）
build: 编译空间 (Build Space)
devel: 开发空间 (Development Space)
install: 安装空间 (Install Space)
2.工作空间的创建
Step1:创建工作空间:

$ mkdir -p ~/catkin_ws/src
$ cd ~/catkin_ws/src
$ catkin_init_workspace

Step2:编译工作空间

$ cd ~/catkin_ws/
$ catkin_make

Step3:设置环境变量

$ source devel/setup.bash

Step4:检察环境变量

$ echo $ROS_PACKAGE_PATH

3.功能包的创建

Step1:创建功能包

$ cd ~/Catkin_ws/src
$ catkin_create_pkgtest_pkgstd_msgs rospy roscpp

Step2:编译功能包

$ cd ~/catkin_ws
$ catkin_make
$ source ~/catkin_ws/devel/setup.bash

同一个工作空间下是不允许存在多个同名功能包的，不同工作空间下是可以存在同名功能包的。

五、发布者Publish的编程实现

1.话题模型

2.话题的发布

(1).创建发布者代码

创建发布者代码总共有以下几个步骤，分别是：

初始化ROS节点；
向ROS Master注册节点信息，其中包括话题名和话题中的消息类型；
创建消息数据；
按照一定频率循环发布消息；

/
语言：c++
此例程将发布turtle/cmd_vel话题，消息类型为geometry_msgs::Twist /

#include<ros/ros.h>
            #include<geometry_msgs/Twist.h>
            int main(int argc, char** argv)
            {
                //ROS节点初始化
                ros::init(argc, argv,"");
                
                //创建节点句柄
                ros::Nodehandle;
                
                //创建一个Publisher,发布名为/turtle1/cmd/vel的topic,消息类型为geometry_msgs::Twist,队列长度为10
                ros::Publisher turtle_vel_pub = n.advertise<geometry_msgs::Twist>("/turtle/cmd_vel",10);
                
                //设置循环的频率
                ros::Rate_loop(10);
                
                int count=0;
                while (ros::ok){
                    //初始化geometry_msgs::Twist类型的消息
                    geometry_msgs::Twist vel_msg;
                    vel_msg.linear.x = 0.5;
                    vel_msg.angular.z = 0.2;
        
                    //发布消息
                    turtle_vel_pub.publish(vel_msg);
                    ROS_INFO("Publish turtle velocity command [%0.2f m/s, %0.2f rad/s]",vel_msg.linear.x,vel_msg.angular.z)
                    
                    //按照时间频率循环
                    loop_rate.sleep();
                }
                return 0;
            }

/
语言：python
此例程将发布turtle/cmd_vel话题，消息类型为geometry_msgs::Twist /

#! /usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
from geometry_msgs.msg import Twist
def velocity_publisher():
    #ROS节点初始化
    rospy.init_node("velocity_publisher", anonymous=True)
    #创建一个Publisher,发布名为/turtle1/cmd_vel的topic,消息类型为geometry_msgs::Twist,队列长度为10
    turtle_vel_pub = rospy.Publisher("/turtle/cmd_vel",Twist,queue_size=10)
    #设置循环频率
    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        #初始化geometry_msgs::Twist类型的消息
        vel_msg = Twist()
        vel_msg.linear.x = 0.5;
        vel_msg.angular.z = 0.2;
        #发布消息
        turtle_vel_pub.publish(vel_msg);
        rospy.loginfo("Publish turtle velocity command[%0.2f m/s, %0.2f rad/s]",vel_msg.linear.x,vel_msg.angular.z)
        rate.sleep()    
    if __name__ == "__main__":
        try:
            velocity_publisher():
        except rospy.ROSInterruptException:
            pass

(2).配置发布者代码编译规则

设置CMakeLists.txt中的编译规则：

设置需要编译的代码和生成的可执行文件
设置链接库

add_executable(velocity_publisher src/velocity_publisher.cpp)
target_link_libraried(velocity_publisher${catkin_LIBRARIES})

六、订阅者Subscriber的编程实现

同样我们可以参考上一节的话题模型的图片。以及工作流程:初始化节点，创建句柄，创建订阅者，循环等待回调函数。

/
语言：c++
此例程将发布turtle/pose话题，消息类型为:turtlesim::Pose /

#include<ros/ros.h>
#include"turtlesim/Pose.h"
//收到订阅消息后的回调函数
void poseCallback(const turtlesim::Pose::ConstPtr msg)
{
    //将收到的消息打印出来
    ROS_INFO("Turtle pose: x:%0.6f, y:%0.6f", msg->x,msg->y);
}
int main(int argc, char **argv)
{
    //初始化ROS节点
    ros::init(argc, argv, "pose_subscriber");
    //创建节点句柄
    ros::NodeHandle n;
    //创建一个Subsciber,订阅名为：/turtle/pose的topic,注册回调函数poseCallback
    ros::Subscriber pose_sub = n.subscribe("turtle1/pose",10,poseCallback);
    //循环等待回调函数
    ros::spin();
    return 0;
}

/
语言：python
此例程将发布turtle/pose话题，消息类型为:turtlesim::Pose /

import rospy
from turtlesim.msg import Pose
def poseCallback(msg)
{
    rospy.loginfo("Turtle pose: x:%0.6f, y:%0.6f", msg.x,msg.y)
}
def pose_subscriber():
    #ROS节点的初始化
    rospy.init_node("pose_subscriber",anonymous=True)
    #创建一个Subscriber,订阅名为/turtle1/pose的topic,注册回调函数poseCallback
    rospy.Subscriber("/turtle1/pose",Pose,poseCallback)
    #循环等待回调函数
    rospy.spin()
if __name__ = "__main__":
    pose_subscriber();

七、话题消息的定义与使用

前面两节我们使用的消息类型都是ROS中已经给我们定义好的消息类型，但是很多时候实际工程需求中需要的消息类型是需要我们自己定义的。

1.自定义话题消息

Step1:定义msg文件
例如Person.msg

string name
uint8 sex
uint8 age
uint unknow=0
uint male = 1
uint8 frmale = 2

Step2:在package.xml中添加功能包依赖

<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>

Step3:在CMakeLists.txt中添加编译选项

find_package(...... message_generation)
add_message_files(FILES Person,msg)
generate_messages(DEPENDENCIES std_msgs)
catkin_package(...... message_runtime)

Step4:编译生成语言相关文件。

2.在程序中调用我们自定义的消息

按照上一节稍作修改即可。
此处需要说明的是，对于已经建立通信的两个节点之间，即使此时关闭NodeMaster,他们之间依然可以继续正常通信。此处给出一个简单的结果截图：

八、客户端Client的编程实现

首先我们来了解以下“服务”这种消息类型的模型架构：

接下来我们以产生新的小乌龟为例，介绍服务类型中客户端Client的代码编写
/
例程使用语言：C++;
该例程将请求：/spawn服务，服务数据类型：turtlesim::Spawn /

    #include<ros/ros.h>
    #include<turtlesim/Spawn.h>
    int main(int argc, char** argv)
    {
        //初始化ROS节点
        ros::init(argc,argv,"turtle_spawn");
        //创建句点节柄
        ros::NodeHandle node;
        //发现/spawn服务后，创建一个服务客户端，连接名为/spawn的Service
        ros::service::waitForService("/spawn");
        ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("/spwan");
        //初始化turtlesim::spawn的请求数据
        turtlesim::Spawn srv;
        srv.request.x = 2.0;
        srv.request.y = 2.0;
        srv.request.name = "turtle2";
        //请求服务调用
        ROS_INFO("Call service to spwan turtle[x:%0.6f, y:%0.6f, name:%s]",srv.request.x,srv.request.y);
        add_turtle.call(srv);
        ROS_INFO("Spawn turtle successfully [name:%s]",srv.request.name.c_str());
    }

同时使用Python的大体过程也是类似的，在此也给出相应的代码:
    
    #!.usr.bin.env python
    # -*- coding: utf-8 -*-
    # 该例程将请求/spawn服务，服务数据类型turtlesim::Spawn
    import sys
    import rospy
    from turtlesim.srv import Spawn
      
    def turtle_spawn():
        #ROS节点初始化
        rospy.init_node("turtle_spawn")
        #发现/spawn服务后，创建一个客户端，连接名为.spwan的service
        rospy.wait_for_service("/spawn")
        try:
            add_turtle = rospy.ServiceProxy("/spawn", Spawn)
            #请求服务器调用，输入请求数据
            response = add_turtle(3.0, 3.0, 0.0, "turtle2")
            return response.name
        except rospy.ServiceException,e:
            print("Service call failed: %d",%e)
    if __name__ = "__main__":
    print "Spawn turtle successfully [name:%s]" %(turtle_spawn())

代码编写完成后，记得在CMakeList.txt文件中修改编译规则:

add_excutable(turtle_spawn src/turtle_spawn.cpp)
target_link_libraries(turtle_spawn ${catkin_LIBRARIES})

于是在命令行中首先启动roscore,而后启动turtlesim,最后启动我们刚刚写好的文件即可得到如下结果：（我们在(2,2)位置产生了一只新的海龟。）

九、服务端Service的编程实现

ROS中实现一个服务器主要有以下四个步骤:

初始化ROS节点
创建Server实例
循环等待服务请求，进入回调函数
在回调函数中完成服务功能的处理，并同时反馈应答数据

接下来我们首先给出C++语言编写的程序代码：

/
例程使用语言：C++;
该例程将执行/turtle_command服务，服务数据类型为:std_srvs/Trigger /

    #include<ros/ros.h>
    #include<geometry_msgs/Twist.h>
    #include<std_srvs/Trigger.h>
    //
    ros::Publisher turtle_vel_pub;
    bool pubCommand = false;
    //Service 回调函数，输入参数req，输出参数res
    bool commandCallback(std_srvs::Trigger::Request &req,
                        std::rsvs::Trigger::Response &res)
    {
        pubCommand = !pubCommand;
        ROS_INFO("Publish turtle velocity command [%s]",pubCommand=true?"Yes":"No");
        //
        //设置反馈数据
        res.success = true;
        res.message = "Change turtle command state!";
        return true;
    }
    int main(int argc, char** argv)
    {
        //ROS节点初始化
        ros::init_node(argc,argv,"turtle_command_service");
        //创建节点句柄
        ros::NodeHandle n;
        //创建一个名为/turtle_command的server,注册回调函数为commandCallback
        ros::ServiceServer command_service = n.advertiseService("/turtle_command",commandCallback);
        //创建一个Publisher,发布名为/turtle/cmd_vel的topic，消息类型为geometry_msgs,队列长度为10；
        turtle_vel_pub = n.advertise<geometry_msgs::Twist>("/turtle/cmd_vel",10);
        //循环等待回调函数
        ROS_INFO("Ready to receive turtle command.")；
        //设置循环频率
        ros::Rate_loop_rate(10);
        //循环
        while(ros::ok)
        {
            //查看一次回调函数
            ros::spinOnce();
            //如果标志为true,则发布速度指令
            if(pubCommand)
            {
                geometry_msgs::Twist vel msg;
                vel_msg.linear.x = 0.5;
                vel_msg.angular.z = 0.2;
                turtle_vel_pub.publish(vel_msg);
            }
            //按照循环频率延时
            loop_rate.sleep();
        }
    }

<div align=center>
    <img src="/imgs/ROS的学习与使用/Server结果.png">
</div>

同样，我们可以使用python来写相应程序
/
例程使用语言：python;
该例程将执行/turtle_command服务，服务数据类型为:std_srvs/Trigger /

import rospy
import thread,time
from geometry_msgs.msg import Twist
from std_srvs.srv import Trigger, TriggerResponse
pubCommand = False;
turtle_vel_pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
def command_thread():   
    while True:
        if pubCommand:
            vel_msg = Twist()
            vel_msg.linear.x = 0.5
            vel_msg.angular.z = 0.2
            turtle_vel_pub.publish(vel_msg)
            
        time.sleep(0.1)
def commandCallback(req):
    global pubCommand
    pubCommand = bool(1-pubCommand)
    # 显示请求数据
    rospy.loginfo("Publish turtle velocity command![%d]", pubCommand)
    # 反馈数据
    return TriggerResponse(1, "Change turtle command state!")
def turtle_command_server():
    # ROS节点初始化
    rospy.init_node('turtle_command_server')
    # 创建一个名为/turtle_command的server，注册回调函数commandCallback
    s = rospy.Service('/turtle_command', Trigger, commandCallback)
    # 循环等待回调函数
    print "Ready to receive turtle command."
    thread.start_new_thread(command_thread, ())
    rospy.spin()
if __name__ == "__main__":
    turtle_command_server()

十、服务数据的类型与使用

在实际的需求场景中，数据类型往往需要我们自己按照实际情况和需求构造。这就是本节课程的主要学习内容。

1.服务数据的定义

服务数据的定义使用srv文件，此文件的定义不依赖于语言。
例如：Person.srv；需要特别说明的服务类型的数据需要使用横线做区分，横线以上是request数据，横线以下是response数据,一定要注意横线。

string name
uint8 age
uint8 sex
uint8 unknown=0
uint8 male=1
uint8 female=2
---
string result

完成创建之后需要记得在package.xml中添加功能包依赖:

<build_depend>message_generation<\build_depend>
<exec_depend>message_runtime<\exec_depend>

在CMakeList.txt中添加编译选项:

find_package(... message_generation)
add_service_files(FILES Person.srv)
catkin_package(... message_runtime)

生成相关的编译文件。此时会生成三个编译文件分别叫做Person.h,PersonRequest.h和PersonResponse.h,其中后两个分别是横线上下编译后所得到的结果，Person.h是后两者总和后的结果。

接下来我们来编写相应的c++的Client和Server文件。

/
Client端：
语言：C++
该例程将请求/show_person服务，服务数据类型learning_service::Person /

#include <ros/ros.h>
#include "learning_service/Person.h"
int main(int argc, char** argv)
{
    // 初始化ROS节点
    ros::init(argc, argv, "person_client");
    // 创建节点句柄
    ros::NodeHandle node;
    // 发现/spawn服务后，创建一个服务客户端，连接名为/spawn的service
    ros::service::waitForService("/show_person");
    ros::ServiceClient person_client = node.serviceClient<learning_service::Person>("/show_person");
    // 初始化learning_service::Person的请求数据
    learning_service::Person srv;
    srv.request.name = "Tom";
    srv.request.age  = 20;
    srv.request.sex  = learning_service::Person::Request::male;
    // 请求服务调用
    ROS_INFO("Call service to show person[name:%s, age:%d, sex:%d]", 
             srv.request.name.c_str(), srv.request.age, srv.request.sex);
    person_client.call(srv);
    // 显示服务调用结果
    ROS_INFO("Show person result : %s", srv.response.result.c_str());
    return 0;
};

/
Server端：
语言：C++
该例程将请求/show_person服务，服务数据类型learning_service::Person /

#include <ros/ros.h>
#include "learning_service/Person.h"
// service回调函数，输入参数req，输出参数res
bool personCallback(learning_service::Person::Request  &req,
                    learning_service::Person::Response &res)
{
    // 显示请求数据
    ROS_INFO("Person: name:%s  age:%d  sex:%d", req.name.c_str(), req.age, req.sex);
    // 设置反馈数据
    res.result = "OK";
    return true;
}
int main(int argc, char **argv)
{
    // ROS节点初始化
    ros::init(argc, argv, "person_server");
    // 创建节点句柄
    ros::NodeHandle n;
    // 创建一个名为/show_person的server，注册回调函数personCallback
    ros::ServiceServer person_service = n.advertiseService("/show_person", personCallback);
    // 循环等待回调函数
    ROS_INFO("Ready to show person informtion.");
    ros::spin();
    return 0;
}

十一、ROS中参数的使用与编程方法

首先对于参数服务器模型，我们可以参考下图：

首先我们介绍参数文件，参数文件常常以yaml的形式存在，我们也称其为参数文件，如下：

background_b:255
background_g:86
background_r:69
rosdistro:'melodic'
roslaunch:
    uris:{host_hcx_vpc__43764:'http://hcx-vpc:43763/'}
rosversion:'1.14.3'
run_id:285y235234hfjdg9375348y

接下来我们介绍ros中参数命令：

rosparam list: 列出当前有多少参数
rosparam get param_key: 显示某个参数值
rosparam set param_key param_value: 设置某个参数值
rosparam dump file_name: 保存参数到文件
rosparam load file_name: 从文件读取参数
rosparam delete param_key: 删除参数

接下来我们来使用代码设置和获取参数：

/**
 * 该例程设置/读取海龟例程中的参数
 */
#include <string>
#include <ros/ros.h>
#include <std_srvs/Empty.h>

int main(int argc, char **argv)
{
    int red, green, blue;

    // ROS节点初始化
    ros::init(argc, argv, "parameter_config");

    // 创建节点句柄
    ros::NodeHandle node;

    // 读取背景颜色参数
    ros::param::get("/background_r", red);
    ros::param::get("/background_g", green);
    ros::param::get("/background_b", blue);

    ROS_INFO("Get Backgroud Color[%d, %d, %d]", red, green, blue);

    // 设置背景颜色参数
    ros::param::set("/background_r", 255);
    ros::param::set("/background_g", 255);
    ros::param::set("/background_b", 255);

    ROS_INFO("Set Backgroud Color[255, 255, 255]");

    // 读取背景颜色参数
    ros::param::get("/background_r", red);
    ros::param::get("/background_g", green);
    ros::param::get("/background_b", blue);

    ROS_INFO("Re-get Backgroud Color[%d, %d, %d]", red, green, blue);

    // 调用服务，刷新背景颜色
    ros::service::waitForService("/clear");
    ros::ServiceClient clear_background = node.serviceClient<std_srvs::Empty>("/clear");
    std_srvs::Empty srv;
    clear_background.call(srv);
    
    sleep(1);

    return 0;
}

之后别忘了增加两句编译规则:

add_excutable(parameter_config src/parameter_config.cpp)
target_link_libraries(paramter_config ${catkin_LIBRARIES})

同时python程序编写：

# 该例程设置/读取海龟例程中的参数

import sys
import rospy
from std_srvs.srv import Empty

def parameter_config():
    # ROS节点初始化
    rospy.init_node('parameter_config', anonymous=True)

    # 读取背景颜色参数
    red   = rospy.get_param('/background_r')
    green = rospy.get_param('/background_g')
    blue  = rospy.get_param('/background_b')

    rospy.loginfo("Get Backgroud Color[%d, %d, %d]", red, green, blue)

    # 设置背景颜色参数
    rospy.set_param("/background_r", 255);
    rospy.set_param("/background_g", 255);
    rospy.set_param("/background_b", 255);

    rospy.loginfo("Set Backgroud Color[255, 255, 255]");

    # 读取背景颜色参数
    red   = rospy.get_param('/background_r')
    green = rospy.get_param('/background_g')
    blue  = rospy.get_param('/background_b')

    rospy.loginfo("Get Backgroud Color[%d, %d, %d]", red, green, blue)

    # 发现/spawn服务后，创建一个服务客户端，连接名为/spawn的service
    rospy.wait_for_service('/clear')
    try:
        clear_background = rospy.ServiceProxy('/clear', Empty)

        # 请求服务调用，输入请求数据
        response = clear_background()
        return response
    except rospy.ServiceException, e:
        print "Service call failed: %s"%e

if __name__ == "__main__":
    parameter_config()

一起来看看结果把：

十二、ROS中坐标系管理

在机器人中存在许多不同的关节和部件，如何描述不同的不见之间的坐标关系是机器人学一个十分困难的问题，对此ROS中研发了相应的TF功能包。TF包底层封装了所有的坐标系之间的位置关系，并将其中的关系保存为一个TF树。

接下来我们通过一个实际的海龟案例来说明TF功能包的实际作用，与实际中机器人坐标变换的使用方案。

sudo apt-install ros-melodic-turtle-tf
roslaunch turtle_tf turtle_tf_demo.launch
rosrun turtlesim turtle_teleop_key
rosrun tf view_frams

于是我们就得到如下的结果：

利用vire_fram我们容易知道，在其中有三个坐标系分别是世界坐标系和两个小海龟坐标系，于是小海龟的位置跟随变换也就是两个坐标不断重合的过程，这也说明了我们没有能够实现路径相同的原因。

十三、tf坐标系广播与监听的编程实现

这一节我们只要学习运用tf包对坐标系完成广播与监听的编程实现，具体如下:

首先我们还是需要创建功能包:

$ cd /catkin_ws/src
$ catkin_make_pkg learning_tf roscpp rospy tf turtlesim

接下来我们分别编写海龟1（广播者）与海龟2（监听者）的代码：

首先是海龟1，它发布自己的坐标位置

/**
 * 该例程产生tf数据，并计算、发布turtle2的速度指令
 */

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>

std::string turtle_name;

void poseCallback(const turtlesim::PoseConstPtr& msg)
{
    // 创建tf的广播器
    static tf::TransformBroadcaster br;

    // 初始化tf数据
    tf::Transform transform;
    transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
    tf::Quaternion q;
    q.setRPY(0, 0, msg->theta);
    transform.setRotation(q);

    // 广播world与海龟坐标系之间的tf数据
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}

int main(int argc, char** argv)
{
    // 初始化ROS节点
    ros::init(argc, argv, "my_tf_broadcaster");

    // 输入参数作为海龟的名字
    if (argc != 2)
    {
        ROS_ERROR("need turtle name as argument"); 
        return -1;
    }

    turtle_name = argv[1];

    // 订阅海龟的位姿话题
    ros::NodeHandle node;
    ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);

    // 循环等待回调函数
    ros::spin();

    return 0;
};

接下来是海龟2，他会更具海龟1 刚刚广播的坐标进行移动，尽可能使得坐标系重合。

/**
 * 该例程监听tf数据，并计算、发布turtle2的速度指令
 */

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>

int main(int argc, char** argv)
{
    // 初始化ROS节点
    ros::init(argc, argv, "my_tf_listener");

    // 创建节点句柄
    ros::NodeHandle node;

    // 请求产生turtle2
    ros::service::waitForService("/spawn");
    ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("/spawn");
    turtlesim::Spawn srv;
    add_turtle.call(srv);

    // 创建发布turtle2速度控制指令的发布者
    ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 10);

    // 创建tf的监听器
    tf::TransformListener listener;

    ros::Rate rate(10.0);
    while (node.ok())
    {
        // 获取turtle1与turtle2坐标系之间的tf数据
        tf::StampedTransform transform;
        try
        {
            listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0));
            listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
        }
        catch (tf::TransformException &ex) 
        {
            ROS_ERROR("%s",ex.what());
            ros::Duration(1.0).sleep();
            continue;
        }

        // 根据turtle1与turtle2坐标系之间的位置关系，发布turtle2的速度控制指令
        geometry_msgs::Twist vel_msg;
        vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),
                                        transform.getOrigin().x());
        vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
                                      pow(transform.getOrigin().y(), 2));
        turtle_vel.publish(vel_msg);

        rate.sleep();
    }
    return 0;
};

这里python代码如下:

import roslib
roslib.load_manifest('learning_tf')
import rospy

import tf
import turtlesim.msg

def handle_turtle_pose(msg, turtlename):
    br = tf.TransformBroadcaster()
    br.sendTransform((msg.x, msg.y, 0),
                     tf.transformations.quaternion_from_euler(0, 0, msg.theta),
                     rospy.Time.now(),
                     turtlename,
                     "world")

if __name__ == '__main__':
    rospy.init_node('turtle_tf_broadcaster')
    turtlename = rospy.get_param('~turtle')
    rospy.Subscriber('/%s/pose' % turtlename,
                     turtlesim.msg.Pose,
                     handle_turtle_pose,
                     turtlename)
    rospy.spin()

import roslib
roslib.load_manifest('learning_tf')
import rospy
import math
import tf
import geometry_msgs.msg
import turtlesim.srv

if __name__ == '__main__':
    rospy.init_node('turtle_tf_listener')

    listener = tf.TransformListener()

    rospy.wait_for_service('spawn')
    spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
    spawner(4, 2, 0, 'turtle2')

    turtle_vel = rospy.Publisher('turtle2/cmd_vel', geometry_msgs.msg.Twist,queue_size=1)

    rate = rospy.Rate(10.0)
    while not rospy.is_shutdown():
        try:
            (trans,rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0))
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            continue

        angular = 4 * math.atan2(trans[1], trans[0])
        linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2)
        cmd = geometry_msgs.msg.Twist()
        cmd.linear.x = linear
        cmd.angular.z = angular
        turtle_vel.publish(cmd)

        rate.sleep()

十四、ROS中launch启动文件的使用方法

launch文件通过XML文件实现多节点的配置和其中（可以自动启动ROS Master）.意味在终端中可能需要启动十分多的ROS节点，于是我们便开发了Launch文件来一次启动多个节点。接下来我们来介绍文件的语法：luanch文件使用标签语言。

<launch>
    <node pkg='turtlesim' name='sim1' type='turtlesim_node'>
    <node pkg='turtlesim' name='sim2' type='turtlesim_node'>
</launch>

以上述代码为例:

pkg:节点所在的功能包名称
typr:节点所在的可执行文件的名称
name:节点运行时的名称
output、respawn、required、ns、args

其他使用的比较多的语法主要有：

更多的标签内容可以参考wiki：

十五、ROS中可视化工具

首先我们简要介绍之前的rqt工具，主要有:

在此我们主要介绍Rviz工具，

Gazebo是一个功能十分强大的三维物理仿真平台：

一、ROS简介

ROS是什么

ROS的历史

二、ROS的核心概念

1.节点与节点管理器

节点（Node）———— 执行单源

节点管理器（Node Master）———— 控制中心

2.话题通信

话题（Topic）————异步通信机制

消息（Message）————话题数据

3.服务（Service）————同步通信机制

4. 参数(Parameter)————全局共享字典

5. 文件系统

功能包(Package)

功能包清单(Package manifest)

元功能包(Meta Package)

三、ROS中的命令行工具

四、工作空间与工作包

1.工作空间

2.工作空间的创建

3.功能包的创建

五、发布者Publish的编程实现

1.话题模型

2.话题的发布

(1).创建发布者代码

(2).配置发布者代码编译规则

六、订阅者Subscriber的编程实现

七、话题消息的定义与使用

1.自定义话题消息

2.在程序中调用我们自定义的消息

八、客户端Client的编程实现

九、服务端Service的编程实现

十、服务数据的类型与使用

1.服务数据的定义

十一、ROS中参数的使用与编程方法

十二、ROS中坐标系管理

十三、tf坐标系广播与监听的编程实现

十四、ROS中launch启动文件的使用方法

十五、ROS中可视化工具