Ros1入门到入土

Ros1从入门到入土

文章总结与古月居ROS入门21讲，对哪一章有问题可以去看原片。

【古月居】古月·ROS入门21讲 | 一学就会的ROS机器人入门教程_哔哩哔哩_bilibili

1.Ros环境配置

（1） 安装VMware虚拟机并配置Ubuntu虚拟环境，并且安装上C++和Python的编程环境

略…（帖子太多，自己去csdn）

（2） 在Ubuntu上安装Ros（本文使用的是Ubuntu18.04）

1）基础环境配置

这四个如果没有选择务必选上

2）换源

Ros有自己单独的一套源，需要添加到原本的源中，命令行输入

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

3 ) 输入密钥

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

4) 更新源

sudo apt-get update
sudo apt-get upgrade

5 ) 安装ros

sudo apt-get update
sudo apt-get install ros-melodic-desktop-full

6 ) 安装rq4t工具箱：

sudo apt-get install ros-melodic-rqt*

7 ) 初始化rosdepc

sudo apt-get install python-pip
sudo pip install rosdepc
sudo rosdepc init
rosdepc update

8 ) 设置环境变量

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

9 ) 安装rosinstall

sudo apt-get install python-rosdep python-rosinstall python-rosinstall-generator python-wstool build-essential

10) 验证ros是否安装成功

创建一个终端输入（启动ros服务）

roscore

这个终端别动了，再创建一个新终端（启动海龟仿真器）【ps：创建新终端快捷键 ==> Ctrl + Alt + t】
新终端2：

rosrun turtlesim turtlesim_node

创建第三个终端，启动海龟控制节点
新终端3：

rosrun turtlesim turtle_teleop_key

新界面有小乌龟，通过上下左右方向键控制。【ps：上下左右的方向输入时，需要在终端3的窗口中输入，而不是小海龟的窗口中输入】

2.Ros简介

ros是什么

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

目的是为了提高机器人研发中的软件复用率（复用别人开发好的东西，然后更改）

通信机制：

Ros提供了一个松耦合分布式通信框架

Ros中所有的框架可以抽象成一个节点图，每一个具体功能都是一个椭圆节点，节点间通过箭头做连接，箭头代表了节点间数据的流向以及数据。所有节点和箭头一起构成了一副计算图。

开发工具

ros提供了很多开发工具，命令行工具，可视化工具，仿真工具

应用功能

ros的应用功能非常多，ros社区里面有很多围绕着ros协议标准开发的功能包，对于这些功能包我们只需要关心他们接口的输入输出，至于核心算法我们其次才回去看看。最终我们实现功能就像搭积木一样把这些功能包拼起来。

生态系统

