【古月据】ROS入门21讲

参考资料

链接：

http://wiki.ros.org/cn/ROS/Installation
https://www.bilibili.com/video/BV1zt411G7Vn?p=10

安装ROS

按照链接http://wiki.ros.org/cn/ROS/Installation进行安装ROS，连接打开的界面如下图：

选择合适的版本进行安装，我的系统是Ubuntu20.04，所以选择ROS Noetic Ninjemys版本进行安装。

8.ROS命令行工具的使用

8.0 常用命令：

• rostopic
• rosservice
• rosnode
• rosparam
• rosmsg
• rossrv

8.1 启动ROS Master

在桌面打开终端（Ctrl+Alt+T），执行指令：$ roscore

8.2 启动小海龟仿真器

在桌面再打开一个新的终端，执行指令：$ rosrun turtlesim turtlesim_node
执行指令后会打开海龟的界面：

8.3 启动海龟控制节点

在桌面再打开一个新的终端，执行指令：$ rosrun turtlesim turtle_teleop_key
此时移动鼠标上下左右键，就可以控制海龟的移动，注意，一定要确认鼠标在该终端中，将鼠标点到海龟界面时，可能操作无效，小海龟就不会移动。

8.4 显示系统计算图

在桌面再打开一个新的终端，执行指令：$ rqt_graph
执行指令后会打开一个计算图的界面：

从界面中可以看出，两个椭圆形表示两个节点，/teleop_turtle是键盘控制节点，/turtlesim是仿真器节点，话题/turtle1/cmd_vel为两个节点通信。

8.5一些常用的没有界面的命令行工具

8.5.1rosnode

它是用来显示系统中所有节点相关信息的指令。
在终端执行指令$ rosnode，如下图：
继续执行$ rosnode list（这个指令是列出系统中所有的节点），如下图：

这里可以看到有三个节点，但是/rosout在之前的计算图（小标题4中显示的计算图）中是没有体现的，一般不用去关心它，只要启动roscore就会启动。
rosnode info查看一个节点的具体信息，如执行指令：$ rosnode info /turtlesim

可以看出该节点在发布哪些话题，在订阅哪些话题等等。

8.5.2 rostopic

它是一个与话题相关的命令行工具
同样在终端执行$ rostopic，如下图：

在终端继续执行$ rostopic list，如下图：

我们从图中看到有许多话题名，其中键盘控制节点与海龟仿真器的节就是通过话题/turtle1/cmd_vel来建立通信的。也可以通过指令来对话题/turtle1/cmd_vel发送指令，控制海龟运动。如下：
现在海龟是在这个位置：

我们执行在终端执行指令（使用Tab对代码进行补全，降低敲代码的工作量）：

$ rostopic pub /turtle1/cmd_vel geometry_msgs/Twist "linear:
  x: 2.0
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 0.0" 
  #平移的单位是米每秒，角度是弧度每秒。
  #执行这条指令，只给话题发送一次消息，海龟只会移动一次，也就是向前移动两米。

现在海龟是在这个位置：

下面这条指令，会每秒发送10次消息，以1m/s前进。

$ rostopic put -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: 0.0" 

执行这条指令后，小海龟会一直前进，如下图：
到达边界后还会移动，直到达到角落。

8.5.3 rosmsg

用来查看消息的数据结构
在终端执行$ rosmsg show geometry_msgs/Twist

8.5.4 rosservice

它是服务相关的指令
在终端执行指令$ rosservice list

再产生一只新的海龟，在终端执行指令:

$ rosservice call /spawn "x: 2.0
y: 2.0
theta: 0.0
name: 'turtle2'" 

从下图可以看出，执行了上面这条指令，海龟产生成功之后，返回了name: "turtle2"告诉我们海龟产生成功了。

海龟产生结果如下图：
可以看到界面中同时存在两只小海龟。

在终端中执行$ rostopic list，如下图：
可以看到，turtle2/cmd_vel这个话题也有了，可以通过对该话题发送指令，控制第二只海龟移动。

8.6 话题记录与复现rosbag

8.6.1 话题记录

在终端执行$ rosbag record -a -O cmd_record（指令中的cmd_record表示记录的数据文件的文件名），如下图。此时可以操控键盘左右上下键，移动海龟（参照前面的内容），记录海龟移动数据，后面可以根据这些数据还原海龟的运动情况。最后按Ctrl+C，结束记录，数据会保存在路cmd_record.bag文件中，可以在终端执行$ pwd来查看当前路径,cmd_record.bag文件就在该路径下。

