ROS介绍

ROS介绍

一.ROS简介

1.ROS操作系统

机器人操作系统（ROS）是一种用于编写机器人软件的灵活框架。 它是工具，库和协议的集合，旨在简化各种机器人平台上，去构建复杂而强大的机器人。

ROS 是 Robot Operating System的简写，翻译过来就是机器人操作系统。它是一个软件框架，目的是提供开发平台，工具及生态给开发人员，让开发人员快速的去开发强大的机器人系统。

2.ROS软件结构组成

ROS Master

• 管理Node节点间进行通讯的
• 每个Node节点都需要到Ros Master中进行通讯

通roscore命令可以启动ROS Master，启动节点前，必须启动ROS Master。

# 启动ROS Master
roscore

ROS Node

• 具备单一功能的可执行程序
• 可以单独编译，可执行，可管理
• 存放在package中

3.ROS的设计哲学

1. Peer to peer

点对点的设计。

• Node节点单元
• 采用了分布式网络结构
• 节点间通过RPC + TCP/UDP进行通讯

2. Distributed

分散协同式布局。可以将ROS同时部署到多台机器上，让多台机器进行通讯。

3. Multi-lingual

多编程语言的支持。

• 可以采用python，c++，lisp等语言进行开发。
• 遵循协议进行编码，和编程语言无关

4. Light-weight

组件工具包丰富。ros提供了丰富的开发工具包。

5. Free and open-source

免费并且开源。

• BSD许可。可修改，可复用，可商用。

• 开源使软件进步

  ​

  4.工作目录说明

• workspace： 工作空间

• build：ros编译打包的结果产出目录。我们不需要对这个文件夹做任何编辑操作，属于自动生成。

• devel: 开发所需要的目录

• src：存放package的目录

• CMakeLists.txt: 整个工作空间编译的脚本。此文件我们通常不用去做修改操作。

  ​

工作单元package

一个项目中可以创建多个工作单元，这个工作单元，我们称之为package。

package的文件组成结构为以下：

• pkg1： package的名称，开发过程中根据自己实际情况进行创建设定。
• CMakeLists.txt: 当前package的编译脚本。通常需要为c++代码添加编译时的依赖，执行等操作。
• package.xml: package相关信息。通常添加一些ros库的支持
• include文件夹: 存放c++ 头文件的
• config文件夹：存放参数配置文件，格式为yaml
• launch文件夹：存放.launch文件的。
• src：c++源代码
• scripts：python源代码
• srv：存放定义的service
• msg: 存放自定义的消息协议
• action： 存放自定义的action

4.第一个ROS程序

创建流程

1. 新建工作空间目录结构

mkdir -p first_ws/src

创建一个first_ws的工作空间，并且在其中创建了src目录

first_ws就是工作空间的名称

2. 编译工作空间

cd first_ws
catkin_make

来到创建的工作空间目录下，调用ros的命令catkin_make，将工作空间进行编译。

编译后，会得到工作空间的文件结构，build，devel，CMakeLists.txt都会自动生成。

catkin_make是ROS的编译工具，我们会经常用到。

创建package流程

来到workspace的src目录下

cd first_ws/src

2. 通过catkin创建package

catkin_create_pkg demo_cpp roscpp rospy rosmsg

catkin_create_pkg是创建package的命令。运行以上命令，会新建好package的目录，并且在目录中创建CMakeLists.txt，package.xml，src，include等文件和目录

第一个参数demo_cpp是指创建的package名称，可以根据自己的实际需求去设定。

后面的参数roscpp，rospy，rosmsg是指当前创建的这个package需要提供哪些环境依赖。

roscpp是对c++的一种依赖，有了它就可以用c++开发ros程序。

rospy是对python的一种依赖，有了它就可以用python开发ros程序。

rosmsg是Node间通讯的消息协议依赖，有了它就可以让节点间进行消息通讯。

使用Clion开发package

1. 启动clion

开启命令行工具，来到工作空间目录下，设置开发环境。

cd first_ws
source devel/setup.bash

此操作非常重要。devel目录中的setup.bash是用于开发环境中，方便找到开发依赖的。

来到clion的安装目录下，通过命令启动clion

cd ~/clion/bin
./clion.sh

笔者的clion安装目录在~/clion，大家根据实际情况，来到自己clion的安装目录，并且进入到bin目录，因为启动文件在bin目录下。

2. 使用clion打开package

clion启动后，首先点击open，然后找到工作空间，在工作空间的src中找到要打开的package，在package中找到CMakeLists.txt，选中双击，此时点击open as project就可以打开package做开发了。

3.cpp版本的helloworld

#include 
#include "ros/ros.h"

using namespace std;

int main(int argc, char **argv) {
	//初始化ros，并且设置节点名称
    ros::init(argc, argv, "cpphello");
	//创建节点
    ros::NodeHandle node;

    cout << "hello ros cpp" << endl;
    return 0;
}

4.python版本的helloworld

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

if __name__ == '__main__':
    # 创建节点
    rospy.init_node("pyhello")
    print("hello ros python")

5.常规API

阻塞线程spin

可以阻塞当前的线程。

• C++对应API

  ros::spin();
  

• Python对应API

  rospy.spin()
  

频率操作Rate

可以按照一定的频率操作循环执行。

• C++对应API

  ros::Rate rate(10);
  while(true) {
      rate.sleep();
  }
  

• Python对应API

  rate = rospy.Rate(10)
  while True:
      rate.sleep()
  
  

rate中的参数10，代表在while循环中，每秒钟运行10次。

节点状态判断

可以判断当前节点是否停止

可以按照一定的频率操作循环执行。

• C++对应API

  ros::ok()
  

• Python对应API

  rospy.is_shutdown()
  

##Node工具

查询当前运行的所有节点

rosnode list

查询当前运行的所有节点测试节点是否存活

rosnode ping /节点名称

查询节点信息

rosnode info /节点名称

杀死节点

rosnode kill /节点名称

清理假死的节点

rosnode cleanup

6.常规配置介绍

理解package.xml配置

在package中，存在package.xml文件，此文件主要是描述当前package的一些信息。

  demo_py
  0.0.0
  The demo_py package
  itheima
  TODO

  catkin
  roscpp
  rosmsg
  rospy
  roscpp
  rosmsg
  rospy
  roscpp
  rosmsg
  rospy

  
  

• name： 描述了当前package的名称
• version: 描述了当前package的版本信息
• build_depend: 编译时依赖
• build_export_depend: 编译导出依赖
• exec_depend: 执行依赖

package.xml文件是我们通过catkin_create_pkg创建package时自动生成的，我们要求能看懂当前文件的一些含义。

我们还需要掌握，当我们的项目在开发阶段，需要引入一些其他依赖时，需要在此处配置一些依赖。主要是掌握build_depend，build_export_depend，exec_depend进行配置依赖

理解CMakeLists.txt

CMakeLists.txt是在创建Package时自动生成的，不需要我们去写。

c++11支持

通常开发过程中我们会添加c++11的支持，只需要解开以下注释：

add_compile_options(-std=c++11)

find_package

find_package(catkin REQUIRED COMPONENTS
        roscpp
        rosmsg
        rospy
        )

此处的find_package提供了ros的package依赖支持。

package依赖指的是一个package依赖另外一个package，在ros中提供了很多package。

roscpp提供的是c++ 开发和编译环境，配置时需要在此处添加，也需要在package.xml中配置。

rospyt提供的是python开发和编译环境

build

在CMakeLists.txt中，搜索以下

###########
## Build ##
###########

在build区域中，通过add_executable添加可执行的配置。

add_executable(exe_name src/xxx.cpp src/h1.h src/h2.h)

内置填写可以分为多个部分：

exe_name指的是打包出来可执行文件名称

src/xxx.cpp指的是可执行文件

src/h1.h src/h2.h ...指的是可执行文件依赖的文件

