【ROS】ROS入门笔记-基础知识篇

ROS入门笔记

前言:
大三夏季小学期，自学一下ROS，参考资料为古月居的“ROS入门21讲”、哈工大李湛老师的“机器人操作系统创新实践训练营”和ROS Wiki。

一、ROS的安装与环境配置

本次安装选择的环境为Ubuntu 20.04 64位操作系统和ROS。虚拟机为VMware Workstation Pro 15。

1.在VM虚拟机中安装Ubuntu 20

需要注意的是：

• 安装过程可能很漫长，请务必耐心等待
• 安装前断网可以加速安装过程
• 用户名和密码请尽可能简单
• 请安装VM tools增强工具
• 设置共享文件夹
• 更换清华源（虽然阿里源更常见，但我喜欢清华源）

对安装完成的系统，连上互联网，在终端执行

sudo apt-get update
sudo apt-get upgrade

需要注意的是：

• 如果你在更新过程中出现了“部分安装包无法安装”的情况，请多运行几次上述两行命令
• 或者保险起见，无论如何都多执行几次

2.连接ros服务器

需要执行的有四步：

1. 此处省略
2. 此处省略
3. 此处省略
4. 此处省略

在这里建议使用GitHub Release 二进制文件的方式安装，而不是终端的方法。这种方法更稳妥。

3.安装ROS

在执行这一步的时候，我建议

• 使用手机热点
• 开启Qv2ray

无论是古月居的还是李老师的教程大多是基于Ubuntu 18 和ROS Melodic Morenia的，我们这里安装Ubuntu 20和ROS Noetic Ninjemys。新版本的安装更为便捷。

具体安装步骤请参考ROS Wiki

http://wiki.ros.org/cn

需要注意的是：
以下安装步骤不保证最新，一切以ROS Wiki为准。

3.1 配置Ubuntu软件仓库

在Ubuntu的“软件与更新”中，确保前四项是勾选的。

3.2 设置sources.list

一般来说我们这样设置

sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'

但这里为了加速下载我们使用清华镜像

sudo sh -c '. /etc/lsb-release && echo "deb http://mirrors.tuna.tsinghua.edu.cn/ros/ubuntu/ `lsb_release -cs` main" > /etc/apt/sources.list.d/ros-latest.list'

还有更多镜像源详见ROS Wiki

3.3 设置密钥

sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654

若无法连接到密钥服务器，可以尝试替换上面命令中的 hkp://keyserver.ubuntu.com:80 为 hkp://pgp.mit.edu:80 。

你也可以使用curl命令替换apt-key命令，这在使用代理服务器的情况下比较有用：

curl -sSL 'http://keyserver.ubuntu.com/pks/lookup?op=get&search=0xC1CF6E31E6BADE8868B172B4F42ED6FBAB17C654' | sudo apt-key add -

3.4 开始安装

首先执行

sudo apt update

之后执行

sudo apt install ros-noetic-desktop-full

注意：

• 这一步如果出现了“部分安装包无法安装”的情况，请再运行一上述命令

3.5 设置环境

你需要在使用ROS的每个bash终端中source这个脚本。

source /opt/ros/noetic/setup.bash

而下面这些命令可以在每次启动新的shell窗口时很方便地为你自动source一下这个脚本：

Bash

echo "source /opt/ros/noetic/setup.bash" >> ~/.bashrc
source ~/.bashrc

如果同时安装了好几个ROS发行版，~/.bashrc只会生效你当前使用的这个版本的setup.bash。

4.测试ROS是否安装成功

在Ubuntu中打开三个终端窗口。

在第一个窗口中输入

roscore

这时候会出现一些输出。

在第二个窗口输入

rosrun turtlesim turtlesim_node

这时候将弹出一个蓝色窗口，中央有一只海龟。

在第三个窗口输入

rosrun turtlesim turtle_teleop_key

此时在这个窗口按动上下左右键（不是WASD），即可控制小海龟运动。

如果可以控制小海龟运动，则表示ROS环境配置成功。

5.其他可能需要安装的东西

vim

sudo apt-get install vim

python

sudo apt-get install python

C++编译器

sudo apt-get install g++

补充：在NVIDIA Jetson Nano上安装ROS

最近正好拿到一套Jetson Nano，尝试在这个开发板上部署ROS。

1.安装操作系统

准备一张大于16Gb的microSD卡，从官网下载镜像，解压后使用Win32DiskImager写入SD卡，插入Jetson nano即可使用。

镜像下载地址：
https://developer.nvidia.com/zh-cn/embedded/downloads

Win32DiskImager下载：
https://sourceforge.net/projects/win32diskimager/

2.安装ROS

还没成功

二、ROS的核心概念

1.ros体系

ROS = 通信机制 + 开发工具 + 应用功能 + 生态系统

ROS

通信机制

计算图

开发工具

命令行&编译器

TF坐标变换

Rviz

QT工具箱

Gazebo

......

应用功能

Navigation

SLAM

Movelt!

各种功能包

生态系统

发行版

软件源

百科ROS_Wiki

邮件列表

问答ROS_Answer

博客

ROS社区

软件仓库

元功能包

功能包

节点

消息

服务

2.ROS核心概念

2.1 节点与节点管理器

节点（Node）——执行单元

• 独立的可执行文件
• 命名必须唯一
• 编程语言可以不同

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