8.6.2 话题复现

现在关掉所有终端，然后启动ROS Master和启动小海龟仿真器（参照前面8.1和8.2的步骤），不用启动海龟控制节点。再打开一个终端执行指令rosbag play cmd_record.bag,我们就可以看到之前记录小海龟的运动数据就恢复出来了

9.创建工作空间与功能包

9.1 工作空间

工作空间(workspace)是一个存放工程开发相关文件的文件夹。
• src：代码空间(Source Space)，用来放置功能包。
• build：编译空间(Build Space)，用来放置在编译过程中所产生的中间文件，一般不用关心。
• devel：开发空间(Development Space)，用来放置在编译过程产生的可执行文件，一些库，脚本等。
• install：安装空间(Install Space)，放置编译过后的可执行文件等，与devel中的文件有部分相同。

9.2 创建工作空间

9.2.1 创建工作空间

$ mkdir -p ~/catkin_ws/src #这里的src只能是src，但是catkin_ws可以改成其他名字
$ cd ~/catkin_ws/src
$ catkin_init_workspace

在终端依次执行以上三条指令，如下图：

从catkin_ws文件夹中打开终端，执行指令$ tree查看catkin_ws文件夹中的树形目录，如下图：

9.2.2 编译工作空间

执行指令$ cd ~/catkin_ws/切换到工作空间的根目录下，也就是catkin_ws这个目录，执行编译命令$ catkin_make（这是ROS中catkin这个编译工具所提供的一个编译器的指令，通过这个指令它可以根据配置来编译src下面所有功能包的源码，结果会放置在devel中与install中）
执行完了之后，会出现在catkin_ws中出现两个新的文件夹bulid和devel，src是之前创建的。如下图：

然后再执行$ catkin_make install就会出现一个install文件夹。如下图：

9.2.3 设置环境变量

$ source devel/setup.bash

只有设置了环境变量之后，系统才能找到这个环境空间，并且找到系统空间里面对应的一些功能包。

9.2.4 检查环境变量

$ echo $ROS_PACKAGE_PATH

结果如下图：

9.3 创建功能包

注意：功能包一定要在src下。

9.3.1 创建功能包

'''
指令：
catkin_creat_pkg是创建功能包的指令
后面的参数:
package_name是功能包的名字，
depend1，depend2，depend3是该功能包所依赖的其他功能包，可以不止三个。
'''
$ catkin_create_pkg <package_name> [depend1] [depend2] [depend3]

注意：
a.一定要在src下面创建功能包；
b.同一个工作空间下,不允许存在同名功能包；
c.不同工作空间下,允许存在同名功能包。

在终端依次执行下面两条指令：

$ cd ~/catkin_ws/src
$ catkin_create_pkg test_pkg std_msgs rospy roscpp

执行成功后，我们执行$ tree来查看src文件夹下的文件。可以看到出现了一个test_pkg文件夹，如下图：

其中test_pkg文件夹中的src文件夹是用来放置功能包的代码include文件夹是用来放置头文件，如C++中的.h文件等。CMakeLists.txt和package.xml是每一个功能包都必须存在的两个文件，这才标志着test_pkg是一个功能包，而不是一个普通的文件夹。
CMakeLists.txt文件如下（只截取了一部分）：

这个文件主要是用来描述功能包的编译规则，这里使用的语法是cmake，后面在使用过程中，会讲解如何设置。

package.xml文件如下:

它包括了功能包的名字，版本号，描述，开发者邮箱，开源许可证以及所依赖的包，依赖包也可以直接在package.xml文件中手动添加。

9.3.2 编译功能包

在终端依次执行下面三条指令：

$ cd ~/catkin_ws # 切换路径
$ catkin_make # 编译功能包
$ source ~/catkin_ws/devel/setup.bash # 设置环境变量

我们还可以执行指令$ echo $ROS_PACKAGE_PATH来查看环境变量。如下图：

10.发布者Publisher的编程实现

10.1 话题模型

ROS Master是节点管理器，Publisher节点需要使用代码编写，Subscriber我们直接调用turtlesim海龟仿真器，turte1/cmd_vel是话题总线，Message是消息，消息的数据结构是geometry_msgs::Twist。Publisher通过Topic这个话题总线发送消息Message，来控制Subscriber。

10.2 创建功能包