在build区域中，通过target_link_libraries添加动态链接库。

target_link_libraries(
        exe_name
        ${catkin_LIBRARIES}
)

TIP

exe_name要和前面add_executable可执行配置名称一致。

${catkin_LIBRARIES}是标准库，默认必须添加。

理解CMakeLists.txt

CMakeLists.txt是在创建Package时自动生成的，不需要我们去写。

c++11支持

通常开发过程中我们会添加c++11的支持，只需要解开以下注释：

add_compile_options(-std=c++11)

find_package

find_package(catkin REQUIRED COMPONENTS
        roscpp
        rosmsg
        rospy
        )

此处的find_package提供了ros的package依赖支持。

package依赖指的是一个package依赖另外一个package，在ros中提供了很多package。

roscpp提供的是c++ 开发和编译环境，配置时需要在此处添加，也需要在package.xml中配置。

rospyt提供的是python开发和编译环境

build

在CMakeLists.txt中，搜索以下

###########
## Build ##
###########

在build区域中，通过add_executable添加可执行的配置。

add_executable(exe_name src/xxx.cpp src/h1.h src/h2.h)

内置填写可以分为多个部分：

exe_name指的是打包出来可执行文件名称

src/xxx.cpp指的是可执行文件

src/h1.h src/h2.h ...指的是可执行文件依赖的文件

在build区域中，通过target_link_libraries添加动态链接库。

target_link_libraries(
        exe_name
        ${catkin_LIBRARIES}
)

exe_name要和前面add_executable可执行配置名称一致。

${catkin_LIBRARIES}是标准库，默认必须添加。

7.Topic通讯

ROS整个工程启动后的一个结构现状如图：

多个Node节点都需要到ROS Master进行注册。

每个Node完成自己的功能逻辑。有的时候Node和Node间需要有数据的传递，这个时候ROS提供了一种数据通讯机制。

ROS 在设计Node间通讯机制的时候，考虑的还是比较周详的，设计了Topic通讯机制，如下图：

Node间进行通讯，其中发送消息的一方，ROS将其定义为Publisher(发布者)，将接收消息的一方定义为Subscriber(订阅者)。考虑到消息需要广泛传播，ROS没有将其设计为点对点的单一传递，而是由Publisher将信息发布到Topic(主题)中，想要获得消息的任何一方都可以到这个Topic中去取数据。我们理解Topic的时候，可以认为Topic相当于一个聚宝盆，东西放进去后，不管同时有多少人来取，都可以拿到数据。

7.1实现publisher的C++代码

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

using namespace std;

int main(int argc, char **argv) {

    string nodeName = "cpppublisher";
    string topicName = "cpptopic";

    //初始化ros节点
    ros::init(argc, argv, nodeName);
    ros::NodeHandle node;

    //通过节点创建publisher
    const ros::Publisher &publisher = node.advertise(topicName, 1000);

    //按照一定周期，发布消息
    int index = 0;
    ros::Rate rate(1);
    while (ros::ok()) {
        std_msgs::String msg;

        msg.data = "hello pub " + to_string(index);
        publisher.publish(msg);
        rate.sleep();
        index++;
    }
    return 0;
}

调试发布者

调试Publisher主要是查看是否有发送数据，也就是提供一个订阅的调试工具。ROS提供了命令行工具和图形化工具进行调试。

1. 通过rostopic工具进行调试

查看所有的主题

rostopic list

打印主题所发布的信息

rostopic echo cpptopic

2. 通过rqt_topic工具进行调试

通过命令启动rqt_topic工具

rosrun rqt_topic rqt_topic

选中要调试的主题

7.2 实现subscribe的C++代码

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

using namespace std;

void topicCallback(const std_msgs::String::ConstPtr &msg) {
    cout << (*msg).data << endl;
}

int main(int argc, char **argv) {
    string nodeName = "cppsubscriber";
    string topicName = "cpptopic";

    //初始化节点
    ros::init(argc, argv, nodeName);
    ros::NodeHandle node;
    //创建订阅者
    const ros::Subscriber &subscriber = node.subscribe(topicName, 1000, topicCallback);
	// 阻塞线程
    ros::spin();
    return 0;
}

调试订阅者

调试Subscriber主要是查看是否能收到数据，也就是提供一个发布的调试工具。ROS提供了命令行工具和图形化工具进行调试。

1. 通过自己编写的publisher进行调试

rosrun demo_topic cpppublisher

2. 通过rostopic工具进行调试

查询主题所需要的数据类型

rostopic type cpptopic

模拟发布数据

rostopic pub cpptopic std_msgs/String hello -r 10

rostopic pub是模拟发布数据的命令

cpptopic是将数据发送到那个主题，根据自己实际调试的主题来写。

std_msgs/String是这个主题所需要的数据类型，我们是通过rostopic type cpptopic进行查询出来的。

hello是发送的数据，根据自己的调试需求来写。

-r 指的是发送频率

3. 通过rqt_publisher工具进行调试

通过命令启动rqt_publisher工具

rosrun rqt_publisher rqt_publisher

4.查看节点关系示意图

rosrun rqt_graph rqt_graph 

7.3实现publisher的python代码

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

import rospy
from std_msgs.msg import String

if __name__ == '__main__':
    nodeName = "pypublisher"
    topicName = "pytopic"
    # 初始化节点
    rospy.init_node(nodeName)

    # 创建发布者
    publisher = rospy.Publisher(topicName, String, queue_size=1000)

    rate = rospy.Rate(10)
    count = 0
    while not rospy.is_shutdown():
        # 发布消息
        publisher.publish("hello %d" % count)
        rate.sleep()
        count += 1;

调试发布者

​ 调试Publisher主要是查看是否有发送数据，也就是提供一个订阅的调试工具。ROS提供了命令行工具和图形化工具进行调试

1.通过rostopic工具进行调试

查看所有的主题

rostopic list

打印主题所发布的信息

rostopic echo pytopic

2. 通过rqt_topic工具进行调试

通过命令启动rqt_topic工具

rosrun rqt_topic rqt_topic

选中要调试的主题

8.Msg消息

在现有的模型中，我们通常都是Node与Node节点进行数据的传递。

查询所有的消息类型

rosmsg list

可以查询出当前支持的所有消息类型。例如我们用到过的std_msgs/String和geometry_msgs/Twist

查询消息类型的数据结构

我们还可以对一个消息的数据结构进行查询。

rosmsg show std_msgs/String

结果显示为string data，说明了std_msgs/String这种消息类型的数据，内部包含了一个叫做data类型为string的数据。

我们也可以看看geometry_msgs/Twist包含了什么数据

rosmsg show geometry_msgs/Twist

结果显示为：

geometry_msgs/Vector3 linear
  float64 x
  float64 y
  float64 z
geometry_msgs/Vector3 angular
  float64 x
  float64 y
  float64 z

geometry_msgs/Twist包含了 linear和angular两个数据。

linear的数据类型为geometry_msgs/Vector3。

angular的数据类型为geometry_msgs/Vector3。

我们发现在linear下面的x，y，z是有缩进的，这个缩进表示的是，geometry_msgs/Vector3这种类型的数据下面包含了三个数据x，y，z，他们的类型都是float64。

8.1自定义消息

自定义消息流程

1 . 创建msg目录，移除不需要的目录

在pakage目录下新建msg目录，删除掉include和src目录

2. 新建Student.msg文件

创建的这个Student.msg文件就是自定义消息文件，需要去描述消息的格式。

我们可以编辑代码如下

string name
int64 age

这个自定义的消息包含两个数据形式，name和age，name的类型 是string，age的类型是int64。

这个msg文件其实遵循一定规范的，每一行表示一种数据。前面是类型，后面是名称。

ros不只是提供了int64和string两种基本类型供我们描述，其实还有很多：