• 为节点提供命名和注册服务
• 跟踪、记录话题和服务
• 辅助节点间查找
• 提供参数服务器

2.2 话题通讯

话题（Topic）

• 异步通信
• 发布/订阅模型
• 同一话题的发布者或订阅者可以不唯一

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

• .msg文件

ROS Node
Publisher

Topic: /example
Message Type: std_msgs/String

ROS Node
Subscriber

ROS Node
Subscriber

2.3 服务通信

服务（Service）

• 客户端/服务端模型
  • 客户端发送请求数据
  • 服务器处理后返回应答数据
• .srv文件

request

response

ROS Node
Service Client

ROS Node
Service Server

2.4 话题 VS 服务

	话题	服务
同步性	异步	同步
通信模型	发布 / 订阅	服务器 / 客户端
底层协议	ROSTCP / ROSUDP	ROSTCP / ROSUDP
反馈机制	无	有
缓冲区	有	无
实时性	弱	强
节点关系	多对多	一对多
适用场景	数据传输	逻辑处理

2.5 参数

参数（Parameter）

• 网络访问
• 多变量字典
• 用于存储运行时的参数
• 静态、非二进制
• 传输方式不是话题或服务

也有一种动态配置方法

2.6 文件系统

功能包

• ROS软件中的基本单元
• 包含节点源码、配置文件、数据定义等

功能包清单

• 记录功能包的基本信息
• 包含作者信息、许可信息、依赖选项、编译标志等

元功能包

• 多个同一目的的功能包

文件系统

元功能包

功能包

功能包清单

消息类型

服务类型

代码

其他

三、ROS命令行工具的使用

1.基础指令

1.1 roscore 环境搭建

roscore

基础命令，用以启动ROS环境

1.2 rosrun 运行

运行命令

rosrun + 功能包名 + 节点名

例如

# 启动小海龟仿真器
rosrun turtlesim turtlesim_node

# 启动小海龟控制节点
rosrun turtlesim turtle_teleop_key

1.3 rqt_graph 可视化工具

rqt开头的工具都是基于Qt的可视化工具。

# 显示计算图
rqt_graph

1.4 rosnode 显示节点相关信息

# 列出所有节点（包括默认节点rosout）
rosnode list

# 查看某个节点信息
rosnode info + 节点名

例如

rosnode info /turtlesim

1.5 rostopic 显示话题相关信息

发布消息，例如

# 让小海龟画圆
rostopic pub -r 10 /turtle1/cmd_vel geometry_msgs/Twist "linear:
  x: 1.0
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 1.0" 

1.6 rosmsg 显示消息相关信息

1.7 rosservice 显示服务相关信息

# 生成一只新海龟
rosservice call /spawn "x: 0.0
y: 0.0
theta: 0.0
name: 'turtlr2'" 

1.8 rosbag 话题记录和复现

# 话题记录
rosbag record -a -O cmd_record

# 话题复现
rosbag play cmd_record.bag

1.9 rossrv 信息查询

例如

rossrv show std_srvs/Trigger

四、创建工作空间与功能包

1.工作空间（workspace）

• src
  • 放置功能包
• build
  • 中间文件，一般不用管
• devel
  • 库、脚本
• install
  • 安装空间，开发一般用不到

2.创建工作空间

2.1 创建工作空间

# 在home下名为catkin_ws的新建文件夹
mkdir -p  ~/catkin_ws/src

# 进入catkin_ws文件夹
cd ~/catkin_ws/src

# 将当前文件夹属性转化为工作空间
catkin_init_workspace

2.2 编译工作空间

# 进入catkin_ws文件夹
cd ~/catkin_ws/

# 或使用返回上一级
cd ..

# 编译文件夹
catkin_make
catkin_make install

2.3 设置环境变量

source devel/setup.bash

2.4 检查环境变量

echo $ROS_PACKAGE_PATH

3.创建功能包

3.1 创建功能包

# 格式
catkin_creat_pkg <功能包名> [依赖1] [依赖2]

# 例如
cd ~/catkin_ws/src
catkin_creat_pkg test_pkg roscpp rospy std_msgs

3.2 编译功能包

cd ~/catkin_ws

# 编译
catkin_make

# 设置环境变量
source ~/catkin_ws/devel/setup.bash

五、发布者Publisher的编程实现

该部分使用Publisher操作海龟运动。

1.创建功能包

cd ~/catkin_ws/src

catkin_create_pkg learning_topic roscpp rospy std_msgs geometry_msgs turtlesim

2.创建Publisher代码（C++）

2.1 如何实现一个Publisher

1. 初始化ROS Node（节点）
2. 向ROS Master注册节点信息，包括发布的话题名和话题中的消息类型
3. 创建消息数据
4. 按照一定频率循环发布消息

2.2 例程

/**
 * 该例程将发布turtle1/cmd_vel话题，消息类型geometry_msgs::Twist
 */
 
#include 
#include 

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

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

        // 创建一个Publisher，发布名为/turtle1/cmd_vel的topic，消息类型为geometry_msgs::Twist，队列长度10
        ros::Publisher turtle_vel_pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);

        // 设置循环的频率
        ros::Rate loop_rate(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("Publsh turtle velocity command[%0.2f m/s, %0.2f rad/s]", 
                                vel_msg.linear.x, vel_msg.angular.z);

            // 按照循环频率延时
            loop_rate.sleep();
        }

        return 0;
}