将9.3节创建的功能包删掉（因为之前是创建的一个空的功能包，没有什么用），也就是删除src文件夹下面的test_pkg文件夹。然后重新创建一个这节要用的新的功能包。依次在终端执行以下两条指令：

$ cd ~/catkin_ws/src
$ catkin_create_pkg learning_topic roscpp rospy std_msgs geometry_msgs turtlesim

10.3 创建发布者代码(C++和Python)

10.3.1 创建C++代码：

在功能包learing_topic中（也就是在learning_topic文件夹下面的src文件夹下面）建立一个.cpp文件，并编辑以下代码。
代码如下：

/**
 * 该例程将发布turtle1/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, "velocity_publisher");
	//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);
	//geometry_msgs::Twist是将要发布消息的类型
	//turtle1/cmd_vel是指定的、将要发布的话题名，向/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; //设置线速度为0.5m/s
		vel_msg.angular.z = 0.2; //设置角速度为0.2rad/s

	    // 发布消息
		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(); //10Hz
	}

	return 0;
}

10.3.1 创建python代码：

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 该例程将发布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

要执行python文件，要查看该文件的属性（鼠标右键该文件——属性——权限），确保该文件的权限是“允许执行文件”，如下图：

10.4 配置发布者代码编译规则

在CMakeLists.txt（注意，是功能包learning_topic文件夹下的CMakeLists.txt）中配置编译规则：

• 设置需要编译的代码和生成的可执行文件；
• 设置链接库；
  将下面两句语句放入CMakeLists.txt并保存，就完成了配置编译规则：

#使用C++的代码复制下面两句
add_executable(velocity_publisher src/velocity_publisher.cpp)
target_link_libraries(velocity_publisher ${catkin_LIBRARIES})
#使用Python的代码改为下面两句,相当于之修改了编译文件的后缀。
add_executable(velocity_publisher scripts/velocity_publisher.py)
target_link_libraries(velocity_publisher ${catkin_LIBRARIES})
#### 目前编译.py文件出现了问题，正在解决...

放入位置如下图：

10.5 编译并运行发布者

依次执行以下指令，编译并运行发布者

$ cd ~/catkin_ws
$ catkin_make
$ source devel/setup.bash
$ roscore
$ rosrun turtlesim turtlesim_node
$ rosrun learning_topic velocity_publisher

11 订阅者Subscriber的编程实现

与10节的内容极为相似，可参看10节的内容。

11.1 话题模型

11.2 创建订阅者代码(C++和Python)

11.2.1 创建C++代码：

/**
 * 该例程将订阅/turtle1/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;

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

    // 循环等待回调函数
    ros::spin(); //死循环，不会执行到下一句的return 0

    return 0;
}

11.2.2 创建Python代码：

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 该例程将订阅/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()

11.3 配置订阅者代码编译规则

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

11.4 编译并运行订阅者

$ cd ~/catkin_ws
$ catkin_make
$ source devel/setup.bash
$ roscore
$ rosrun turtlesim turtlesim_node
$ rosrun learning_topic pose_subscriber

11.4.1 给小海龟发布指令方式1

执行了指令之后，可以在终端中看到一直在刷新小海龟的位置，但是都没有改变，我们可以通过键盘控制小海龟移动，执行指令$ rosrun turtlesim turtle_teleop_key,按键盘上下左右键来移动小海龟，同时也可以在终端看到小海龟的位置变化。

11.4.1 给小海龟发布指令方式2

执行第10节编译好的可执行文件，执行指令：$ rosrun learning_topic velocity_publisher，用程序向小海龟发布移动指令，让小海龟动起来，同时可以观察到小海龟在作圆周运动，它的位置信息也在变化。如下图：

12 话题消息的定义与使用

12.1 话题模型

12.2 自定义话题消息

Step1:
在功能包里面创建一个文件夹msg，然后在msg文件夹中打开终端，执行指令$ touch Person.msg创建一个.msg文件，然后编辑以下内容：

string name
uint8 sex
uint8 age

uint8 unknown = 0
uint8 male = 1
uint8 female = 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)
  
• 编译生成语言相关文件
  依次在终端执行以下两条指令：

 $ cd /home/aolei/catkin_ws/
 $ catkin_make

12.3 创建发布者和订阅者代码（C++ 和Python）

12.3.1 创建发布者和订阅者代码（C++）

发布者代码（C++）：

/**
 * 该例程将发布/person_info话题，自定义消息类型learning_topic::Person
 */
 
#include <ros/ros.h>
#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;
}