msg类型	C++对应类型	Python对应类型
bool	uint8_t	bool
int8	int8_t	int
int16	int16_t	int
int32	int32_t	int
int64	int64_t	int，long
uint8	uint8_t	int
uint16	uint16_t	int
uint32	uint32_t	int
uint64	uint64_t	int，long
float32	float	float
float64	float	float
string	std:string	str，bytes
time	ros:Time	rospy.Time
duration	ros::Duration	rospy.Duration

3. 配置package.xml文件

在package.xml种添加如下配置:

message_generation
message_runtime

message_generation是消息生成工具，在打包编译时会用到

message_runtime运行时消息处理工具

4. 配置CMakeLists.txt

在find_package添加message_generation，结果如下:

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rosmsg
  rospy
  message_generation
)

添加add_message_file，结果如下:

add_message_files(
        FILES
        Student.msg
)

这里的Student.msg要和你创建的msg文件名称一致，且必须时在msg目录下，否则编译会出现问题

添加generation_msg，结果如下:

generate_messages(
        DEPENDENCIES
        std_msgs
)

这个配置的作用是添加生成消息的依赖，默认的时候要添加std_msgs

修改catkin_package，结果如下:

catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES demo_msg
#  CATKIN_DEPENDS roscpp rosmsg rospy
#  DEPENDS system_lib
        CATKIN_DEPENDS message_runtime
)

为catkin编译提供了依赖message_runtime

检验自定义消息

1. 编译项目

来到工作空间目录下，运行编译

catkin_make

2. 查看生成的头文件

来到devel的include目录下，如果生成了头文件说明，自定义消息创建成功。

3. 通过rosmsg工具校验

rosmsg show demo_msgs/Student

查看运行结果，运行结果和自己定义的相一致，说明成功。

C++使用自定义消息

1. 自定义消息依赖的添加

在开发过程种，自定义消息是以一个package存在的，其他package需要用到这个自定义消息的package，是需要添加依赖的。

来到package.xml中，添加如下:

demo_msgs
demo_msgs
demo_msgs

来到CMakeLists.txt文件中，找到find_package，添加demo_msgs自定义消息依赖，添加结果如下：

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rosmsg
  rospy
  demo_msgs
)

2. 引入依赖

#include "demo_msgs/Student"

3. 构建消息

demo_msgs::Student stu;
stu.name = "itheima";
stu.age = 13;

消息依赖加入后，具体类型是demo_msgs::Student

Python使用自定义消息

1. 导入模块

from demo_msgs.msg import Student

python导入自定义消息模块，遵循一定的规范，from 模块名.msg import 具体的消息

2. 构建消息

stu = Student()
stu.name = "itheima"
stu.age = 13

8.2 复杂类型消息

引用自定义消息

开发过程中，有的时候会碰到传递的数据结构复杂，会有嵌套消息的存在，例如现在需要创建一个Team.msg，基本结构如下:

名称	类型	描述
name	string	团队名称
leader	TODO	团队领导

在这个设计过程中，我们希望leader这个属性是一个复杂类型，对应着我们之前自定义的Student.msg。那么当前的Team.msg该如何编写。

首先，我们在msg目录下新建Team.msg，Team.msg的内容如下:

string name
Student leader

Team.msg要去引用Student.msg，Student就是具体类型，通过Student leader来去声明。

接着，我们要到CMakeLists.txt文件中，修改add_message_files，修改如下：

add_message_files(
        FILES
        Student.msg
        Team.msg
)

其实我们就是添加了Team.msg。

值得注意的是，Team.msg不能放到Student.msg前面，原因是，Team.msg引用了Student.msg，如果调换位置，编译器会先去编译Team.msg，这个时候编译器是找不到Student.msg，因此会出现错误。

被引用对象要放到引用者的前面。

引用标准消息库

rosmsg是ros的标准消息库，开发中，有的时候我们需要将标准消息封装到自己的消息中去的。例如Team.msg中需要加入一个数据进行描述Team情况：

名称	类型	描述
name	string	团队名称
leader	Student	团队领导
intro	TODO	团队介绍

在此处，我们希望intro的类型是std_msgs/String，我们对Team.msg实际编码为:

string name
Student leader
std_msgs/String intro

Team.msg要去使用std_msgs/String，std_msgs/String就是具体类型，通过std_msgs/String intro来去声明。

引用其他三方消息

作为自定义的消息，有可能还需要使用三方的消息库，我们在此以geometry_msgs这个三方库作为案例进行说明。

名称	类型	描述
name	string	团队名称
leader	Student	团队领导
intro	std_msgs/String	团队介绍
location	TODO	位置

在此处，我们希望location的类型是geometry_msgs/Twist，我们对Team.msg实际编码为:

string name
Student leader
std_msgs/String intro
geometry_msgs/Twist location

需要对msgs文件夹内CMakeLists.txt进行修改(新增geometry_msgs):

find_package(catkin REQUIRED COMPONENTS
        roscpp
        rosmsg
        rospy
        message_generation
        geometry_msgs
        )

generate_messages(
        DEPENDENCIES
        std_msgs
        geometry_msgs
)

引入三方库过程中，我们需要对src文件夹的package.xml文件进行配置添加：

geometry_msgs
geometry_msgs
geometry_msgs

我们还需要对CMakeLists.txt进行修改，在find_package中添加如下:

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rosmsg
  rospy
  message_generation
  geometry_msgs
)

添加geometry_msgs是方便编译器在查找包时，能找到这个库。

在generate_messages中添加geometry_msgs如下:

generate_messages(
        DEPENDENCIES
        std_msgs
        geometry_msgs
)

此处是为了生成定义好的头文件而去做配置

消息类型为数组

作为自定义的消息，有的时候我们需要去定义数组集合来去帮我们存储数据，例如我们要加入members来描述成员:

名称	类型	描述
name	string	团队名称
leader	Student	团队领导
intro	std_msgs/String	团队介绍
location	geometry_msgs/Twist	位置
members	TODO	团队成员

在此处，我们希望members的类型是Student的数组集合类型，我们对Team.msg实际编码为:

string name
Student leader
std_msgs/String intro
geometry_msgs/Twist location
Student[] members

我们采用[]表示数组，对应中C++中的Vector类型，对应Python中的列表list

二.小乌龟案例

小乌龟节点启动

1. 启动小乌龟模拟器节点

rosrun turtlesim turtlesim_node

2. 启动小乌龟键盘输入节点

rosrun turtlesim turtle_teleop_key

启动完成后，可以通过键盘输入操控小乌龟移动。

小乌龟操控原理

1. 节点信息查看

小乌龟启动过程中，我们启动了两个可执行的程序：turtlesim_node和turtle_teleop_key 。可以通过命令查看当前的启动的节点：

rosnode list

可以查看到启动程序对应的节点/turtlesim和/teleop_turtle

通过命令可以查看/turtlesim节点的详情

rosnode info /turtlesim

命令运行后，可以看到以下结果:

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

Subscriptions: 
 * /turtle1/cmd_vel [geometry_msgs/Twist]

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


contacting node http://ubuntu:42049/ ...
Pid: 20218
Connections:
 * topic: /rosout
    * to: /rosout
    * direction: outbound
    * transport: TCPROS
 * topic: /turtle1/cmd_vel
    * to: /teleop_turtle (http://ubuntu:44433/)
    * direction: inbound
    * transport: TCPROS

rosnode info命令可以查看当前节点的一些信息：

• Publications：此节点上定义的发布者
• Subscriptions：此节点上定义的订阅者
• Services：此节点上定义的服务
• 进程id，占用的网络端口
• Connections： 此节点和其他节点间的连接信息

同理，我们也可以通过rosnode info查询/teleop_turtle节点的信息，结果如下:

--------------------------------------------------------------------------------
Node [/teleop_turtle]
Publications: 
 * /rosout [rosgraph_msgs/Log]
 * /turtle1/cmd_vel [geometry_msgs/Twist]

Subscriptions: None

Services: 
 * /teleop_turtle/get_loggers
 * /teleop_turtle/set_logger_level


contacting node http://ubuntu:44433/ ...
Pid: 20443
Connections:
 * topic: /rosout
    * to: /rosout
    * direction: outbound
    * transport: TCPROS
 * topic: /turtle1/cmd_vel
    * to: /turtlesim
    * direction: outbound
    * transport: TCPROS

现在我们大致可以搞清楚一些通讯的关系：

• /teleop_turtle节点存在一个发布者，往/turtle1/cmd_vel主题中发布数据。
• /turtlesim节点存在一个订阅者，去/turtle1/cmd_vel主题中获取数据。

2. 可视化工具查询节点关系

rqt_graph工具提供了可视化的工具方便我们查看这种节点间的关系：

rosrun rqt_graph rqt_graph

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-hLwtVjh4-1581315862493)(img/小乌龟节点关系图.png)]