1.发行版(Distribution) ： ROS发行版包括一系列带有版本号、可以直接安装的功能包。
2.软件源(Repository) ： ROS依赖于共享网络上的开源代码，不同的组织机构可以开发或者共享自己的机器人软件。
3.ROS wiki：记录ROS信息文档的主要论坛。
4.邮件列表(Mailing list)：交流ROS更新的主要渠道，同时也可以交流ROS开发的各种疑问。
5.ROS Answers：咨询ROS相关问题的网站。
6.博客(Blog)：发布ROS社区中的新闻、图片、视频(http://www.ros.org/news)

3.Ros的核心概念

（1）节点——执行单元

节点是执行具体任务的进程、独立运行的可执行文件

不同节点可使用不同的编程语言，可分布式运行在不同的主机

节点在系统中的名称必须是唯一的

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

为节点提供命名和注册服务

跟踪和记录话题/服务通信，辅助节点相互查找、建立连接

提供参数服务器（全局的对象字典，记录全局变量名和变量值），节点使用此服务器存储和检索运行时的参数

Ros给节点间的通信设置了两种通信方式——话题的服务

（3）话题（Topic）——异步通信集资

话题通讯机制是单向数据传输

话题是节点间用来传输数据的重要总线

使用发布/订阅模型，数据有发布者传输到订阅者，同一个话题的发布者和订阅者可以不唯一

（4）消息——话题数据

具有一定的类型和数据结构，包括ROS提供的标准类型和用户自定义类型

使用编程语言无关的“ .msg ”文件定于，编译过程中生成对应的代码文件

话题是数据传输的通道，消息是描述我们传输信息时的信息数据类型。

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

使用客户端/服务端（C/S）模型，客户端发送请求数据，服务器完成处理后返回应答数据

使用编程语言无关的“ .srv ” 文件定义请求和应答数据结构，编译过程中生成对应的代码文件。

话题通信和服务通信的区别

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

（6）参数——全局共享字典

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

（7）功能包(Package)

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

（8）功能包清单(Package manifest)

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

（9）元功能包(Meta Packages)

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

4.Ros命令行工具的使用

常用命令

​ rostopic

​ rosservice

​ rosnode

​ rosparam

​ rosmsg

​ rossrv

先命令行输入roscore启动ros-master，也就是节点管理器

使用rosrun运行某个功能包的某个节点，比如调用turtlesim的turtlesim_node节点（ps：rosrun 包名 之后输入两个Tap。可以查看这个包所有的节点）

rosrun turtlesim turtlesim_node

（1） rqt_graph工具

ros中有很多rqt开头的工具，都是有界面的命令行工具，rqt_graph会显示当前系统计算图的工具，ros通信机制的核心就是有箭头和节点组成的计算图。

命令行输入rqt_graph即可查看系统计算图，帮助我们快速了解系统，回车之后会输出计算图

（2） rosnode list

列出所有系统的节点

（3） rosnode info /节点名称

查看当前节点的全部信息

（4） rostopic list

打印当前系统的所有话题列表

（5） rostopic pub

发布数据给某一个topic，输入rostopic pub 话题名 后，按两下Tap可以显示当前数据的数据结构，再按两下Tap可以显示当前数据的数据内容，我们只需要改变数据参数即可。

回车之后海龟动了，但是就动了一下

所以我们需要在pub后面加参数， -r 10表示已10赫兹每秒动，一直动，撞墙也继续动

（6） rosmsg show 消息名

显示某个消息的数据结构

（7）rosservice list

查看当前仿真器里面所有的服务内容

（8）rosservice call + 服务名

前面显示服务里面的spawn就是产生一直海龟，如果我们需要再产生一直海龟该怎么操作呢。

可以 rosservice call /spawn然后两下Tap，会出现这个服务初始化信息

（9）话题记录工具

可以记录当前系统内部的所有话题数据并且保存下来

话题记录：

rosbug record -a -0 cmd_record			
// cmd_record为保存文件名

我们输入这个指令当显示这几行就说明开始记录了，然后我们去终端用上下左右操作海龟运行一会儿。

运行完毕直接回到这个终端，Ctrl + C结束，然后就会把刚才的运行话题记录成一个.bag文件存放带默认文件夹，某认文件夹为主目录

话题复现：

我们启动roscore和海龟节点，不启动海龟运动节点。

然后新建终端运行如下命令

rosbag play cmd_record.bag

我并不需要移动海龟，海龟就会自己移动。

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

工作空间（workspace）是一个存放工程开发相关文件的文件夹

​ src：代码空间（Source Space）【放置功能包的代码，功能包的配置文件等】

​ build：编译空间（Build Space）【放置编译过程中所产生的的中间文件，这个文件夹可以不用管】

​ devel：开发空间（Development Space）【放置编译生成的可执行文件，库以及一些脚本等】

​ install：安装空间（Install Space）【用install安装的东西放在这个空间，也可以不用管】

（1）创建工作空间

创建工作空间

mkdir -p ~/addam_first/src		// addam_first是文件夹名字，随便怎么取,创建文件夹以及文件夹的子文件夹src
cd ~/addam_first/src		// 进入这个src文件夹
catkin_init_wordspace		// 初始化生成一个.txt文件，把这个文件夹初始化变成一个工作空间

编译工作空间

cd ~/admin_first/		//进入项目根目录			
catkin_make			// 产生build和devel
catkin_make install //产生install安装空间

设置环境变量

source devel/setup.bash

检查环境变量

echo $ROS_PACKAGE_PATH

（2） 创建功能包

功能包是放置Ros源码的最小单元，所以所有源码必须放在功能包里面。

我们需要先创建一个功能包

功能包的放置一定要放置到src中。test_pkg是我们的包名，后面std_msgs rospy roscpp使我们这个功能包需要依赖的一些库，std_msgs是一些基本的数据类型，rospy是ros的Python依赖，roscpp是c++依赖。【PS：同一个工作空间下，不允许存在相同的功能包名，不同的工作空间下允许存在相同的功能包名】

cd ~/admin_first/src
catkin_create_pkg test_pkg std_msgs rospy roscpp

编译功能包

cd ~/admin_first
catkin_make
source ~/admin_first/devel/setuo.bash

功能包结构如下：

package.xml描述了这个功能包的一些基本信息，比如名字，版本号等。

下面还有这个功能包的依赖信息

CMakeList.txt文件设置了功能包代码的编译规则

6：发布者Publisher的编程实现

实现程序发布指令使得海龟动起来

C++创建Publisher过程如下

第一步：创建功能包

cd ~/addam/src/
catkin_create_pkg learning_topic rospy roscpp std_msgs geometry_msgs turtlesim

第二步：C++实现一个发布者（四步）

初始化ROS节点

向ROS Master 注册节点信息，包括发布的话题名和话题中的消息类型

创建消息数据

按照一定频率循环发布消息。

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
int main(int argc, char **argv)
{
	// ROS节点初始化 [velocity_publisher为节点名字]
	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;
}

第三步：配置CMakeList.txt中的编译规则

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

设置链接库

# 描述把src/velocity_publisher.cpp和可执行文件velocity_publisher建立连接
add_executable(velocity_publisher src/velocity_publisher.cpp)
# 把可执行文件和ROS的库去做连接
target_link_libraries(velocity_publisher ${catkin_LIBRARIES})

第四步: 编译并运行发布者

cd ~/addam_first
catkin_make
source devel/setup.bash
roscore

Ctrl+Alt+T

rosrun tue=rtlesim turtlesim_node

Ctrl+Alt+T

source devel/setup.bash
rosrun learning_topic velocity_publisher

每一次运行都需要使用“source devel/setup.bash”重新编译，非常容易忘而且非常麻烦，我们在系统根目录下找到“ .bashrc ”，点开之后在最后加上这样一条命令

source /home/系统名字/项目名/devel/setup.bash
// 比如我的就是source /home/addam/addam_first/devel/setup.bash

之后就直接运行就好啦。

Python创建Publisher过程如下

第一步：为了区分python和c++代码，我们在原功能包上创建一个文件夹，随便叫什么，就叫scripts。

第二步：python实现一个发布者

和C++一样都是四步，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

还有就是这个权限一定要放开，右击属性，打钩允许作为程序执行文件

7：订阅者Subscriber的编程实现

实现通过海龟仿真器去发布数据，实现一个订阅者去订阅海龟的位置信息（Post信息）。由ROS Master管理节点，Publisher就是海龟仿真器，Subscriber就是需要实现的程序。两者之间传输的Message消息就是海龟的post信息，话题内容分就是/turtle1/pose，数据传输是从海龟仿真器传输到需要实现的程序。

7.1： C++实现一个订阅者

（1）初始化ROS节点

（2）订阅需要的话题

（3）循环等待话题消息，接收到消息后进入回调函数

（4）在回调函数中完成消息处理【ps：回调函数如果执行时间非常长就会一直卡在这里，回调函数是不会嵌套的，所以我们需要保证回调函数处理是高效，时间不能太长】

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，设置长度为10的缓冲区队列（和之前的缓冲区一样，如果缓冲区内元素超过10，就会抛弃时间戳最老的数据，保证队列里的数据是最新的数据），注册回调函数poseCallback（订阅者不知道发布者什么时候会发布消息，所以注册回调函数就是当订阅者订阅到消息，就会立刻调用回调函数）
    ros::Subscriber pose_sub = n.subscribe("/turtle1/pose", 10, poseCallback);
    // 循环等待回调函数
    ros::spin();
    return 0;
}

配置CMakeList.txt中的编译规则

和之前发布者一样的

# 描述把src/pose_subscriber.cpp和可执行文件pose_subscriber建立连接
add_executable(pose_subscriber src/pose_subscriber.cpp)
# 把可执行文件和ROS的库去做连接
target_link_libraries(pose_subscriber ${catkin_LIBRARIES})