订阅者代码（Python）：

/**
 * 该例程将订阅/person_info话题，自定义消息类型learning_topic::Person
 */
 
#include <ros/ros.h>
#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;
}

12.3.2 创建发布者和订阅者代码（Python）

发布者代码(C++)：

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 该例程将发布/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

订阅者代码(Python)：

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 该例程将订阅/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()

12.4 配置代码编译规则

如何配置CMakeLists.txt中的编译规则

• 设置需要编译的代码和生成的可执行文件;
• 设置链接库;
• 添加依赖项。
  在CMakeLists.txt中添加以下6条语句：

add_executable(person_publisher src/person_publisher.cpp)
target_link_libraries(person_publisher ${catkin_LIBRARIES})
add_dependencies(person_publisher ${PROJECT_NAME}_generate_messages_cpp)

add_executable(person_subscriber src/person_subscriber.cpp)
target_link_libraries(person_subscriber ${catkin_LIBRARIES})
add_dependencies(person_subscriber ${PROJECT_NAME}_generate_messages_cpp)

如下图：

12.5 编译并运行发布者和订阅者

在终端依次执行以下指令：

$ cd ~/catkin_ws
$ catkin_make
$ source devel/setup.bash
$ roscore
$ rosrun learning_topic person_subscriber
$ rosrun learning_topic person_publisher

结果如下：

13 客户端Client的编程实现

13.1 话题模型

13.2 创建功能包

在终端依次执行以下两条指令：

$ cd ~/catkin_ws/src
$ catkin_create_pkg learning_service roscpp rospy std_msgs geometry_msgs turtlesim

13.3 创建客户端代码（C++和Python）

13.3.1 客户端代码（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>("/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;
};

13.3.2 客户端代码（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服务后，创建一个服务客户端，连接名为/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())

13.4 配置客户端代码编译规则

如何配置CMakeLists.txt中的编译规则（与前面一样）：

• 设置需要编译的代码和生成的可执行文件;
• 设置链接库;
  在CMakeLists.txt中添加下面两条语句：

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

如下图：

13.5 编译并运行客户端

依次执行以下指令：

$ cd ~/catkin_ws
$ catkin_make
$ source devel/setup.bash
$ roscore
$ rosrun turtlesim turtlesim_node
$ rosrun learning_service turtle_spawn

结果如下：

产生了第二只小海龟，在终端中也打印了两条信息。

14 服务端Server的编程实现

14.1 服务模型

14.2 创建服务器代码

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

14.2.2 Python代码

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 该例程将执行/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()

14.3 配置服务器代码编译规则

如何配置CMakeLists.txt中的编译规则

• 设置需要编译的代码和生成的可执行文件;
• 设置链接库;
  在CMakeLists.txt中添加下面两条语句：

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

如下图：

14.4 编译并运行服务器

在终端依次执行以下指令：

$ cd ~/catkin_ws
$ catkin_make
$ source devel/setup.bash
$ roscore
$ rosrun turtlesim turtlesim_node
$ rosrun learning_service turtle_command_server
$ rosservice call /turtle_command "{}"

如下图：

15 服务数据的定义与使用

15.1 服务模型

15.2 自定义服务数据

与12.2节相似
Step1:
在功能包里面创建一个文件夹srv，然后在srv文件夹中打开终端，执行指令$ touch Person.srv创建一个.srv文件，然后编辑以下内容：

string name
uint8 age
uint8 sex

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

注意：—以上部分是Request的内容，—以下部分是Response的内容
Step2:
在package.xml中添加功能包依赖

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

如下图：

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

• find_package( … message_generation)
  

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

• catkin_package(… message_runtime)
  

• 编译生成语言相关文件
  依次在终端执行以下两条指令：

 $ cd /home/aolei/catkin_ws/
 $ catkin_make

15.3 创建服务器/客户端代码（C++和Python）

15.3.1 创建服务器/客户端代码（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;
}

创建客户端代码