图像显示，/teleop_turtle通过主题/turtle1/cmd_vel给/turtlesim进行数据传递

调试工具操控小乌龟

1. rqt_publisher模拟数据发送

启动rqt_publisher工具

rosrun rqt_publisher rqt_publisher

通过图形化配置参数：

2. 通过命令行模拟数据发送

rostopic pub /turtle1/cmd_vel geometry_msgs/Twist "linear:
  x: 1.0
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 3.0"

小乌龟坐标系相关

面板

• 面板的坐标原点在左下脚，即左下角为(0,0)
• 面板的X轴是自左向右，数值是0开始正向增长
• 面板的Y轴是自下向上，数值是0开始正向增长
• 面板的宽度和高度相同，值为11.088899

小乌龟

• 小乌龟的坐标原点为小乌龟的中心点。

小乌龟移动指令参数

我们通过rostopic命令可以获得小乌龟的移动数据类型为geometry_msgs/Twist

rostopic type /turtle1/cmd_vel

通过rosmsg命令可以查看数据的详细格式:

rosmsg show geometry_msgs/Twist

输出的格式为:

geometry_msgs/Vector3 linear
  float64 x
  float64 y
  float64 z
geometry_msgs/Vector3 angular
  float64 x
  float64 y
  float64 z

geometry_msgs/Vector3 linear指的是线速度，小乌龟只用到了float64 x，代表着乌龟向前进的线速度

geometry_msgs/Vector3 angular指的是角速度，小乌龟只用到了float64 z，代表着乌龟旋转的角速度

重置小乌龟

rosservice call/reset

需求介绍与分析

实现图形化界面，来控制小乌龟的运行。

小乌龟的模拟器，其实是一个Subscriber订阅者，我们要控制小乌龟的移动，我们就去创建一个Publisher发布者，按照Topic主题规范发布信息。

总结起来，我们要干的事情就是:

• 创建QT UI
• 创建 Publisher
• 整合QT UI 和 Publisher进行数据发布

Qt 环境配置

C++ 开发ROS项目过程中，如果要引入Qt，需要进行依赖的配置。

以新建的demo_turtlepackage为例，我们就要对CMakeLists.txt进行依赖配置。

1. 添加c++ 11编译的支持

add_compile_options(-std=c++11)

默认的时候，这个配置是被注释起来的，我们只要解开注释就可以。

2. 添加Qt的环境配置

##############################################################################
# Qt Environment
##############################################################################

set(CMAKE_INCLUDE_CURRENT_DIR ON)
set(CMAKE_AUTOMOC ON)

find_package(Qt5 COMPONENTS Core Gui Widgets PrintSupport)
##############################################################################

find_package中，都是要加载的Qt模块，后续如果还需要添加其他的Qt模块，可以在后面追加。

Node 创建流程

1. 创建node启动的cpp文件

在src目录下创建turtle_control.cpp，代码如下：

#include "ros/ros.h"
#include 
using namespace std;

int main(int argc, char *argv[]) {
    string nodeName = "qt_turtle_ctrl";
	// 创建节点
    ros::init(argc,argv,nodeName);
    ros::NodeHandle node;
    return 0;
}

2. 配置CMake

来到CMakeLists.txt文件中，添加如下:

add_executable(turtle_control src/turtle_control.cpp)
target_link_libraries(turtle_control
        ${catkin_LIBRARIES}
        Qt5::Core
        Qt5::Gui
        Qt5::Widgets
        Qt5::PrintSupport
 )

add_executable是把turtle_control.cpp标记为可执行的

target_link_libraries是为turtle_control.cpp提供链接库的，值得注意的是，${catkin_LIBRARIES}是ros的依赖库，Qt5::Core，Qt5::Gui，Qt5::Wigets，Qt5::PrintSupport是qt的依赖库。

Qt UI的创建

C++代码实现

1. Qt库的引入

#include 
#include 

QtWidgets是qt的组件模块，提供大量的ui控件

QApplication是Qt应用入口

2. 编写Qt窗体

// 创建Qt Application
QApplication app(argc, argv);
// 创建窗体
QWidget window;
// 设置窗体为当前激活窗体
app.setActiveWindow(&window);
//显示窗体
window.show();
// Qt程序执行
app.exec();

3. 根据需求进行UI布局

// 设置布局
QFormLayout layout;
window.setLayout(&layout);
// 距离输入框
QLineEdit editLinear;
layout.addRow("距离", &editLinear);
// 角度输入框
QLineEdit editDegrees;
layout.addRow("角度", &editDegrees);
// 发送按钮
QPushButton btnSend("发送");
layout.addRow(&btnSend);

4. 事件添加

btnSend.connect(&btnSend, &QPushButton::clicked, &window, btnClicked);

void btnClicked() {
    std::cout << "clicked" << std::endl;
}

Publisher创建

1. 创建publisher对象

const ros::Publisher &publisher = node.advertise(topicName, 1000);

值得注意的是，在创建publisher对象时，这里要去确定的有两个点，第一就是topicName，第二就是传递的消息类型。

此处我们只能确定topic 的名称，给小乌龟发送数据的topic为/turtle1/cmd_vel

2. 确定消息传递的数据类型

通过rostopic命令查询主题对应的消息类型

rostopic type /turtle1/cmd_vel

得到的结果为geometry_msgs/Twist接下来我们需要导入这个消息类型对应的库

#include "geometry_msgs/Twist.h"

接下来就是确定publisher创建时候的类型

const ros::Publisher &publisher = node.advertise(topicName, 1000);

一些规律的掌握，消息类型为geometry_msgs/Twist，需要导入的头文件为geometry_msgs/Twist.h，需要确定的编码类型为geometry_msgs::Twist

3. 发送消息

//创建消息
geometry_msgs::Twist twist;
//填充消息数据
twist.linear.x = linearX;
twist.angular.z = angularZ * M_PI / 180;
//发送消息
publisherPtr->publish(twist);

完整实例代码

#include "ros/ros.h"
#include 
#include "std_msgs/String.h"
#include "QtWidgets"
#include "QApplication"
#include "geometry_msgs/Twist.h"
#include "math.h"

using namespace std;


void sendclicked(ros::Publisher &publisher, QLineEdit &lineredit, QLineEdit &angularedit) {

    double x = lineredit.text().toDouble();
    double z = angularedit.text().toDouble();
    geometry_msgs::Twist twist;
    twist.linear.x = x;
    twist.angular.z = z *M_PI / 180;
    publisher.publish(twist);
//    cout << "clicked" << endl;
}