编译并运行发布者

编译是一样的

cd ~/addam_first
catkin_make

运行试一下，先运行roscore，在运行海龟仿真器=

roscore

Ctrl+Alt+T

rosrun turtlesim turtlesim_node

Ctrl+Alt+T

rosrun learning_topic post_subscriber

由于海龟没动，所以现在一直是x：5，y：5。我们运行海龟控制器改变一下海龟位置

rosrun turtlesim turtle_teleop_key

7.2： python实现订阅者

流程和C++是一样的，就是实现方式略有偏差

代码如下

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

python代码无需设置编译规则，改一下权限之后可以直接运行，效果和C++的订阅者是一样的。

8： 话题消息的定义与使用

之前我们都是使用ROS给我们定义好的Twist（发布）和Post（订阅）方法。但是有的时候开发过程中ROS定义好的消息无法满足我们的需求，这个时候我们就需要自己去定义消息类型。

我们可以自己定义消息类别，然后传输一个Person的个人信息。

– 通过语言无关的文件定义一个Topic（话题），话题名叫“ person_info ”

– Publisher（发布者）会通过这个话题发布这个人的信息

– Subscriber（订阅者）会通过这个话题订阅这个人的信息。

8.1：自定义话题消息

（1） 定义一个msg文件

关于一个人有如下定义：

string name		//这个人的名字
uint8 sex		//这个人的性别
uint8 age		//这个人的年纪
// 然后我们对这个人的性别进行一些宏定义，做一些限制
uint8 unknown = 0		//为知性别为0
uint8 male = 1			//男性为1
uint8 female = 2		//女性为2

所有的person定义我们需要放到一个“ .msg ”的文件中去，这个文件是一个跟语言无关的文件。string uint8都是表示我们需要在对应程序里扩展成该种程序的表示方法。

所以我们具体实施如下：

1） 在组件目录下创建一个文件夹名为“msg”，把跟消息相关的定义都放在该文件夹里面。

2）在msg文件夹下右键，新建终端，打开该文件夹下的终端。然后输入以下代码创建一个“ .msg ”文件

touch Person.msg

3）双击打开这个Person.msg文件，输入以下内容然后保存（每一句的解释见上面）

string name
uint8 sex
uint8 age

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

（2） 添加功能包依赖及编译选项

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

<!-- 编译依赖 -->
<build_depend>message_generation</build_depend>
<!-- 执行依赖 -->
<exec_depend>message_runtime</exec_depend>

然后在CMakeList.txt添加编译选项

find_package(
	......
	message_generation
)

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

catkin_package(
	......
	message_runtime
)

现在我们就完成了msg文件的生成，可以回到工作空间的根目录下试一下编译catkin_make，只要底下的输出没出现红色，出现什么颜色都是好颜色。编译完成之后就会生成一个C++的头文件。

8.2：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 = "Addam";
		person_msg.age  = 21;
		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;
}

还是一样的流程，先初始化，创建节点句柄，创建一个Publisher，设置循环频率

初始化一个learning_topic::Person类型的消息，消息主要就是名字，年龄，和调用刚才宏定义生成的性别。

然后发布消息并添加延时，这个发布者会不停发布这个个人信息。

订阅者的创建

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

先初始化节点，创建节点句柄，创建一个Subscriber，订阅名为/person_info的主题，并且注册回调函数

回到函数就会把这个人的信息打印出来。

订阅者发布者都需要先引入这个头文件，而且这个发布和订阅的话题也是我们自己定义的

#include "learning_topic/Person.h"

接下来编译我们的发布者订阅者

配置CMakeList.txt中的编译规则

1）设置需要编译的代码和生成的可执行文件

2）设置链接库

3）添加依赖项

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)

编译是一样的

cd ~/addam_first
catkin_make

运行试一下

roscore

Ctrl+Alt+T

rosrun learning_topic person_subscriber

订阅者不会接收到数据，因为发布者还没发布数据，订阅者只能等待。

Ctrl+Alt+T

rosrun learning_topic person_publisher

开始有数据了。

8.3：Python实现发布者和订阅者创建

发布者

# 该例程将发布/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

订阅者

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

python代码最舒服的就是不需要编译

运行起来效果是一样的

9： 客户端Clien的编程实现

之前讲的都是话题通信的发布/订阅模型，但是ROS还有另一个通信方式就是Service服务的通信方式

Service服务由客户端（Clien）和服务端（server）组成

实现通过程序发布请求，使得客户端能够让海龟仿真器产生一直新的海龟。

Client客户端就是我们即将要实现的程序，会发布一个产生海龟的Request请求并发给Server端，Server端就是海龟仿真器会产生一个海龟并且回馈一个Response给Client客户端，这样客户端知道海龟有没有生成成功。Service的消息结构叫做turtlesim::Spawn，这个是turtlesim这个功能包自定义的一个消息结构。

创建一个功能包

本功能包对标learning_topic，取名为learning_service

cd ~/addam_first/src
catkin_create_pkg learning_service std_msgs rospy roscpp geometry_msgs turtlesim

C++创建客户端

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这个服务，如果没有就一直等着，只有这个服务在系统当中存在了，才能去请求这个服务
	ros::service::waitForService("/spawn");
    // 发现/spawn服务后，创建一个服务客户端，连接名为/spawn的服务service，请求的数据类型是turtlesim::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());
    // 通过call方法把封装的数据发出去，并且这个cal函数也是一个阻塞性的函数，发出之后会一直等待服务器返回的response
	add_turtle.call(srv);
	// 显示服务调用结果（输出产生海龟的名字）
	ROS_INFO("Spwan turtle successfully [name:%s]", srv.response.name.c_str());
	return 0;
};

所以在Ros中创建客户端一共有四步

（1）初始化ROS节点

（2）创建一个Client实例

（3）发布服务请求数据

（4）等待Server处理之后的应答结果

配置CMakeList.txt中的编译规则

和前面是完全一样的

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

开始编译

回到项目根目录

catkin_make

最后运行程序

新建终端

roscore

Ctrl+Alt+T

rosrun turtlesim turtlesim_node

Ctrl+Alt+T

rosrun learning_service turtle_spawn