3.配置Publisher代码编译规则（C++）

3.1 配置CMakeList.txt中的编译规则

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

3.2 实现方法

打开CMakeList.txt文件，找到build块的末尾，添加如下内容

# 把velocity_publisher.cpp编译成velocity_publisher
add_executable(velocity_publisher src/velocity_publisher.cpp)
# 链接库
target_link_libraries(velocity_publisher ${
     catkin_LIBRARIES})

4.编译并运行Publisher（C++）

# 进入目录
cd ~/catkin_ws

# 编译
catkin_make

#设置环境变量
source devel/setup.bash

---

关于设置环境变量这一步
如果觉得每次都设置一次环境变量太麻烦了，也可以这么做。

在home目录下按下Ctrl+H显示隐藏文件，找到文件.bashrc，在最后一行添加

# 每个人路径不同
source /opt/ros/noetic/setup.bash
source /home/work/catkin_ws/devel/setup.bash

重新启动终端以生效。

---

打开一个新的终端

# 启动ROS环境
roscore

打开第二个新的终端

# 启动海龟模拟器
rosrun turtlesim turtlesim_node

打开第三个新的终端

rosrun learning_topic velocity_publisher

此时可观察到海龟做圆周运动。

5.创建Publisher代码（Python）

例程

# 该例程将发布turtle1/cmd_vel话题，消息类型geometry_msgs::Twist

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('/turtle1/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("Publsh 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

6.运行Publisher（Python）

Python程序无需编译可直接执行。

打开一个新的终端

# 启动ROS环境
roscore

打开第二个新的终端

# 启动海龟模拟器
rosrun turtlesim turtlesim_node

打开第三个新的终端

rosrun learning_topic velocity_publisher.py

此时可观察到海龟做圆周运动。

我未能成功运行Python程序。
受此原因，本文档的后半部分不再写Python版本的教程。

六、订阅者Subscriber的编程实现

1.创建Subscriber代码（C++）

因为延续使用上面已经创建好的文件包，这里我们直接新建文件。

1.1 如何实现一个Subscriber

1. 初始化ROS 节点 Node
2. 订阅需要的话题 Topic
3. 循环等待话题消息，接收到消息后进入回调函数
4. 在回调函数中完成消息处理

1.2 例程

/**
 * 该例程将订阅/turtle1/pose话题，消息类型turtlesim::Pose
 */
 
#include 
#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;

    // 创建一个Subscriber，订阅名为/turtle1/pose的topic，注册回调函数poseCallback
    ros::Subscriber pose_sub = n.subscribe("/turtle1/pose", 10, poseCallback);

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

    return 0;
}

2.配置Subscriber的编译规则（C++）

2.1 配置CMakeList.txt中的编译规则

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

2.2 实现方法

打开CMakeList.txt文件，找到build块的末尾，添加如下内容

# 把pose_subscriber.cpp编译成pose_subscriber 
add_executable(pose_subscriber src/pose_subscriber .cpp)
# 链接库
target_link_libraries(pose_subscriber  ${catkin_LIBRARIES})

3.编译并运行Subscriber（C++）

# 进入目录
cd ~/catkin_ws

# 编译
catkin_make

# 设置环境变量
source devel/setup.bash

打开一个新的终端

# 启动ROS环境
roscore

打开第二个新的终端

# 启动海龟模拟器
rosrun turtlesim turtlesim_node

打开第三个新的终端

rosrun learning_topic pose_subscriber

此时可见不断输出海龟位置信息。

打开第四个新的终端

rosrun turtlesim turtle_teleop_key

此时在这个窗口按动上下左右键（不是WASD），即可控制小海龟运动，可见窗口三输出的位置信息在变化。

4.创建Subscriber代码（Python）

例程

需要注意的是，我这里使用的是Python 3，但不一定都好用，我仍不建议使用Python开发ROS。

#!/usr/bin/env python3
# -*- coding: utf-8 -*-

########################################################################
####          Copyright 2020 GuYueHome (www.guyuehome.com).          ###
########################################################################

# 该例程将订阅/turtle1/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()

5.运行Subscriber（Python）

Python程序无需编译可直接执行。

打开一个新的终端

# 启动ROS环境
roscore

打开第二个新的终端

# 启动海龟模拟器
rosrun turtlesim turtlesim_node

打开第三个新的终端

rosrun learning_topic pose_subscriber.py

此时可见不断输出海龟位置信息。

打开第四个新的终端

rosrun turtlesim turtle_teleop_key

此时在这个窗口按动上下左右键（不是WASD），即可控制小海龟运动，可见窗口三输出的位置信息在变化。

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

在上述部分我们使用了现成的Topic中的geometry_msgs::Twist和turtlesim::Pose类型，在实际使用中，我们可能会使用到一些全新的消息格式。对此，我们也可以自行定义Topic。

1.如何自定义Topic话题

1.1 定义msg文件

例如如下的Person.msg文件

string name
uint8 sex
uint8 age

uing8 unknown  =  0
uint8 male 	   =  1
uint8 female   =  2

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

message_generation</build_depend>
message_runtime</exec_depend>

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

find_package( ...... message_generation)

add_message_files(FILES Person.msg)
generate_messages(DEPENDENCIES std_msgs)

catkin_package( ...... message_runtime)

1.4 编译生成语言相关文件