int main(int argc, char **argv) {

    string nodeName = "turtle_ctrl";
    string topicName = "turtle1/cmd_vel";

    //初始化ros节点
    ros::init(argc, argv, nodeName);
    ros::NodeHandle node;

    //创建publisher
    ros::Publisher publisher = node.advertise(topicName, 1000);

    //创建Qt，application
    QApplication app(argc, argv);

    //创建窗体
    QWidget window;
    //设置窗体大小及标题
    window.resize(600, 150);
    window.setWindowTitle("小乌龟控制器");
    //表单布局
    QFormLayout layout;
    window.setLayout(&layout);

    //线速度输入框
    QLineEdit lineredit("0.0");
    layout.addRow("线速度", &lineredit);
    //角速度输入框
    QLineEdit angularedit("0.0");
    layout.addRow("线速度", &angularedit);

    //button
    QPushButton btn("发送");

    layout.addRow(&btn);

    //点击事件
    btn.connect(&btn, &QPushButton::clicked, &window, [&publisher, &lineredit, &angularedit]() {
        sendclicked(publisher, lineredit, angularedit);
    });
    //展示窗体
    window.show();

    //程序退出
    return app.exec();
}

python代码实现

1. python环境配置

为clion添加python2的支持，具体可以参考前面的讲解

在新建的py文件开头添加环境说明:

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

修改新建py文件的权限，添加可执行权限

chmod +x turtle_control.py

2. node源码编写

import rospy

if __name__ == '__main__':
    nodeName = "qt_turle_ctrl";
    # 创建ros node
    rospy.init_node(nodeName, anonymous=True)

Qt UI的创建

1. Qt库的引入

from PyQt5.QtWidgets import *

值得注意的是，在导入模块过程中是没有提示的。系统已经有Qt5的库，只是编译器没有找到。

2. 编写Qt窗体

# 创建Qt程序
app = QApplication(sys.argv) 
# 创建窗体
window = QWidget()
window.setWindowTitle("小乌龟控制")
window.resize(400, 0)
# 显示窗体
window.show();
# Qt程序执行
sys.exit(app.exec_())

值得注意的是后面的app.exec()这个函数不能用，要用app.exec_()。

原因是当前的Qt版本是5.5.1， 5.10.+的版本后面才支持的exec()

3. 根据需求进行UI布局

# 设在布局
layout = QFormLayout()
window.setLayout(layout)

# 添加组件
editLinear = QLineEdit()
editLinear.setText("0.0")
layout.addRow("距离", editLinear)

editAngular = QLineEdit()
editAngular.setText("0.0")
layout.addRow("角度", editAngular)

btnSend = QPushButton("发送")
layout.addRow(btnSend)

4. 事件添加

btnSend.clicked.connect(btnClicked)

define btnClicked():
    print "clicked"

Publisher创建

1. 确定消息传递的数据类型

通过rostopic命令查询主题对应的消息类型

rostopic type /turtle1/cmd_vel

得到的结果为geometry_msgs/Twist接下来我们需要导入这个消息类型对应的模块到py文件中

from geometry_msgs.msg import Twist

一些规律的掌握，消息类型为geometry_msgs/Twist，需要导入的模块为geometry_msgs.msg下的Twist

2. 创建publisher对象

publisher = rospy.Publisher(topicName, Twist, queue_size=1000)

值得注意的是，在创建publisher对象时，这里要去确定的有两个点，第一就是topicName，第二就是传递的消息类型。

给小乌龟发送数据的topic为/turtle1/cmd_vel 类型为Twist

3. 发送消息

# 创建消息
twist = Twist()
# 填充数据
twist.linear.x = linearX
twist.angular.z = angluarZ * math.pi / 180
# 发送消息
publisher.publish(twist)

完整示例代码

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

import rospy
from geometry_msgs.msg import Twist

from PyQt5.QtWidgets import *
import sys
import math


def btnClicked(editLinear, editAngluar, publisher):
    linearX = float(editLinear.text())
    angluarZ = float(editAngular.text())

    # 创建消息
    twist = Twist()
    # 填充数据
    twist.linear.x = linearX
    twist.angular.z = angluarZ * math.pi / 180
    # 发送消息
    publisher.publish(twist)


if __name__ == '__main__':
    nodeName = "qt_turle_ctrl";
    topicName = "/turtle1/cmd_vel"

    # 创建ros node
    rospy.init_node(nodeName, anonymous=True)

    # 创建publisher
    publisher = rospy.Publisher(topicName, Twist, queue_size=1000)

    # 创建Qt程序
    app = QApplication(sys.argv)
    # 创建窗体
    window = QWidget()
    window.setWindowTitle("小乌龟控制")
    window.resize(400, 0)

    # 设在布局
    layout = QFormLayout()
    window.setLayout(layout)

    # 添加组件
    editLinear = QLineEdit()
    editLinear.setText("0.0")
    layout.addRow("距离", editLinear)

    editAngular = QLineEdit()
    editAngular.setText("0.0")
    layout.addRow("角度", editAngular)

    btnSend = QPushButton("发送")
    layout.addRow(btnSend)

    # 事件
    btnSend.clicked.connect(lambda: btnClicked(editLinear, editAngular, publisher))

    window.show()

    sys.exit(app.exec_())

小乌龟位置相关信息

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-6TF4LN5N-1581315862494)(img/小乌龟案例.png)]

小乌龟的模拟器，也是一个Publisher发布者，发布了当前小乌龟相关的数据信息，我们要获取小乌龟的数据，我们就去创建一个Subscriber订阅者，按照Topic主题规范进行订阅数据。

总结起来，我们要干的事情就是:

• 创建QT UI
• 创建 Publisher
• 整合QT UI 和 Subscriber进行数据订阅

C++代码实现

#include "ros/ros.h"
#include 
#include "std_msgs/String.h"
#include "QtWidgets"
#include "QApplication"
#include "geometry_msgs/Twist.h"
#include "math.h"
#include "turtlesim/Pose.h"

using namespace std;

QLabel *lbX;
QLabel *lbY;
QLabel *linerV;
QLabel *angularV;
QLabel *theta;

void receiveCall(const turtlesim::Pose::ConstPtr &message) {
    lbX->setText(QString::fromStdString(to_string(message->x)));
    lbY->setText(QString::fromStdString(to_string(message->y)));
    linerV->setText(QString::fromStdString(to_string(message->linear_velocity)));
    angularV->setText(QString::fromStdString(to_string(message->angular_velocity)));
    theta->setText(QString::fromStdString(to_string(message->theta)));

}


void sendclicked(ros::Publisher &publisher, QLineEdit &lineredit, QLineEdit &angularedit) {

    double x = lineredit.text().toDouble();
    double z = angularedit.text().toDouble();
    geometry_msgs::Twist twist;
    twist.linear.x = x;
    twist.angular.z = z * M_PI / 180;
    publisher.publish(twist);
//    cout << "clicked" << endl;
}

int main(int argc, char **argv) {

    string nodeName = "turtle_ctrl";
    //publisher的广播地址
    string topicName = "turtle1/cmd_vel";
    //subcriber的接收地址
    string receiveName = "/turtle1/pose";

    //初始化ros节点
    ros::init(argc, argv, nodeName);
    ros::NodeHandle node;

    //开启异步轮循器
    ros::AsyncSpinner spinner(2);
    spinner.start();

    //创建publisher
    ros::Publisher publisher = node.advertise(topicName, 1000);

    //创建subcriber
    const ros::Subscriber &subscriber = node.subscribe(receiveName, 100, receiveCall);

    //创建Qt，application
    QApplication app(argc, argv);

    //创建窗体
    QWidget window;
    //设置窗体大小及标题
    window.resize(600, 150);
    window.setWindowTitle("小乌龟控制器");
    //表单布局
    QFormLayout layout;
    window.setLayout(&layout);

    //线速度输入框
    QLineEdit lineredit("0.0");
    layout.addRow("线速度", &lineredit);
    //角速度输入框
    QLineEdit angularedit("0.0");
    layout.addRow("角速度", &angularedit);

    //当前X坐标
    lbX = new QLabel();
    layout.addRow("当前X坐标", lbX);
    //当前Y坐标
    lbY = new QLabel();
    layout.addRow("当前Y坐标", lbY);
    //当前线速度
    linerV = new QLabel();
    layout.addRow("当前线速度", linerV);
    //当前角速度
    angularV = new QLabel();
    layout.addRow("当前角速度", angularV);
    //当前角度
    theta = new QLabel();
    layout.addRow("当前角速度", theta);

    //button
    QPushButton btn("发送");

    layout.addRow(&btn);

    //点击事件
    btn.connect(&btn, &QPushButton::clicked, &window, [&publisher, &lineredit, &angularedit]() {
        sendclicked(publisher, lineredit, angularedit);
    });
    //展示窗体
    window.show();

//    ros::spin();
    //程序退出
    return app.exec();
}