运行之后就会在（2，2）的位置上生成一只名叫turtle2的新的小海龟

Python创建客户端

Python创建客户端代码如下

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

代码和C++代码逻辑是一样的，就是有一些方法的函数名不太相同

最后运行程序

新建终端

roscore

Ctrl+Alt+T

rosrun turtlesim turtlesim_node

Ctrl+Alt+T

rosrun learning_service turtle_spawn.py

运行之后效果和C++运行之后的效果是一样的

10： 服务端Server的编程实现

任务介绍

本章主要实现通过Client端的程序发送Request，控制Server端的海龟运动还是停止。当Client发一个Request后，海龟随即开始运动，当再发送一个Requests后，海龟就停止运动。Server端就接收Client端发送过来的指令，并且完成海龟是否运动的指令的发送，同时我们需要返回一个Response告诉Client端你的指令是否成功发送了，所以Client就可以实时知道海龟当前的运动状态。

这一次的Server实现时机会包含Service的Server这一端的实现，也会包含发布者（Publisher）的实现。

C++服务端Server实现

创建一个Server服务端的步骤

（1）初始化ROS节点

（2）创建Server实例

（3）循环等待服务请求，进入回调函数

（4）在回调函数中完成服务功能的处理，并反馈应答数据

代码如下

/**
 * 该例程将执行/turtle_command服务，服务数据类型std_srvs/Trigger
 */
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <std_srvs/Trigger.h>

// 由于我们之后会在回调函数中使用这个publisher，所以我们把他创建成全局变量
ros::Publisher turtle_vel_pub;
// 这个额作为标志位标志我们的海龟是运动还是停止（false-停止，true-运动）
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;
}

接下来开始重复之前的工作环节

配置CMakeList.txt中的编译规则

和前面是完全一样的

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

开始编译

回到项目根目录

catkin_make

最后运行程序

新建终端

roscore

Ctrl+Alt+T

rosrun turtlesim turtlesim_node

Ctrl+Alt+T

rosrun learning_service turtle_command_server

Ctrl+Alt+T(下面这个运行一次就会跑起来，再运行一次就会停下来，再发送就就运动，类似于开关)

rosservice call /turtle_command "{}"

Python服务端Server实现

代码如下

# 该例程将执行/turtle_command服务，服务数据类型std_srvs/Trigger
import rospy
import thread,time
from geometry_msgs.msg import Twist
from std_srvs.srv import Trigger, TriggerResponse

# 全局变量（标志位和一个发布指令的publisher）
pubCommand = False;
turtle_vel_pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
# 在python中不同于C++，c++中有ros::spinOnce()函数可以查看一次回调队列，看有没有数据，如果有就进入回调函数，如果没有就跳出函数继续where循环的程序，但是在Python中并没有实现spinOnce，只有spin可以不断循环，所以我们需要通过多线程的机制来实现spinOnce加if的标志位判断。
#所以在这里创建一个线程，如果标志位是true就发宋海龟运动指令，否则就跳过
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):
    # Python中的取反操作
	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()

运行起来效果是一样的

11： 服务数据的定义与使用

很多场景下ROS定义好的服务数据很难满足我们的需求，所以如何自己定义一个ROS的服务数据类型就很重要了

之前定义了一个Person消息类别，然后传输一个Person的个人信息。但是无论订阅者是否接收到了数据发布者都一直在发送，非常不人性化。所以我们可以利用Service信息传递的机制，让数据接收者也就是客户端只有接收到数据，并且返回成功状态给服务端，服务端才会继续发送数据。

– 通过语言无关的文件定义一个Service，服务名叫“ /show_person”，服务数据类型叫“ learning_service::Person ”

11.1：自定义服务数据的步骤

（1）定义srv文件

这里的数据类型和之前订阅发布者模型类似，就不过多注释了，区别就是Service模式是有反馈的，所以我们用“ — ”来区别发送信息和接收反馈。三个横线以上就是发送的信息，三个横线以下就是接受到的反馈。

一样的都是新建一个srv文件夹，然后新建文件输入一下代码。

Person.srv

string name
uint8 sex
uint8 age

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

（2）在package.xml中添加功能包依赖

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

<!-- 编译依赖 -->
<build_depend>message_generation</build_depend>
<!-- 执行依赖 -->
<exec_depend>message_runtime</exec_depend>

（3）在CMakeList.txt中添加编译选项

然后在CMakeList.txt添加编译选项

find_package(
	......
	message_generation
)

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

catkin_package(
	......
	message_runtime
)

（4）编译生成语言相关文件

还是说回到项目根目录下，使用catkin_make编译项目

catkin_make

11.2：C++使用自定义服务数据

（1）C++创建客户器

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 person_srv;
	person_srv.request.name = "Tom";
	person_srv.request.age  = 20;
	person_srv.request.sex  = learning_service::Person::Request::male;
    // 请求服务调用
	ROS_INFO("Call service to show person[name:%s, age:%d, sex:%d]", 
			 person_srv.request.name.c_str(), person_srv.request.age, person_srv.request.sex);
	person_client.call(person_srv);
	// 显示服务调用结果
	ROS_INFO("Show person result : %s", person_srv.response.result.c_str());

	return 0;
};

（2）C++创建服务端

C++创建服务器还是之前讲的四步：

（1）初始化ROS节点

（2）创建Server实例

（3）循环等待服务请求，进入回调函数

（4）在回调函数中完成服务功能的处理，并反馈应答数据