# 进入目录
cd ~/catkin_ws

# 编译
catkin_make

# 设置环境变量
# 之前设置过了环境变量的不用运行这句了
source devel/setup.bash

此时在~/catkin_ws/devel/include/learning_topic/目录下会生成Person.h头文件。

2.创建Publisher代码（C++）

例程

/**
 * 该例程将发布/person_info话题，自定义消息类型learning_topic::Person
 */
 
#include 
#include "learning_topic/Person.h"

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

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

    // 创建一个Publisher，发布名为/person_info的topic，消息类型为learning_topic::Person，队列长度10
    ros::Publisher person_info_pub = n.advertise<learning_topic::Person>("/person_info", 10);

    // 设置循环的频率
    ros::Rate loop_rate(1);

    int count = 0;
    while (ros::ok())
    {
     
        // 初始化learning_topic::Person类型的消息
    	learning_topic::Person person_msg;
		person_msg.name = "Tom";
		person_msg.age  = 18;
		person_msg.sex  = learning_topic::Person::male;

        // 发布消息
		person_info_pub.publish(person_msg);

       	ROS_INFO("Publish Person Info: name:%s  age:%d  sex:%d", 
				  person_msg.name.c_str(), person_msg.age, person_msg.sex);

        // 按照循环频率延时
        loop_rate.sleep();
    }

    return 0;
}

3.创建Subscriber代码（C++）

例程

/***********************************************************************
Copyright 2020 GuYueHome (www.guyuehome.com).
***********************************************************************/

/**
 * 该例程将订阅/person_info话题，自定义消息类型learning_topic::Person
 */
 
#include 
#include "learning_topic/Person.h"

// 接收到订阅的消息后，会进入消息回调函数
void personInfoCallback(const learning_topic::Person::ConstPtr& msg)
{
     
    // 将接收到的消息打印出来
    ROS_INFO("Subcribe Person Info: name:%s  age:%d  sex:%d", 
			 msg->name.c_str(), msg->age, msg->sex);
}

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

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

    // 创建一个Subscriber，订阅名为/person_info的topic，注册回调函数personInfoCallback
    ros::Subscriber person_info_sub = n.subscribe("/person_info", 10, personInfoCallback);

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

    return 0;
}

4.配置代码编译规则（C++）

配置CMakeList.txt文件

# 把person_publisher.cpp编译成person_publisher
add_executable(person_publisher src/person_publisher.cpp)
# 设置链接库
target_link_libraries(person_publisher ${catkin_LIBRARIES})
# 添加依赖项
add_dependencies(person_publisher ${PROJECT_NAME}_generate_messages_cpp)

# 把person_subscriber.cpp编译成person_subscriber
add_executable(person_subscriber src/person_subscriber.cpp)
# 设置链接库
target_link_libraries(person_subscriber ${catkin_LIBRARIES})
# 添加依赖项
add_dependencies(person_subscriber ${PROJECT_NAME}_generate_messages_cpp)

5.编译并运行Publisher和Subscriber（C++）

# 进入目录
cd ~/catkin_ws

# 编译
catkin_make

# 设置环境变量
# 之前设置过了环境变量的不用运行这句了
source devel/setup.bash

打开一个新的终端

# 启动ROS环境
roscore

打开第二个新的终端

rosrun learning_topic person_subscriber

打开第三个新的终端

rosrun learning_topic person_publisher

此时可见publisher不断发布topic，subscriber不断接收topic。

---

值得注意的是，此时即便关闭ROS Master（即roscore终端），publisher和subscriber之间的通讯也不会中断。

这是因为，ROS Master起到的是联系介绍的功能，当这个功能完成之后，两个Node之间可以自行工作，不再依赖ROS Master了。

---

6.创建Publisher和Subscriber代码（Python）

6.1 Publisher代码

#!/usr/bin/env python
# -*- coding: utf-8 -*-

########################################################################
####          Copyright 2020 GuYueHome (www.guyuehome.com).          ###
########################################################################

# 该例程将发布/person_info话题，自定义消息类型learning_topic::Person

import rospy
from learning_topic.msg import Person

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

	# 创建一个Publisher，发布名为/person_info的topic，消息类型为learning_topic::Person，队列长度10
    person_info_pub = rospy.Publisher('/person_info', Person, queue_size=10)

	#设置循环的频率
    rate = rospy.Rate(10) 

    while not rospy.is_shutdown():
		# 初始化learning_topic::Person类型的消息
    	person_msg = Person()
    	person_msg.name = "Tom";
    	person_msg.age  = 18;
    	person_msg.sex  = Person.male;

		# 发布消息
        person_info_pub.publish(person_msg)
    	rospy.loginfo("Publsh person message[%s, %d, %d]", 
				person_msg.name, person_msg.age, person_msg.sex)

		# 按照循环频率延时
        rate.sleep()

if __name__ == '__main__':
    try:
        velocity_publisher()
    except rospy.ROSInterruptException:
        pass

6.2 Subscriber代码

#!/usr/bin/env python
# -*- coding: utf-8 -*-

########################################################################
####          Copyright 2020 GuYueHome (www.guyuehome.com).          ###
########################################################################

# 该例程将订阅/person_info话题，自定义消息类型learning_topic::Person

import rospy
from learning_topic.msg import Person