python代码实现

日志级别

日志级别的划分：

级别	描述
DEBUG	调试日志，供开发测试使用
INFO	常规日志，用户可见级别的信息
WARN	警告信息。
ERROR	错误信息。程序出错后打印的信息
FATAL	致命错误。出现宕机的日志记录

日志可见等级顺序是：

DEBUG > INFO > WARN > ERROR > FATAL

通常程序在运行时，都会设置一个日志等级，默认等级时INFO。

• 假如将当前程序日志等级设置为DEBUG，可查看到的日志信息包含:DEBUG , INFO , WARN , ERROR , FATAL
• 假如将当前程序日志等级设置为INFO，可查看到的日志信息包含: INFO , WARN , ERROR , FATAL
• 假如将当前程序日志等级设置为WARN，可查看到的日志信息包含: WARN , ERROR , FATAL
• 假如将当前程序日志等级设置为ERROR，可查看到的日志信息包含: ERROR , FATAL
• 假如将当前程序日志等级设置为FATAL，可查看到的日志信息包含: FATAL

C++日志API

在ROS系统中，提供了常规API供我们使用

基础API格式：

ROS_DEBUG("打印的内容");
ROS_INFO("打印的内容");
ROS_WARN("打印的内容");
ROS_ERROR("打印的内容");
ROS_FATAL("打印的内容");

stream API格式：

ROS_DEBUG_STREAM("打印的内容" << "hello");
ROS_INFO_STREAM("打印的内容" << "hello");
ROS_WARN_STREAM("打印的内容" << "hello");
ROS_ERROR_STREAM("打印的内容" << "hello");
ROS_FATAL_STREAM("打印的内容" << "hello");

Python日志API

rospy.logdebug("打印的内容")
rospy.loginfo("打印的内容")
rospy.logwarn("打印的内容")
rospy.logerror("打印的内容")
rospy.logfatal("打印的内容")

日志查看系统

我们可以时使用rqt_console命令来查看过滤日志

rosrun rqt_console rqt_console

日志级别设置

通过右上角的设置按钮进入进行日志级别的设置：

日志级别过滤

三.Service通讯

1.Service通讯架构

ROS提供了节点与节点间通讯的另外一种方式：service通讯。

Service通讯分为client端和server端。

• client端负责发送请求(Request)给server端。
• server端负责接收client端发送的请求数据。
• server端收到数据后，根据请求数据和当前的业务需求，产生数据，将数据(Response)返回给client端。

Service通讯的特点：

• 同步数据访问
• 具有响应反馈机制
• 一个server多个client
• 注重业务逻辑处理

Service通讯的关键点：

• service的地址名称
• client端访问server端的数据格式
• server端响应client端的数据格式

需求

构建一个service通讯。需求是一个client端，一个server端。

server端为client端提供服务。

服务的内容是：帮助client端计算加法求和。

c++实现server端

#include "ros/ros.h"
#include 
#include "roscpp_tutorials/TwoInts.h"

using namespace std;

bool callback(roscpp_tutorials::TwoIntsRequest &request, roscpp_tutorials::TwoIntsResponse &response) {
    // 运算逻辑
    response.sum = request.a + request.b;
    return true;
}

int main(int argc, char **argv) {
    string nodeName = "cppservice";
    string serviceName = "/demo_service/add_tow_int";

    // 创建节点
    ros::init(argc, argv, nodeName);
    ros::NodeHandle node;

    // 创建服务端
    const ros::ServiceServer &server = node.advertiseService(serviceName, callback);

    //阻塞
    ros::spin();
    return 0;
}

调试server端

调试server端主要是查看server端是否能接收到请求，并根据请求数据处理相应的业务逻辑，然后返回处理好的结果。

在这里，我们只需要模拟client端发送请求就可以了。

ROS提供了命令行工具和图形化工具供我们调试开发。

1. rosservice命令行调试

通过rosservice list命令可以帮助我们查询出当前运行的所有service

rosservice list

查询的结果中，我们可以得到对应的服务名称/demo_service/add_tow_int

通过查询的服务名称，来调用此服务

rosservice call /demo_service/add_tow_int "a:1 b:3"

rosservice call负责调用service。第一个参数是要调用的service的名称，后面的参数是调用时需要传入的参数。

2.rqt_service_caller工具调试

通过命令呼出工具

rosrun rqt_service_caller rqt_service_caller

c++实现client端

#include "ros/ros.h"
#include 

#include "roscpp_tutorials/TwoInts.h"

using namespace std;

int main(int argc, char **argv) {
    string nodeName = "service_client";
    string serviceName = "/demo_service/add_tow_int";

    //创建节点
    ros::init(argc, argv, nodeName);
    ros::NodeHandle node;

    //创建client
    ros::ServiceClient &&client = node.serviceClient(serviceName);

    //创建service
    roscpp_tutorials::TwoInts service;
    service.request.a = 10;
    service.request.b = 5;
    //调用service
    client.call(service);
	//打印结果
    ROS_INFO_STREAM(service.response.sum);

    return 0;
}

调试Client端

通过已有的server来调试client

rosrun demo_service client

python实现server端

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

import rospy
from roscpp_tutorials.srv import TwoInts


def handler(request):
    rospy.loginfo("python call")
    return request.a + request.b


if __name__ == '__main__':
    nodeName = "pyservice";
    serviceName = "/demo_service/add_two_int"

    # 创建节点
    rospy.init_node(nodeName)
    # 创建service
    rospy.Service(serviceName, TwoInts, handler)
    # 阻塞线程
    rospy.spin()

调试Server

调试server端主要是查看server端是否能接收到请求，并根据请求数据处理相应的业务逻辑，然后返回处理好的结果。

在这里，我们只需要模拟client端发送请求就可以了。

ROS提供了命令行工具和图形化工具供我们调试开发。

1. rosservice命令行调试

通过rosservice list命令可以帮助我们查询出当前运行的所有service

rosservice list

查询的结果中，我们可以得到对应的服务名称/demo_service/add_tow_int

通过查询的服务名称，来调用此服务

rosservice call /demo_service/add_tow_int "a:1 b:3"

rosservice call负责调用service。第一个参数是要调用的service的名称，后面的参数是调用时需要传入的参数。

2.rqt_service_caller工具调试

通过命令呼出工具

rosrun rqt_service_caller rqt_service_caller

python实现client端

#!/usr/bin/env python
# encoding:utf-8

import rospy

from roscpp_tutorials.srv import TwoInts

if __name__ == '__main__':
    nodeName = "pyclient"
    serviceName = "/demo_service/add_two_int"

    # 创建节点
    rospy.init_node(nodeName)

    # 等待服务器连接
    rospy.wait_for_service(serviceName)
    # 创建服务调用代理
    call = rospy.ServiceProxy(serviceName, TwoInts)
    # 调用服务
    result = call(4, 9)

    # 打印服务调用结果
    rospy.loginfo(result)

调试Client端

通过已有的server来调试client

rosrun demo_service client.py

server_client案例

c++版本

python版本

四.Srv消息

业务与数据

在Service通讯模型中，我们通常都是Client将数据发送给Server，Server经过业务逻辑处理，将结果返回给client。如下图：