/**
 * 该例程将执行/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);
	// 设置反馈数据,返回一个OK
	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）配置CMakeList.txt编译规则

和之前都是一样的

1） 设置需要编译的代码和可执行文件

2） 设置链接库

3） 添加依赖项

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)

（4）编译并运行客户端和服务端

还是回到项目根目录下新建终端

catkin_make
roscore

Ctrl+Alt+T

rosrun learning_service person_server

Ctrl+Alt+T

rosrun learning_service person_client

运行结果如下，客户端发一条数据，服务端就接收一条数据，并且给客户端一个OK的回复。

11.3：Python使用自定义服务数据

（1）python客户端

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

（2）python服务端

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

python不需要编译，打开一个终端

roscore

Ctrl+Alt+T

rosrun learning_service person_server

Ctrl+Alt+T

rosrun learning_service person_client

运行结果和C++代码一毛一样，客户端发一条数据，服务端就接收一条数据，并且给客户端一个OK的回复。

12：参数的使用与编程的方法

在ROS Master（节点管理器）中有一个Parameter Server（参数服务器），Parameter Server一个全局字典，用来保存节点间的配置参数，这样一个配置参数是各个节点都可以随时访问的。可以把Parameter Server理解为一个存储全局变量的存储空间。

我们使用一下这个参数服务器

先创建一个功能包

cd addam_first/src
catkin_create_pkg learning_parameter roscpp rospy std_srvs

12.1：Parameter命令行的使用（rosparam）

（1）列出当前所有参数

rosparam list

（2）显示某个参数值

rosparam get 参数名

（3）设置某个参数值

rosparam set 参数名 参数值

（4）保存参数到文件

在ROS中如果我们参数比较多可能会保存在YAML参数文件，类似于下图的格式。

rosparam dump 文件名

（5）从文件读取参数

rosparam load 文件名

（6）删除参数

rosparam delete 参数名

（7）运行起来试一下

还是用小海龟做测试，新建终端把小海龟仿真器运行起来

roscore

Ctrl+Alt+T（新建终端）

rosrun turtlesim turtlesim_node

再新建一个终端学习rosparam的使用方式

输入rosparam回车，会输出rosparam后面可以跟的所有参数

输入rosparam list，可以查看当前海龟历程所有的参数，输出如下图

$ rosparam list

我想看看背景色b的值，可以通过rosparam get 参数名获取，输出如下：

rosparam get turtlesim/background_b

我想修改背景色的b值，可以通过rosparam set 参数名 参数值 修改

rosparam set /turtlesim/backgrount_b

但是小海龟的背景色还是没有修改，我们需要发送请求让它重置，可以看到这个命令运行后背景色就变了。

rosservice call clear "{}"

比如现在需要保存参数作为后期调试的依据，就使用rosparam dump 文件名.yaml

rosparam dump param.yaml

这个文件会保存在当前终端的路径，默认终端是主文件夹。可以双击打开文件后看到的就是参数及对应的值。

我们可以修改RGB成255，255,255

然后rosparam load param.yaml，这样所有的参数就会像我们修改的文件里面一样，然后rosservice call clear "{}"更新一下，小海龟的背景色就会变成白色。

通过rosparam delete 参数名 可以删除掉某个参数。

比如

rosparam delete /turtlesim/backgrount_b
rosparam list
rosservice call clear "{}"

就可以看到没有这个参数了

12.2：Parameter在程序的使用（C++）

（1）代码编写

1）初始化ROS节点

2）get函数获取参数

3）set函数设置参数

主要就是 ros::param::get(“变量名”, 接受的变量值的变量名);

以及 ros::param::set(“变量名”, 更改后的变量值);

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

（2）设置CMakeList中的编译规则

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

（3）回到根目录使用catkin_make编译文件

（4）运行发布者

roscore

此时背景颜色是蓝色

rosrun turtlrsim turtlesim_node

运行完一下语句之后背景色会变成白色

rosrun learnning_parameter parameter_config

12.3：Parameter在程序的使用（Python）

13：ROS中的坐标系管理系统（TF）

坐标变换是机器人基本理论中非常重要的一块，详情可以去看之前发布的机器人运动学一帖。学会平移和旋转的变换。任意两个坐标系的坐标变换都可以用一个4*4的矩阵来描述他的平移和旋转。本节不介绍理论，只介绍ROS中对坐标变换相对应的方法

Ros中提供了TF功能包管理所有的坐标系，我们用TF功能包使用查询的方式就可以知道两个坐标之间的关系是什么样的。

TF功能包默认会记录十秒钟之内机器人所有坐标系的位置关系，比如说可以查询五秒钟之前机器人的头部和机器人腿部的位置关系。

TF实现的基本原理

从实现来看TF是是一种广播加监听的形式，和之前的Client Server Subscriber publisher都不一样。只要启动了ROSMaster，启动了TF，就会在后台去维护一个TF树，所有的坐标系都以树形结构保存在TF树中，我们任意一个节点想去查询两个坐标系之间的关系都可以通过TF树去查到。

那这个有什么用呢，打个比方，智能小车车顶的摄像和车身是不同的两种坐标系，现在车顶的摄像头检测到前方30厘米有一个障碍物，这个障碍物离车身有多远呢，通过tf把车顶摄像做坐标关系和车身的坐标关系找出来，然后乘以前方障碍物的坐标。

教学案例

还是借助于小海龟，生成两只海龟，一只海龟出生在中心点，另一只海龟出生在下方。然后下面那只海龟会像中心点的海龟走，我们可以用键盘移动中心点的海龟，另一只海龟虽然我们没有控制它，但是它会跟着中心点的海龟走。

先安装turtle_tf 的功能包

用sudo命令去安装功能包，sudo apt-get install ros-版本号-包名

sudo apt-get install ros-melodic-tuetle-tf

这里提示已经安装好了，那应该是ROS原本的库里已经有这个功能包了

运行的话需要启动以下指令

roslaunch turtle_tf turtle_tf_demo.launch

roslaunch指令会启动ros中的launch文件，launch文件类似于一个脚本文件，可以把很多个节点的启动都写进去。

工具一：view_frames5

可以运行如下代码监听节点间的位姿关系

rosrun tf view_frames

这个命令会把节点间的位姿关系生成一个pdf文件，默认保存在主文件夹。

工具二：tf_echo

这个工具可以直接帮助我们查询TF树中任意两个坐标系，并实时输出他们之间的关系以及通过什么坐标可以使得两个坐标系能相等。rosrun tf tf_echo 坐标系1 坐标系2

rosrun tf tf_echo turtle1 turtle2

工具三：rviz

坐标系的可视化工具，可以运行如下命令来获取当前坐标系之间的关系

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

先把Fixed Frame改成World，由于是显示位姿信息，所以添加一个TF，我们移动一下turtle1，可以看到这里面的坐标也变了。黄线的中心点就是would。

14：TF坐标系广播与监听的实现

还是去创建一个功能包来存放代码，回到项目的根目录下的src文件夹，输入以下命令

catkin_ceate_pkg learning_tf roscpp rospy tf turtlesim

这个包需要依赖于roscpp rospy tf turtlesim

C++实现TF广播器

在ROS中如何实现一个TF广播器（通过TF广播任意两个坐标系的关系）

定义TF广播器（TransformBroadcaster）

创建坐标变换值

发布坐标变换（sendTransform）

代码如下：

/**
 * 该例程产生tf数据，并计算、发布turtle2的速度指令
 */
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>