def personInfoCallback(msg):
    rospy.loginfo("Subcribe Person Info: name:%s  age:%d  sex:%d", 
			 msg.name, msg.age, msg.sex)

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

	# 创建一个Subscriber，订阅名为/person_info的topic，注册回调函数personInfoCallback
    rospy.Subscriber("/person_info", Person, personInfoCallback)

	# 循环等待回调函数
    rospy.spin()

if __name__ == '__main__':
    person_subscriber()

7.运行Publisher和Subscriber（Python）

打开一个新的终端

# 启动ROS环境
roscore

打开第二个新的终端

rosrun learning_topic person_subscriber.py

打开第三个新的终端

rosrun learning_topic person_publisher.py

此时可见publisher不断发布topic，subscriber不断接收topic。

没运行成功

八、客户端Client的编程实现

1.创建功能包

# 切换目录
cd ~/catkin_ws/src

# 新建功能包，申明依赖
catkin_create_pkg learning_service roscpp rospy std_msgs geometry_msgs turtlesim

2.创建Client代码（C++）

2.1 如何实现一个Client

1. 初始化ROS节点
2. 创建一个Client实例
3. 发布服务请求数据
4. 等待Server处理之后的应答结果

2.2 例程

/**
 * 该例程将请求/spawn服务，服务数据类型turtlesim::Spawn
 */

#include 
#include 

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>("/spawn");

    // 初始化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, srv.request.name.c_str());

	add_turtle.call(srv);

	// 显示服务调用结果
	ROS_INFO("Spwan turtle successfully [name:%s]", srv.response.name.c_str());

	return 0;
};

3.配置Client代码编译规则（C++）

修改CMakeList.txt

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

4.编译并运行客户端（C++）

# 进入目录
cd ~/catkin_ws

# 编译
catkin_make

# 设置环境变量
# 之前设置过了环境变量的不用运行这句了
source devel/setup.bash

打开一个新的终端

# 启动ROS环境
roscore

打开第二个新的终端

rosrun turtlesim turtlesim_node

打开第三个新的终端

rosrun learning_service turtle_spawn

此时可见增加了一个海龟。

5.创建Client代码（Python）

#!/usr/bin/env python
# -*- coding: utf-8 -*-

########################################################################
####          Copyright 2020 GuYueHome (www.guyuehome.com).          ###
########################################################################

# 该例程将请求/spawn服务，服务数据类型turtlesim::Spawn

import sys
import rospy
from turtlesim.srv import Spawn

def turtle_spawn():
	# ROS节点初始化
    rospy.init_node('turtle_spawn')

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

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

if __name__ == "__main__":
	#服务调用并显示调用结果
    print "Spwan turtle successfully [name:%s]" %(turtle_spawn())

6.运行Client（Python）

打开一个新的终端

# 启动ROS环境
roscore

打开第二个新的终端

rosrun turtlesim turtlesim_node

打开第三个新的终端

rosrun learning_service turtle_spawn.py

运行失败

九、服务端Server的编程实现

1.创建Server代码（C++）

1.1 如何实现一个Server

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

1.2 例程

/**
 * 该例程将执行/turtle_command服务，服务数据类型std_srvs/Trigger
 */
 
#include 
#include 
#include 

ros::Publisher turtle_vel_pub;
bool pubCommand = false;

// service回调函数，输入参数req，输出参数res
bool commandCallback(std_srvs::Trigger::Request  &req,
         			std_srvs::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(argc, argv, "turtle_command_server");

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

    // 创建一个名为/turtle_command的server，注册回调函数commandCallback
    ros::ServiceServer command_service = n.advertiseService("/turtle_command", commandCallback);

	// 创建一个Publisher，发布名为/turtle1/cmd_vel的topic，消息类型为geometry_msgs::Twist，队列长度10
	turtle_vel_pub = n.advertise<geometry_msgs::Twist>("/turtle1/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();
	}

    return 0;
}

2.配置Server代码编译规则（C++）

修改CMakeList.txt

add_executable(turtle_command_server  src/turtle_command_server.cpp)
target_link_libraries(turtle_command_server ${catkin_LIBRARIES})

3.编译并运行Server（C++）

# 进入目录
cd ~/catkin_ws

# 编译
catkin_make

# 设置环境变量
# 之前设置过了环境变量的不用运行这句了
source devel/setup.bash

打开一个新的终端

# 启动ROS环境
roscore

打开第二个新的终端

rosrun turtlesim turtlesim_node

打开第三个新的终端

rosrun learning_service turtle_command_server 

打开第四个新的终端

rosservice call /turtle_command "{}"

这一句运行海龟运动，再运行一次海龟停止。

4.创建Server代码（Python）

#!/usr/bin/env python
# -*- coding: utf-8 -*-

########################################################################
####          Copyright 2020 GuYueHome (www.guyuehome.com).          ###
########################################################################

# 该例程将执行/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()

5.运行Server（Python）

打开一个新的终端

# 启动ROS环境
roscore

打开第二个新的终端

rosrun turtlesim turtlesim_node

打开第三个新的终端

rosrun learning_service turtle_command_server.py

打开第四个新的终端

rosservice call /turtle_command "{}"

这一句运行海龟运动，再运行一次海龟停止。

运行失败

十、服务端的定义与使用

1.如何自定义服务数据

1.1 定义srv文件

string name
uint8 age
uint8 sex

uint8 unknown = 0
uint8 male = 1
uint8 female = 2
---
string result

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

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