在此处，我们需要特别关注的就是传输过程中的Request和Response，这两个都是数据，一个是请求的数据，一个是响应的数据。从数据的角度来说，数据由业务所产生，但不应该与业务无关，应该与规范相关，其实就是定义规范来限制数据类型，来规范业务间的通讯。

在此，ROS提供的是srv数据来做此操作，如下图:

在数据交互过程中的请求阶段，我们将整个包当成一个srv，包含了request和response，这个阶段交给client端去填充request的数据。

在数据交互过程中的server处理阶段，server拿到client发送的srv包，从中获得request数据，根据业务逻辑来去处理操作数据。

在数据交互过程中的响应阶段，将server操作的结果填充到srv的response，将srv返回。

在这三个阶段中，srv自始至终就是一个数据包，规范了client的数据填充，也规范了server的数据填充。

在ROS中对于Service通讯模式的数据类型，系统提供了一些现成的类型供我们参考和使用。

查询所有的消息类型

rossrv list

可以查询出当前支持的所有消息类型。例如我们用到过的roscpp_tutorials/TwoInts

查询消息类型的数据结构

我们还可以对一个消息的数据结构进行查询。

rossrv show roscpp_tutorials/TwoInts

显示的结果为:

int64 a
int64 b
---
int64 sum

结果显示分为两个部分，中间用---分隔。

上面部分是request的数据规范。

下面部分是response的数据规范。

发散与探讨

我们在前面可以发现，ros系统还是提供了大量的数据类型供我们使用。但是数据类型再多，很有可能也满足不了我的实际业务场景，这个时候，我们就需要定制自己的数据类型了。

后面我们会着重讲到如何是定制自己的数据类型。

自定义消息

前言

在Ros中，如果没有现成的消息类型来描述要去传递的消息时，我们会自定义消息。

我们会新建一个Package来去自定义消息，通常这个Package写任何的业务逻辑，只是用来声明自定义的消息类型，可以只定义一种消息类型，也可以定义多种消息类型，根据业务需求来定。

所以，首先我们单独的创建一个package，我们取名为demo_srvs，一定要要添加roscpp，rospy，rosmsg的依赖。

这个包名取名也是有讲究的，业务名_srvs。

自定义消息流程

1 . 创建srv目录，移除不需要的目录

在pakage目录下新建srv目录，删除掉include和src目录

2. 新建.srv文件

创建的这个NumOption.srv文件就是自定义消息文件，需要去描述消息的格式。

我们可以编辑代码如下

float64 a
float64 b
string option
---
float64 result

这个.srv文件以---分隔为两部分，上面一部分包含a，b，option，下面一部分包含一个result.

在这里，上面部分是request的数据，下面部分是response的数据。

此案例中，我们要去做的就是，发送request，例如，a=3，b=3，option=*，那么server端接收到数据后，做a option b 的操作，即3 * 3，结果放到response中。

这个srv文件遵循一定规范的，每一行表示一种数据。前面是类型，后面是名称。和msg的规范一致

ros不只是提供了int64和string两种基本类型供我们描述，其实还有很多：

msg类型	C++对应类型	Python对应类型
bool	uint8_t	bool
int8	int8_t	int
int16	int16_t	int
int32	int32_t	int
int64	int64_t	int，long
uint8	uint8_t	int
uint16	uint16_t	int
uint32	uint32_t	int
uint64	uint64_t	int，long
float32	float	float
float64	float	float
string	std:string	str，bytes
time	ros:Time	rospy.Time
duration	ros::Duration	rospy.Duration

3. 配置package.xml文件

在package.xml种添加如下配置:

message_generation
message_runtime

message_generation是消息生成工具，在打包编译时会用到

message_runtime运行时消息处理工具

4. 配置CMakeLists.txt

在find_package添加message_generation，结果如下:

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rosmsg
  rospy
  message_generation
)

添加add_message_file，结果如下:

add_service_files(
        FILES
        NumOption.srv
)

这里的NumOption.srv要和你创建的srv文件名称一致，且必须时在srv目录下，否则编译会出现问题

添加generation_msg，结果如下:

generate_messages(
        DEPENDENCIES
        std_msgs
)

这个配置的作用是添加生成消息的依赖，默认的时候要添加std_msgs

修改catkin_package，结果如下:

catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES demo_msg
#  CATKIN_DEPENDS roscpp rosmsg rospy
#  DEPENDS system_lib
        CATKIN_DEPENDS message_runtime
)

为catkin编译提供了依赖message_runtime

检验自定义消息

1. 编译项目

来到工作空间目录下，运行编译

catkin_make

2. 查看生成的头文件

来到devel的include目录下，如果生成了头文件说明，自定义消息创建成功。

3. 通过rossrv工具校验

rossrv show demo_srvs/NumOption

查看运行结果，运行结果和自己定义的相一致，说明成功。

C++使用自定义消息

1. 自定义消息依赖的添加

在开发过程种，自定义消息是以一个package存在的，其他package需要用到这个自定义消息的package，是需要添加依赖的。

来到package.xml中，添加如下:

demo_srvs
demo_srvs
demo_srvs

来到CMakeLists.txt文件中，找到find_package，添加demo_msgs自定义消息依赖，添加结果如下：

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rosmsg
  rospy
  demo_srvs
)

2. 引入依赖

#include "demo_srvs/NumOption.h"

3. 构建消息

demo_srvs::NumOption service;
service.request.a = 10;
service.request.b = 5;
service.request.option = "+";

消息依赖加入后，具体类型是demo_srvs::NumOption

Python使用自定义消息

导入模块

from demo_srvs.srv import NumOption

python导入自定义消息模块，遵循一定的规范，from 模块名.srv import 具体的消息

请求为复杂对象

现在我们制定一个.srv文件，用来描述一项服务：发送学生信息到服务端，让服务端返回生成的学生Id。

新建GenStudentId.srv文件，内容如下:

Student student
---
string id

这里的request中，包含的是一个复杂数据类型Student，这种类型来源于前面demo_msgs包中定义的Student.msg。那么我们该如何正确构建这个srv？

首先，在package.xml中去添加demo_msgs包的依赖:

demo_msgs
demo_msgs
demo_msgs

接着，在CMakeLists.txt中添加需要查找的包demo_msgs:

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rosmsg
  rospy
  message_generation
  demo_msgs
)

然后，在generate_messages中添加demo_msgs如下:

generate_messages(
        DEPENDENCIES
        std_msgs
        demo_msgs
)

最后，在add_service_fils中，添加此文件：

add_service_files(
        FILES
        NumOption.srv
        GenStudentId.srv
)

以上操作完成后，我们只需要编译工作空间，那么在devel/include目录中就会生成对应的依赖。

案例:扫地机器人

实现分析

小乌龟是按照一定的方式进行移动的，在整个移动过程中，小乌龟都是有自己的路径的，路径的规划是需要考虑的。

路径规划的算法有很多，但是根本就是生成一系列的点，将点按照顺序连在一起就是路径和轨迹了，点越多，路径越清晰。

目前小乌龟案例中，我们要解决的是小乌龟由一个点到另外一个点的运动。

小乌龟运动的特点是，前进或是转角，前进控制了移动的距离，转角控制了移动的方向。

要控制小乌龟移动到具体的点，就是要解决前进和转向的问题

案例设计

通过指定目标点的X和Y值，让小乌龟到达指定点。

完整初始化代码

#include 
#include "ros/ros.h"

#include "QApplication"
#include "QtWidgets"

using namespace std;

void clickDone(QLineEdit *xEdit, QLineEdit *yEdit) {
    double distX = xEdit->text().toDouble();
    double distY = yEdit->text().toDouble();

    ROS_INFO_STREAM("distX: " << distX << "  distY:" << distY);
}