std::string turtle_name;

// 在poseCallback，我们传入参数是海龟的位置，这个位置就是海龟的x坐标，y坐标和偏转角度
void poseCallback(const turtlesim::PoseConstPtr& msg)
{
	// 创建tf的广播器，通过这个实例可以把海龟相对于would坐标系的位姿关系广播出去
	static tf::TransformBroadcaster br;

	// 初始化tf数据
	// Transform是四乘四的矩阵
	tf::Transform transform;
	// 接下来给transform添加数据，首先是平移数据。通过setOrigin设置平移参数。包括x平移和y平移，由于是平面，所以z平移永远为0
	transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
	// 接下来就是旋转了，Quaternion就是四元树
	tf::Quaternion q;
	// 一样的由于是平面的关系，所以绕x轴y轴的旋转都是0，只会绕z轴旋转
	q.setRPY(0, 0, msg->theta);
	// setRotation设置到transform里面去，现在的transform保存了平移和旋转的关系，两个坐标系之间的位姿关系就可以完全描述了
	transform.setRotation(q);

	// 广播world与海龟坐标系之间的tf数据，ros::Time::now()=>添加时间戳，该坐标系描述的是world与turtle_name之间的位姿关系
	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;
	} 
	// 获取到当前海龟的名字，turtle1或者是turtle2，是一个字符串
	turtle_name = argv[1];

	// 订阅海龟的位姿话题
	// 创建一个句柄
	ros::NodeHandle node;
	// 生成一个订阅者，订阅海龟仿真器里面不断发布的位姿消息，一旦有消息进来就调用poseCallback
	ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);

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

	return 0;
};

C++实现TF监听器

如何实现一个TF监听器

定义TF监听器（TransformListener）

查找坐标变换（waitForTransform 、 lookupTransform）

代码如下：