1.3 在CMakeList.txt添加编译选项

find_package( ...... message_generation)

add_service_files(FILES Person.srv)
generate_messages(DEPENDENCIES std_msgs)

catkin_package( ...... message_runtime)

1.4编译生成语言相关文件

# 进入目录
cd ~/catkin_ws

# 编译
catkin_make

# 设置环境变量
# 之前设置过了环境变量的不用运行这句了
source devel/setup.bash

此时在~/catkin_ws/devel/include/learning_service/目录下会生成Person.h、PersonRequest.h和PersonResponse.h共三个头文件。

2.创建Server代码（C++）

/**
 * 该例程将执行/show_person服务，服务数据类型learning_service::Person
 */
 
#include 
#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;
}

3.创建Client代码（C++）

/**
 * 该例程将请求/show_person服务，服务数据类型learning_service::Person
 */

#include 
#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;
};

4.配置Server/Client代码编译规则（C++）

配置CMakeList.txt文件

# 把person_server.cpp编译成person_server
add_executable(person_server src/person_server.cpp)
# 设置链接库
target_link_libraries(person_server ${catkin_LIBRARIES})
# 添加依赖项
add_dependencies(person_server ${PROJECT_NAME}_generate_messages_cpp)

# 把person_client.cpp编译成person_client
add_executable(person_client src/person_client.cpp)
# 设置链接库
target_link_libraries(person_client ${catkin_LIBRARIES})
# 添加依赖项
add_dependencies(person_client ${PROJECT_NAME}_generate_messages_cpp)

5.编译并运行Server/Client（C++）

# 进入目录
cd ~/catkin_ws

# 编译
catkin_make

# 设置环境变量
# 之前设置过了环境变量的不用运行这句了
source devel/setup.bash

打开一个新的终端

# 启动ROS环境
roscore

打开第二个新的终端

rosrun learning_service person_server 

打开第三个新的终端

rosrun learning_service person_client

这一句运行显示信息，再运行一次再次显示信息。

6.创建Server/Client代码（Python）

6.1 Server

#!/usr/bin/env python
# -*- coding: utf-8 -*-

########################################################################
####          Copyright 2020 GuYueHome (www.guyuehome.com).          ###
########################################################################

# 该例程将执行/show_person服务，服务数据类型learning_service::Person

import rospy
from learning_service.srv import Person, PersonResponse

def personCallback(req):
	# 显示请求数据
    rospy.loginfo("Person: name:%s  age:%d  sex:%d", req.name, req.age, req.sex)

	# 反馈数据
    return PersonResponse("OK")

def person_server():
	# ROS节点初始化
    rospy.init_node('person_server')

	# 创建一个名为/show_person的server，注册回调函数personCallback
    s = rospy.Service('/show_person', Person, personCallback)

	# 循环等待回调函数
    print "Ready to show person informtion."
    rospy.spin()

if __name__ == "__main__":
    person_server()

6.2 Client

#!/usr/bin/env python
# -*- coding: utf-8 -*-

########################################################################
####          Copyright 2020 GuYueHome (www.guyuehome.com).          ###
########################################################################

# 该例程将请求/show_person服务，服务数据类型learning_service::Person

import sys
import rospy
from learning_service.srv import Person, PersonRequest

def person_client():
	# ROS节点初始化
    rospy.init_node('person_client')

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

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

if __name__ == "__main__":
	#服务调用并显示调用结果
    print "Show person result : %s" %(person_client())

7.运行Server/Client（Python）

打开一个新的终端

# 启动ROS环境
roscore

打开第二个新的终端

rosrun learning_service person_server 

打开第三个新的终端

rosrun learning_service person_client

这一句运行显示信息，再运行一次再次显示信息。

未运行。

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

1.参数模型

Parameter Server

2.创建功能包

cd ~/catkin_ws/src
catkin_create_pkg learning_parameter roscpp rospy std_srvs

3. rosparam 参数命令的使用

首先打开海龟仿真器环境。

打开一个新的终端

# 启动ROS环境
roscore

打开第二个新的终端

# 启动海龟模拟器
rosrun turtlesim turtlesim_node

3.1 rosparam list 列出参数

# 直接使用
rosparam list

3.2 rosparam get 显示某个参数

# rosparam get + <参数名>，例如
rosparam get /turtlesim/background_g

3.3 rosparam set 设置某个参数

# rosparam set + <参数名> + <参数值>，例如
rosparam set /turtlesim/background_g 150

此时观察不到弹窗颜色变化，我们刷新一下。

rosservice call /clear "{}"

此时可见弹窗颜色变化。

3.4 rosparam dump 保存参数到文件

# rosparam dump + <文件名>
rosparam dump para.yaml

YAML文件会保存到当前目录。

3.5 rosparam load 从文件读取参数

# rosparam load + <文件名>
rosparam load para.yaml

我们刷新一下。

rosservice call /clear "{}"

3.6 rosparam delete 删除参数

# rosparam delete + <参数名>
rosparam delete /turtlesim/background_g

4.编程实现（C++）

4.1 如何获取/设置参数

1. 初始化ROS节点
2. get函数获取参数
3. set函数设置参数

4.2 例程

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

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;
}

5.配置代码编译规则（C++）

修改CMakeList.txt

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

6.编译并运行（C++）

# 进入目录
cd ~/catkin_ws