int main(int argc, char **argv) {
    string nodeName = "turtle_control";

    // 创建节点
    ros::init(argc, argv, nodeName, ros::init_options::AnonymousName);
    ros::NodeHandle node;

    // 创建Qt程序
    QApplication app(argc, argv);
    //窗体
    QWidget window;
    window.setWindowTitle("小乌龟控制器");
    window.resize(400, 0);
    //布局
    QFormLayout layout;
    window.setLayout(&layout);

    // 目标x坐标
    QLineEdit xEdit("10.54444");
    layout.addRow("x坐标", &xEdit);

    // 目标y坐标
    QLineEdit yEdit("5.54444");
    layout.addRow("y坐标", &yEdit);
    
    //当前姿态坐标信息
    QHBoxLayout poseLayout;
    layout.addRow(&poseLayout);

    QFormLayout xLayout;
    QLabel xLb;
    xLayout.addRow("x坐标", &xLb);
    poseLayout.addLayout(&xLayout);

    QFormLayout yLayout;
    QLabel yLb;
    yLayout.addRow("y坐标", &yLb);
    poseLayout.addLayout(&yLayout);

    QFormLayout thetaLayout;
    QLabel thetaLb;
    thetaLayout.addRow("角度", &thetaLb);
    poseLayout.addLayout(&thetaLayout);

    // 执行按钮
    QPushButton btnDone("执行");
    layout.addRow(&btnDone);

    // 事件设置
    btnDone.connect(&btnDone, &QPushButton::clicked, bind(&clickDone, &xEdit, &yEdit));

    window.show();

    return app.exec();
}

运动分析

小乌龟如何到达目标点?

数据获取

小乌龟当前的坐标和角度

可以通过订阅/turtle1/pose获得相关的信息

控制操作

通过设置小乌龟的线速度和角速度可以让小乌龟动起来

可以通过发布数据到/turtle1/cmd_vel控制小乌龟移动

示例代码

发布者与订阅者

//小乌龟控制地址
string velTopicName = "/turtle1/cmd_vel";
//小乌龟数据获得
string poseTopicName = "/turtle1/pose";
// 创建小乌龟移动发布者
ros::Publisher &&publisher = node.advertise(velTopicName, 1000);
// 创建小乌龟位置的订阅者
const ros::Subscriber &subscriber = node.subscribe(poseTopicName, 1000, poseCallback);

获取小乌龟实时位置信息的回调

void poseCallback(const turtlesim::Pose::ConstPtr &msg) {
    ROS_INFO_STREAM("x: " << msg->x);
    ROS_INFO_STREAM("y: " << msg->y);
    ROS_INFO_STREAM("theta: " << msg->theta);
    ROS_INFO_STREAM("linear: " << msg->linear_velocity);
    ROS_INFO_STREAM("angular: " << msg->angular_velocity);
    ROS_INFO_STREAM("degrees: " << msg->theta * 180 / M_PI);
    ROS_INFO_STREAM("=========================================");
}

直线运动计算

通过最简单的示例，先解决指线运动。

距离 = 速度 * 时间
速度 = 距离 / 时间

我们已知的是当前小乌龟的坐标turtle(x, y) ，和目标点dist(x, y)，我们要去得到是小乌龟的速度。

首先我们需要计算出两点间的距离：

distance = sqrt(pow(srcX - distX) + pow(srcY - distY))

其次我们需要确定的是时间time，我们可以给定一个预期的值。

我们可以将计算的结果进行运行测试。

测试发现，小乌龟默认运行的时间是1s。没有提供给我们设置时间接口。

解决时间不可控的问题

方案一:

ros::Rate rate(1);
for (int i = 0; i < 5; ++i) {
    //设置速度
    geometry_msgs::Twist twist;
    twist.linear.x = linearVel;
    twist.angular.z = 0;
    publisher.publish(twist);

    rate.sleep();
 }

方案二：

double runDistance = 0;
while (runDistance < distance) {

    //设置速度
    geometry_msgs::Twist twist;
    twist.linear.x = linearVel;
    twist.angular.z = 0;
    publisher.publish(twist);

    rate.sleep();

    runDistance += linearVel / hz;
}

方案三:

while (calcDistance(srcX, srcY, distX, distY) > 0.1) {
      // 获取srcX，srcY
      srcX = pose->x;
      srcY = pose->y;

      //设置速度
      geometry_msgs::Twist twist;
      twist.linear.x = linearVel;
      twist.angular.z = 0;
      publisher.publish(twist);
      rate.sleep();
}

思考：是否存在完美的事物

• 速度是否是绝对平均
• 距离差值是否是绝对为0
• 时间是否绝对为预期

什么是控制系统

人其实也是一个复杂的控制系统，体温，血压，ph值等…

人： 走进很热的房间，体温升高， sensor皮肤表面，感觉到热，controller下丘脑释放神经胆碱，你开始出汗。水分蒸发带走热量，体温回到正常。

电梯：当用户选择楼层后，电梯会在指定楼层停靠。

汽车，地铁，自动门，飞机定速巡航，自动导航都需要用到控制系统。

开环控制(Open Loop Control)

根据你选定的时间，衣服类型进行控制， 清洗不是根据衣服的干净程度控制。 一旦洗衣机开始运行，时间到了就停止工作，不管衣服是不是清洗干净了。

闭环控制

变频空调就采用了闭环控制。

位式控制算法

早期的空调采用的算法是位式控制算法。

位式控制的算法，输出信号只有两种，True和False

依据比较（sv和pv）

缺点：pv总是在sv的值上下波动

原因：控制对象具有惯性，空调降温时是慢慢往下降的，某个时刻自动断电，如果温度超出设定值，又自动的供电进行制冷。

PID算法

空调的变频算法其实就是PID算法。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-tJVawh1F-1581315862500)(img/PID算法.png)]

proportional（比例）， integral（积分），derivative（微分）

对位式控制的优化

1. 位式控制，只考虑当前。
2. pid不仅考虑当前，还要考虑历史
3. pid输出更多样，更平滑。不是是true或者false，

变频空调的P控制：

error是误差。

k是一个系数。

设置值的温度和当前温度存在误差，我们可以设置这个空调的温度为 当前温度减去 误差值(或者给个系数)，长久来说，当前温度会在设定温度上下浮动。

变频空调的I控制：

I算法主要是记录历史的错误，将这些错误进行累加。

在P控制中，当前温度是在设定温度上下摆动的，因此，误差应该是有正有负，全部累加到一起，正负抵消，剩下的得到的是总误差值。

让当前温度+总误差（或者给定系数），让当前温度趋向于设定温度。

变频空调的D控制

D算法主要是记录当前误差和最近一次的误差，

两次误差比较结果为0，说明已经到达设置水平。

两次误差比较结果绝对值大，说明往反方向走，温度没有趋向于设置的值，反而还在背离。

让当前温度+两次误差比较结果（给定系数），可以收敛误差，让温度值更趋向于设定温度。

krqt_Plot使用

rosrun rqt_plot rqt_plot

运行plot可视化界面。

设置侦听的topic地址，接收绘制。

小乌龟的PID实现

P控制逻辑

//计算当前距离目标点的距离
double linearDistance = calcDistance(srcX, srcY, distX, distY);
//计算剩下了的平均速度
double linearTargetVel = linearDistance / time;

//计算速度误差
double linearError = linearTargetVel - linearVel;

Kp值为0.1时，速度的趋势

Kp值为0.4时，速度的趋势

Kp值为0.7时，速度的趋势

I控制逻辑

linearTotalError += linearError;

D控制逻辑

double linearDeltaError = linearError - linearLastError;
linearLastError = linearError;

PID口诀

参数整定找最佳，从小到[大顺序查 先是比例后积分，最后再把微分加 曲线振荡很频繁，比例度盘要放大 曲线漂浮绕大湾，比例度盘往小扳 曲线偏离回复慢，积分时间往下降 曲线波动周期长，积分时间再加长 曲线振荡频率快，先把微分降下来 动差大来波动慢。微分时间应加长 理想曲线两个波，前高后低4比1 一看二调多分析，调节质量不会低.