/**
 * 该例程监听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，通过如下程序发布请求生成一直新的海龟（之前学过的Service编程）
	ros::service::waitForService("/spawn");
	ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("/spawn");
	turtlesim::Spawn srv;
	add_turtle.call(srv);

	// 创建发布turtle2速度控制指令的发布者，要让海龟2动就要发布/turtle2/cmd_vel这样的指令，然后输入速度（topic编程）
	ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 10);

	// 创建tf的监听器，通过TransformListener可以监听任意两个坐标系之间的位姿关系
	tf::TransformListener listener;

	ros::Rate rate(10.0);
	while (node.ok())
	{
		// 获取turtle1与turtle2坐标系之间的tf数据，创建一个StampedTransform保存平移和变换之间的关系
		tf::StampedTransform transform;
		try
		{
            // 这两句是程序的关键，怎么通过tf监听两个坐标系之间的关系，waitForTransform等待变换，lookupTransform查询变换
            // 查看系统中当前时间是否存在turtle1和turtle2两个坐标系，如果存在这句话才会跳过，不存在等待三秒就会报错。
			listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0));
            // 查询当前时间这"/turtle2", "/turtle1"两个坐标系关系是什么样的，结果保存在transform
			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;
};

编译刚才所写的C++代码

编译规则还是一样的，设置需要编译的代码和生成的可执行文件，然后去设置链接库

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

然后回到工作空间的根目录用catkin_make去编译。

运行C++代码

首先打开终端，开启ROS Master节点管理器

roscore

新建一个终端，去运行启动可视化的海龟仿真器

rosrun turtlesim turtlesim_node

接下来运行刚才写的可执行文件，因为有两只海龟，分别要发布两只海龟的位置关系

我们先来运行第一个，发布的是turtle1和world之间的位姿关系，“__name:=turtle1_tf_broadcaster”是ROS中的重映射的机制，后面的“/turtle1”就是输入的参数

重映射：在ROS中给节点初始化时默认有一个命名，但是由于我们需要运行两次，所以在运行时需要重映射修改节点名，避免在ROS中发生资源冲突。

rosrun learning_tf turtle_tf_broadcaster __name:=turtle1_tf_broadcaster /turtle1

接下来运行第二个，发布的是turtle2和world之间的位姿关系

rosrun learning_tf turtle_tf_broadcaster __name:=turtle2_tf_broadcaster /turtle2

然后查询海龟1和海龟2两个坐标系之间的位姿关系吗，并且给海龟2发布一个速度指令让海龟2去移动

rosrun learning_tf turtle_tf_listener

最后发布指令，通过键盘控制海龟1，海龟2是自动跟着海龟1去走的

rosrun turtlesim turtle_teleop_key

效果和之前是一样的。

15：launch启动文件的使用方法

15.1：launch文件概述:

launch启动文件旨在减少用户不断打开终端，不断去输入的重复工作。

launch文件：通过XML文件实现多节点的配置和启动（可自动启动ROS Master）

一个launch文件使用XML描述的内容和属性，launch文件可以通过如下这种XML的形式实现ROS中多个节点的配置与启动，如果使用launch文件就不需要不断打开终端使用rosrun的命令了，在一个复杂的机器人系统中节点是很多的，一个个去rosrun非常麻烦，我们可以通过roslaunch去启动一个launch文件，实现launch文件包含节点的所有功能。

每个node搜是一个节点，就是一个rosrun的命令。

launch文件在启动时会自动去查找当前系统中有没有ROS Master（roscore）在运行，如果有的话就不会去运行ROS Master，如果没有就自己启动一个ROS Master

15.2：launch文件的常用语法

（1）node节点标签

launch中的所有内容都是通过XML这种标签来做描述的。

所有launch文件的内容都必须要包含到一个根标签（根元素），launch文件中的根元素采用标签定义。

为启动节点

其中 pkg：节点所在的功能包的名称

type： 节点的可执行文件

name：节点运行时的名称

node节点中除了刚才的三个属性外还有其他的可选属性。

output：用来控制某个节点是不是要把日志信息打印到当前的终端里面的

respawn：控制节点如果启动运行挂掉了是否重启

required：表示launch文件里某个节点是不是必须要启动起来

ns： ns代表命名空间，可以给每个节点创建命名空间，避免命名冲突

args： 节点的输入参数。

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

（2）参数设置标签

launch文件中还包含了一些其他标签，比如参数设置的标签

用来设置ROS系统运行中的参数，存储在ROS参数服务器中，其中，name：参数名，value：参数值。

可以加载参数文件中多个参数，把一个参数文件中的所有参数都加载到服务器中来。

<param name="output_frame" value="odom"/>
<rosparam file="params.yaml" command="load" ns="params"/>

表示某个launch文件所使用的局部变量。仅限于该launch文件使用，使用语法见下方。其中name：参数名，default：参数值。

和param不同，param表示存在参数服务器中的参数，而arg表示只存在我们这个launch文件中的参数只允许内部使用。

<arg name="arg-name" default="arg-value"/>
<!-- 调用的方式=》$(arg 参数名) -->
<param name="foo" value="$(arg arg-name)"/>
<node name="node" pkg="package" type="type" args="$(arg arg-name)"/>

（3）重映射 标签

可以把ROS中某些计算图的资源进行重新命名，重映射ROS计算图资源的命名，语法如下，其中from为原命名，to为映射之后的命名。

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

（4）嵌套 标签

可以包含其他launch文件，在开发过程中有可能会出现launch文件比较多，可以用include去给launch文件模块化，然后一个main函数就包含多个launch文件

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

file ：包含的其他launch文件路径

在launch中的标签是非常·多的，更多标签可以参考“ roslaunch/XML - ROS Wiki ”。

15.3：launch使用演示

（1）创建一个功能包，这个功能包不需要任何的依赖

回到项目根目录的src文件夹

catkin_create_pkg learning_launch

然后在新的功能包里创建一个文件夹，命名为launch

（2） 第一个launch文件（simple.launch）

用一个launch的根标签包含两个node节点。

第一个节点是位于learning_topic的listener，节点名叫做person_subscriber

第二个节点是位于learning_topic的talker，节点名叫做person_publisher

后面跟这个output=“screen”，默认两个节点运行成功之后日志信息不会打印到终端里来，为了看到他们的输入效果，我们让他们在终端进行显示，就是让 output=“screen”。

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

可以启动一下这个launch文件

还是回到项目根目录使用catkin_make进行编译

然后使用roslaunch 包名 文件名进行运行

catkin_make
roslaunch learning_launch simple.launch

（3）用launch文件配置参数（turtlesim_parameter_config.launch）

这里用到了param和rosparam的标签都是设置变量的

首先一开始设置了一个海龟的数量为2，我们会把这个变量及参数传输到参数服务器。

接下来我们运行功能包turtlesim下的节点turtlesim_node，就是海龟仿真器节点。

接下来利用param创建两个参数及值，海龟1的名字叫Tom，海龟2的名字叫Jerry

然后用rosparam去加载一个参数文件。

最后启动我们的键盘控制节点

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

建立一个config文件夹，param.yaml文件内容如下：

A: 123
B: "hello"

group:
  C: 456
  D: "hello"

可以新建一个终端，rosparam list可以看到设置的参数都在了，设置在node里面的前面就多一个命名空间，就是node的名字。

还有yaml文件夹中的，我们在文件中设置了group的命名空间，但是由于这个rosparam在节点中，所以优先会加载节点中的命名空间。

（4）启动两个海龟的跟随文件的launch案例

上一节中用五个终端启动五个ros文件才运行得了的现在一个launch文件就能包含

主要就是args是传入参数的内容。运行之后的效果和上一节是完全一样的。

<launch>

    <!-- Turtlesim Node-->
    <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>

（5）remap标签和include标签的使用案例

通过include使得这个launch文件可以包含并启动learning_launch包下的launch/simple.launch文件。

通过node标签启动一个仿真器节点。在启动海龟后我们不希望海龟输入指令的话题名叫/turtle1/cmd_vel，就可以通过remap直接改成/cmd_vel，然后/turtle1/cmd_vel就不复存在了

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

当然，这个node节点不会只可以调用C++打包好的可执行文件，type中加上.py就可以使用python文件了。

16：常用可视化工具的使用

16.1：rqt可视化工具

ros有提供一个qt工具箱，都是基于qt开发的可视化工具，可以使用一系列rqt开头的命令行来启动这些qt工具。

之前使用过rqt_graph，这是一个计算图可视化工具，箭头代表话题的方向，椭圆代表节点。

（1）：rqt_console（日志输出工具）

还是以小海龟为例

roscore + rosrun turtlesim turtlesim_node启动海龟节点

然后新建终端，rqt_console。

上面的窗口使用来显示ros系统中的日志信息的，包括warn信息，infer，error等

下面就可以对上面的信息进行筛选。

开启一个海龟的控制节点并且让海龟去撞墙。这个时候就会输出warn信息告诉我们海龟撞墙了。

（2）：rqt_plot（数据绘图工具）

我们可以选择要绘制的数据，比如把/turtle1/pose数据绘制出来，那就在Topic上选择/turtle1/pose

（3）：rqt_image_view（图像渲染工具）

该工具可以显示图像信息，但是由于现在没有摄像头，当ros调用摄像头就会开始显示图像信息了。

（4）：rqt（ros中所有的rqt工具的集合）

这是一个非常综合的工具。

16.2：Rviz可视化工具

机器人开发过程中的数据可视化界面，可以通过ros中的rviz工具显示点云、地图、模型、障碍物、图像等。

（1）Rviz概述

Rviz是一款三维可视化工具，可以很好的兼容基于ROS软件框架的机器人平台。
在rviz中，可以使用可扩展标记语言XML对机器人、周围物体等任何实物进行尺寸、质量、位置、材质、关节等属性的描述，并且在界面中呈现出来。

同时，rviz还可以通过图形化的方式，实时显示机器人传感器的信息、机器人的运动状态、周围环境的变化等信息。

总而言之，rviz通过机器人模型参数、机器人发布的传感信息等数据，为用户进行所有可监测信息的图形化显示。用户和开发者也可以在rviz的控制界面下，通过按钮、滑动条、数值等方式，控制机器人的行为。

（2）Rviz运行区域描述

0：显示区，比如点云、模型、障碍物、图像等，都会显示在这个区域。

1：工具栏。可以选择Rviz的工具。

2：显示项列表。比如我们要显示一个图像，就需要add添加一个图像，设置话题等参数。

3：用来显示视角的，可以通过上面的选项调整观看模型的角度

4：时间县市区：主要显示ROS启动的时间以及系统本身默认的时间

（3）启动Rviz

roscore
rosrun rviz rviz

16.3：Gazebo三维物理仿真平台

（1）Gazebo概述

Rviz是数据显示平台，显示的前提是有数据。Gazebo是数据仿真平台，就是本来没有数据去创建数据。

Gazebo是一款功能强大的三维物理仿真平台

​ 具备强大的物理引擎

​ 高质量的图形渲染

​ 方便的编程与图形接口

​ 开源免费

Gazebo典型的应用场景包括

​ 测试机器人算法

​ 机器人的设计

​ 显示情景下的回溯测试

（2）Gazebo工作区介绍

0：显示仿真出来的模型和传感器

1：基本的配置区，可以调整一些光线及模型的位姿

2和3：是中间显示的模型的列表以及属性，会有哪些模型或几个机器人。insert可以去插入我们模型库里的模型

4：和Rviz一样也是时间

（3）运行起来

运行资源会需求比较多，用虚拟机可能会运行不起来。

roslaunch gazebo_ros willowgarage_world.launch

17：课程总结与进阶攻略

课程总结

aunch/simple.launch文件。

通过node标签启动一个仿真器节点。在启动海龟后我们不希望海龟输入指令的话题名叫/turtle1/cmd_vel，就可以通过remap直接改成/cmd_vel，然后/turtle1/cmd_vel就不复存在了

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

当然，这个node节点不会只可以调用C++打包好的可执行文件，type中加上.py就可以使用python文件了。

16：常用可视化工具的使用

16.1：rqt可视化工具

ros有提供一个qt工具箱，都是基于qt开发的可视化工具，可以使用一系列rqt开头的命令行来启动这些qt工具。

之前使用过rqt_graph，这是一个计算图可视化工具，箭头代表话题的方向，椭圆代表节点。

（1）：rqt_console（日志输出工具）

还是以小海龟为例

roscore + rosrun turtlesim turtlesim_node启动海龟节点

然后新建终端，rqt_console。

[外链图片转存中…(img-SYET5Ead-1656580149747)]

上面的窗口使用来显示ros系统中的日志信息的，包括warn信息，infer，error等

下面就可以对上面的信息进行筛选。

开启一个海龟的控制节点并且让海龟去撞墙。这个时候就会输出warn信息告诉我们海龟撞墙了。

[外链图片转存中…(img-OFVSZpxQ-1656580149748)]

（2）：rqt_plot（数据绘图工具）

[外链图片转存中…(img-Gn936uRE-1656580149748)]

我们可以选择要绘制的数据，比如把/turtle1/pose数据绘制出来，那就在Topic上选择/turtle1/pose

[外链图片转存中…(img-YwlIQXK5-1656580149748)]

（3）：rqt_image_view（图像渲染工具）

该工具可以显示图像信息，但是由于现在没有摄像头，当ros调用摄像头就会开始显示图像信息了。

[外链图片转存中…(img-Ben7O9In-1656580149749)]

（4）：rqt（ros中所有的rqt工具的集合）

这是一个非常综合的工具。

16.2：Rviz可视化工具

机器人开发过程中的数据可视化界面，可以通过ros中的rviz工具显示点云、地图、模型、障碍物、图像等。

（1）Rviz概述

Rviz是一款三维可视化工具，可以很好的兼容基于ROS软件框架的机器人平台。
在rviz中，可以使用可扩展标记语言XML对机器人、周围物体等任何实物进行尺寸、质量、位置、材质、关节等属性的描述，并且在界面中呈现出来。

同时，rviz还可以通过图形化的方式，实时显示机器人传感器的信息、机器人的运动状态、周围环境的变化等信息。

总而言之，rviz通过机器人模型参数、机器人发布的传感信息等数据，为用户进行所有可监测信息的图形化显示。用户和开发者也可以在rviz的控制界面下，通过按钮、滑动条、数值等方式，控制机器人的行为。

（2）Rviz运行区域描述

0：显示区，比如点云、模型、障碍物、图像等，都会显示在这个区域。

1：工具栏。可以选择Rviz的工具。

2：显示项列表。比如我们要显示一个图像，就需要add添加一个图像，设置话题等参数。

3：用来显示视角的，可以通过上面的选项调整观看模型的角度

4：时间县市区：主要显示ROS启动的时间以及系统本身默认的时间

[外链图片转存中…(img-0fV470EV-1656580149749)]

（3）启动Rviz

roscore
rosrun rviz rviz

16.3：Gazebo三维物理仿真平台

（1）Gazebo概述

Rviz是数据显示平台，显示的前提是有数据。Gazebo是数据仿真平台，就是本来没有数据去创建数据。

Gazebo是一款功能强大的三维物理仿真平台

​ 具备强大的物理引擎

​ 高质量的图形渲染

​ 方便的编程与图形接口

​ 开源免费

Gazebo典型的应用场景包括

​ 测试机器人算法

​ 机器人的设计

​ 显示情景下的回溯测试

（2）Gazebo工作区介绍

0：显示仿真出来的模型和传感器

1：基本的配置区，可以调整一些光线及模型的位姿

2和3：是中间显示的模型的列表以及属性，会有哪些模型或几个机器人。insert可以去插入我们模型库里的模型

4：和Rviz一样也是时间

[外链图片转存中…(img-XmJxIIYN-1656580149749)]

（3）运行起来

运行资源会需求比较多，用虚拟机可能会运行不起来。

roslaunch gazebo_ros willowgarage_world.launch

17：课程总结与进阶攻略

课程总结

进阶攻略就没详细描述了，感兴趣的可以去看古月ROS入门21讲的最后一节

最后

以上就是淡淡大白为你收集整理的Ros1入门到入土的全部内容，希望文章能够帮你解决Ros1入门到入土所遇到的程序开发问题。

如果觉得靠谱客网站的内容还不错，欢迎将靠谱客网站推荐给程序员好友。