# 编译
catkin_make

# 设置环境变量
# 之前设置过了环境变量的不用运行这句了
source devel/setup.bash

打开一个新的终端

# 启动ROS环境
roscore

打开第二个新的终端

rosrun turtlesim turtlesim_node

打开第三个新的终端

rosrun learning_parameter parameter_config

刷新一下

rosservice call /clear "{}"

7.编程实现（Python）

#!/usr/bin/env python
# -*- coding: utf-8 -*-

########################################################################
####          Copyright 2020 GuYueHome (www.guyuehome.com).          ###
########################################################################

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

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中的坐标系管理系统

1.机器人中的坐标变换

1.1 TF功能包

1.2 海龟跟随试验

---

未完成的工作

在之前的学习中，我始终没能成功运行Python程序。这里的海龟跟随实验也是Python写的。

我找到了一些方法，可以运行海龟跟随试验了，但是Python程序仍然存在一些问题，例如空格和缩进。

具体方法为

# 进入管理员模式
sudo su

# 进入usr.bin目录
cd /usr/bin/

# 删除原默认编译器文件
rm -r python

# 复制python3编译器设置为原编译器文件
cp python3 python

---

1.2.1 启动海龟跟随

# 安装功能包，其中的noetic是ROS版本名，请自行修改
sudo apt-get install ros-noetic-turtle-tf

打开一个新的终端

roscore

打开第二个终端

# 启动launch脚本
roslaunch turtle_tf turtle_tf_demo.launch

打开第三终端

# 启动键盘控制
rosrun turtlesim turtle_teleop_key

此时用方向键移动海龟，另一只会自动跟随。

1.2.2 调试工具

PDF可视化 view_frames

#  启动可视化工具
rosrun tf view_frames

如果一切顺利，在监听五秒之后会生成一个frames.pdf文件，位置在当前终端目录下。

如果出错，请打开报错文件的地址，例如

sudo gedit /opt/ros/noetic/lib/tf/view_frames

在第88行新增一行

vstr = str(vstr)

重新运行即可。

命令行工具 tf_echo

rosrun tf tf_echo turtle1 turtle2

输出为两个海龟之间转换的数据。

三维可视化工具 rviz

rosrun rviz rviz -d `rospack find turtle_tf` /rviz/turtle_rviz.rviz

选择Fixed Frame为world，左下方点击add添加一个tf，可见屏幕上两个坐标。

此时移动海龟，两个坐标也移动。

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

1.创建功能包

# 进入工作空间目录
cd ~/catkin_ws/src

# 创建功能包
catkin_create_pkg learning_tf roscpp rospy tf turtlesim

2.创建TF广播器代码（C++）

2.1 如何实现一个TF广播器

1. 定义TF广播器（TransformBroadcaster）
2. 创建坐标变换值
3. 发布坐标变换

2.2 例程

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

#include 
#include 
#include 

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;
};

3.创建TF监听器代码（C++）

3.1 如何实现一个TF监听器

1. 定义TF监听器（TransformListener）
2. 查找坐标变换（waitForTransform、lookupTransform）

3.2 例程

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

#include 
#include 
#include 
#include 

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;
};

4.配置TF广播器与监听器代码编译规则

修改CMakeList.txt

add_executable(turtle_tf_broadcaster src/turtle_tf_broadcaster.cpp)
target_link_libraries(turtle_tf_broadcaster ${catkin_LIBRARIES})

add_executable(turtle_tf_listener src/turtle_tf_listener.cpp)
target_link_libraries(turtle_tf_listener ${catkin_LIBRARIES})

5.编译并运行（C++）

# 进入目录
cd ~/catkin_ws

# 编译
catkin_make

# 设置环境变量
# 之前设置过了环境变量的不用运行这句了
source devel/setup.bash

打开一个新的终端

# 启动ROS环境
roscore

打开第二个新的终端

rosrun turtlesim turtlesim_node

打开第三个新的终端

rosrun learning_tf turtle_tf_broadcaster __name:=turtle1_tf_broadcaster /turtle1

打开第四个新的终端

rosrun learning_tf turtle_tf_broadcaster __name:=turtle2_tf_broadcaster /turtle2

打开第五个新的终端

rosrun learning_tf turtle_tf_listener

打开第六个新的终端

rosrun turtlesim turtle_teleop_key

6.创建tf广播器与监听器代码（Python）

6.1 广播器例程

#!/usr/bin/env python
# -*- coding: utf-8 -*-

# 该例程将请求/show_person服务，服务数据类型learning_service::Person

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()

6.2 监听器例程

#!/usr/bin/env python
# -*- coding: utf-8 -*-

# 该例程将请求/show_person服务，服务数据类型learning_service::Person

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()

7.运行广播器与监听器（Python）

打开一个新的终端

# 启动ROS环境
roscore

打开第二个新的终端

rosrun turtlesim turtlesim_node

打开第三个新的终端

rosrun learning_tf turtle_tf_broadcaster.py __name:=turtle1_tf_broadcaster /turtle1

打开第四个新的终端

rosrun learning_tf turtle_tf_broadcaster.py __name:=turtle2_tf_broadcaster /turtle2

打开第五个新的终端

rosrun learning_tf turtle_tf_listener.py

打开第六个新的终端

rosrun turtlesim turtle_teleop_key

运行失败

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

1.Launch文件