/**
 * 该例程将请求/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;
};

15.3.1 创建服务器/客户端代码（Python）

创建服务器代码

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 该例程将执行/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()

创建客户端代码

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 该例程将请求/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())

15.4 配置服务器/客户端代码编译规则

如何配置CMakeLists.txt中的编译规则：

• 设置需要编译的代码和生成的可执行文件;
• 设置链接库;
• 添加依赖项。
  在CMakeLists.txt中添加以下6条语句：

add_executable(person_server src/person_server.cpp)
target_link_libraries(person_server ${catkin_LIBRARIES})
add_dependencies(person_server ${PROJECT_NAME}_gencpp)
add_executable(person_client src/person_client.cpp)
target_link_libraries(person_client ${catkin_LIBRARIES})
add_dependencies(person_client ${PROJECT_NAME}_gencpp)

15.5 编译并运行客户端和服务端

在终端依次执行以下指令：

$ cd ~/catkin_ws
$ catkin_make
$ source devel/setup.bash
$ roscore
$ rosrun learning_service person_server
$ rosrun learning_service person_client

结果如下图：

16 参数的使用与编程方法

16.1 参数模型

参考链接：http://wiki.ros.org/Parameter%20Server

16.2 创建功能包

与之前的一样，在终端依次执行以下两条指令：

$ cd ~/catkin_ws/src
$ catkin_create_pkg learning_parameter roscpp rospy std_srvs

16.3 参数命令行使用

如下图：
可以将文件保存为.yaml格式的文件。

16.4 编程方法（C++和Python）

16.4.1 编程方法（C++）

/**
 * 该例程设置/读取海龟例程中的参数
 */
#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;
}

16.4.2 编程方法（Python）

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 该例程设置/读取海龟例程中的参数

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

16.5 配置代码编译规则

如何配置CMakeLists.txt中的编译规则：

• • 设置需要编译的代码和生成的可执行文件;
• • 设置链接库;
  在CMakeLists.txt中添加以下2条语句：

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

16.6 编译并运行发布者

$ cd ~/catkin_ws
$ catkin_make
$ source devel/setup.bash
$ roscore
$ rosrun turtlesim turtlesim_node
$ rosrun learning_parameter parameter_config

如下图：
问题：
终端中显示，颜色也更改了从[0,0,0,]变为[255,255,255]，但是小海龟界面的背景颜色没有随之发生改变。

17 ROS中的坐标系管理系统

17.1 机器人中的坐标变换

可以参考下面的书籍：

该书的第3讲有较为详细的推导与说明。

17.2 机器人中的坐标变换

17.3 机器人中的坐标变换（图解）

17.4 机器人中的坐标变换（演示）

17.4.1

• 在终端执行指令$ sudo apt-get install ros-noetic-turtle-tf来安装ros-noetic-turtle-tf库
• 在终端中依次执行以下两条指令：

$ roslaunch turtle_tf turtle_tf_demo.launch
$ rosrun turtlesim turtle_teleop_key

如下图：

使用键盘控制节点来控制第一只小海龟移动，第二只小海龟会跟随，并且第二只小海龟会挑选较近的路线跟随，不会傻乎乎的沿着第一只小海龟运动的路径走。

• 执行指令$ rosrun tf view_frames来查看功能包中所以坐标系之间的关系：
  执行指令$ rosrun tf view_frames出现的错误及解决办法：
  错误提示1：
  
  解决办法：
  参考链接：https://blog.csdn.net/shiye527/article/details/107800764
  根据提示，我们需要将m = r.search(vstr)修改为m = r.search(vstr.decode('utf-8'))
  在终端中依次执行以下指令与操作：

# 参考链接
$ cd /opt/ros/noetic/lib/tf
$ sudo chmod a+w view_frames
$ vim view_frames
输入大写的i对文件进行编辑:
将m = r.search(vstr)修改为m = r.search(vstr.decode('utf-8')) 
修改之后：按Esc退出，输入     :wq！回车保存

修改过后，在终端中在运行$ rosrun tf view_frames，就会在目录下得到一个.pdf文件和一个.gv文件。

打开frames.pdf文件如下图：

错误提示2：

可能原因：在执行过程中，Ubuntu自带的python2.7中没有yaml库，安装的时候安装到了python3中，后来我把python2.7卸载了还是不行，又把python2.7安装上了，结果后面百度发现了以下办法。
解决办法：
在终端执行指令：

$ sudo update-alternatives --install /usr/bin/python python /usr/bin/python3 1000

17.4.2

执行指令：$ rosrun tf tf_echo turtle1 turtle2
用键盘控制节点控制小海龟移动，可以看到终端中会显示小海龟的位置信息（平移量和旋转量）如下图：

17.4.3

执行指令：$ rosrun rviz rviz -drospack find turtle_tf/rviz/turtle_rviz.rviz
会打开下面的界面，通过控制节点移动小海龟，可以看到两只小海龟在界面中的位置。