通过XML文件实现多节点的配置和启动

2.Launch文件语法

2.1 根元素标签

例程

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

2.2 启动节点

启动节点格式为

<node pkg="功能包名" name="该节点在ros中运行时的名字" type="可执行文件名" />

• pkg：节点所在功能包名称
• type：节点的可执行文件名称
• name：节点运行时的名称
• output：是否打印日志信息
• respawn：启动失败后是否重启
• required：某个节点是否必须启动
• ns：node的命名空间
• ards：给节点输入参数

2.3 设置参数

设置ROS系统运行中的参数，存储在参数服务器中。

<param name="output_frame" value="odom" />

• name：参数名
• value：参数值

2.4 设置多个参数

加载参数文件中的多个参数

<rosparam file="params.yaml" command="load" ns="params" />

2.5 Launch文件内部的局部变量

<arg name="arg-name" default="arg-value" />

调用方式

<param name="foo" value="$(arg arg-name)">
<node name="node" pkg="package" type="type" args="$(arg arg-name)" \>

2.6 重映射

重新映射ROS计算图资源的命名

<remap from="/turtlebot/cmd_vel" to="/cmd_vel" />

• from：原名
• to：新名

2.7 嵌套

包含其他Launch文件，类似C语言中的头文件

<include file="$(dirname)/other.launch" />

2.8 更多

更多标签可参考

http://wiki.ros.org/roslaunch/XML

3.Launch示例

3.1 simple

例程

<launch>
    <node pkg="learning_topic" type="person_subscriber" name="talker" output="screen" />
    <node pkg="learning_topic" type="person_publisher" name="listener" output="screen" /> 
launch>

运行

roslaunch learning_launch simple.launch

3.2 turtlesim_parameter_config

例程

<launch>

	<param name="/turtle_number"   value="2"/>

    <node pkg="turtlesim" type="turtlesim_node" name="turtlesim_node">
		<param name="turtle_name1"   value="Tom"/>
		<param name="turtle_name2"   value="Jerry"/>

		<rosparam file="$(find learning_launch)/config/param.yaml" command="load"/>
	node>

    <node pkg="turtlesim" type="turtle_teleop_key" name="turtle_teleop_key" output="screen"/>

launch>

运行

roslaunch learning_launch turtlesim_parameter_config.launch

3.3 start_tf_demo_c++

例程

 <launch>

    
    <node pkg="turtlesim" type="turtlesim_node" name="sim"/>
    <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>

    <node pkg="learning_tf" type="turtle_tf_broadcaster" args="/turtle1" name="turtle1_tf_broadcaster" />
    <node pkg="learning_tf" type="turtle_tf_broadcaster" args="/turtle2" name="turtle2_tf_broadcaster" />

    <node pkg="learning_tf" type="turtle_tf_listener" name="listener" />

  launch>

运行

roslaunch learning_launch start_tf_demo_c++.launch

3.4 start_tf_demo_py

例程

<launch>

	
	<node pkg="turtlesim" type="turtlesim_node" name="sim"/>
	<node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>

	<node name="turtle1_tf_broadcaster" pkg="learning_tf" type="turtle_tf_broadcaster.py">
	  <param name="turtle" type="string" value="turtle1" />
	node>
	<node name="turtle2_tf_broadcaster" pkg="learning_tf" type="turtle_tf_broadcaster.py">
	  <param name="turtle" type="string" value="turtle2" /> 
	node>

    <node pkg="learning_tf" type="turtle_tf_listener.py" name="listener" />

launch>

运行

roslaunch learning_launch start_tf_demo_py.launch

3.5 turtlesim_remap

例程

<launch>

	<include file="$(find learning_launch)/launch/simple.launch" />

    <node pkg="turtlesim" type="turtlesim_node" name="turtlesim_node">
		<remap from="/turtle1/cmd_vel" to="/cmd_vel"/>
	node>

launch>

运行

roslaunch learning_launch turtlesim_remap.launch

十五、常见可视化工具的使用

1.Qt工具箱

1.1 rqt_graph 计算图可视化工具

1.2 rqt_console 日志输出工具

1.3 rqt_plot 数据绘图工具

1.4 rqt_image_view 图像渲染工具

1.5 rqt 综合工具

2.Rviz 三维可视化工具

3.Gazebo 三维物理仿真平台

# 这几个打开是空的
roslaunch gazebo_ros elevator_world.launch
roslaunch gazebo_ros empty_world.launch

# 这几个比较小，可以打开试试
roslaunch gazebo_ros rubble_world.launch
roslaunch gazebo_ros shapes_world.launch

# 这几个比较大，电脑不好打开容易卡死
roslaunch gazebo_ros mud_world.launch
roslaunch gazebo_ros willowgarage_world.launch
roslaunch gazebo_ros range_world.launch   

十六、总结与展望

目前的学习告一段落，本阶段的学习主要集中在ROS的基础知识上，后续的学习规划大致路线为：

• 手写URDF模型
• SolidWorks模型的URDF导出
• movelt仿真
• 自定义运动规划算法（可选）
• movelt编程练习
• ROS + SLAM
• ROS + 视觉
• ROS机器人自主导航
• Gazebo仿真

十七、其他感觉有用的东西

1.Vim

vim是Linux下常用的文编编辑器，使用方法可以参考这里。

https://www.runoob.com/linux/linux-vim.html