Jiangeng Sun | ROS学习笔记 03 ROS通信编程

前言

系统环境: Ubuntu 18.04，硬盘安装。

ROS版本: melodic

深蓝学院（古月居）胡春旭ROS理论与实践笔记整理。

1. ROS开发流程

ROS开发流程一共有六个步骤：

ROS开发流程

如果是python代码则不需要配置编译规则。

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

2.1 创建工作空间

工作空间（workspace）是一个存放工程开发相关文件的文件夹。包含以下四种区：

src：代码空间(Source Space)
build：编译空间
devel：开发空间
install：安装空间

创建工作空间的指令如下:

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

编译工作空间的指令如下：

$ cd ~/catkin_ws/
$ catkin_make

设置环境变量的指令如下：

$ source devel/setup.bash

检查环境变量的指令如下：

$ echo $ROS_PACKAGE_PATH

在检查环境变量后，应该生成一个带冒号的地址。例如:

检查环境变量的脚本返回

为了不每次打开终端都设置环境变量（即都执行一遍$ source devel/setup.bash），可以把这句话添加到系统环境变量.bashrc中。

通过$ sudo vim ~/.bashrc打开.bashrc文件。
在文件最后添加：source ~/catkin_ws/devel/setup.bash
保存，关闭。

install空间的创建:

$ catkin_make install

2.2 创建功能包

创建功能包的命令:

$ cd ~/catkin_ws/src
# 创建一个名叫learning_communication的功能包
# 这个功能包实现的接口有python接口，C++接口，标准定义的消息与服务接口
$ catkin_create_pkg learning_communication rospy roscpp std_msgs std_srvs

编译功能包的命令：

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

注意：共一个工作空间下，不允许存在同名功能包。不同工作空间下，允许存在同名功能包。

功能包创建好后，就会产生两个文件CMakeLists.txt和package.xml：

功能包文件

package.xml可以自定义这个功能包的说明，声明了包的依赖。

CMakeLists.txt描述了这个功能包的编译规则。

剩下的三步：创建源代码，配置编译规则，编译与运行。

3. ROS Topic通信编程

3.1 发布/订阅”Hello world”

基于话题模型通信，信息类型是string。话题模型如下:

话题模型（发布/订阅）

3.1.1 创建源代码

发布者：

/**
 * 该例程将发布chatter话题，消息类型String
 */

// 字符串的流的数据转换
#include <sstream>
// 包含ros头文件，一般都会写
#include "ros/ros.h"
// 后面会用到ROS中string的消息类型
#include "std_msgs/String.h"

int main(int argc, char **argv)
{
    // ROS节点初始化，声明节点名，这个名字不能和其他节点重名
    ros::init(argc, argv, "string_publisher");

    // 创建节点句柄，完成ROS核心资源的管理。
    // 在节点初始化和关闭一节中,使用句柄管理节点的内部引用,使启动和关闭一个节点变得简单。

    ros::NodeHandle n;

    // 创建一个Publisher，发布名为chatter的topic，消息类型为std_msgs::String。
    // 在ros::Publisher类下实例化一个chatter_pub对象。
    // 1000表示缓冲区大小，超过1000会把时间戳最老的删除掉。
    // advertise( ) 返回一个 Publisher 对象。 通过调用对象的 publish( )函数可以在这个topic上发布 message。
    ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
	
    // 设置循环的频率
    // loop_rate()是ros::Rate类可定制频率的函数，接受一个int型参数，通过调用主函数末尾的上来sleep（）函数，将该参数置为发布频率。
    ros::Rate loop_rate(10);

    int count = 0;
    
    // ros::ok()会设置一个SIGINT监听，函数返回True，仅当以下四种情况发生时，该函数返回False：
　　 // 1. SIGINT被触发（Ctrl+c）
　　 // 2.被另一同名节点踢出
　　 // 3.函数ros::shutdown（）被程序另一部分调用
　　 // 4.程序所有句柄被销毁
    while (ros::ok())
    {
        // 初始化std_msgs::String类型的消息
        std_msgs::String msg;
        // 初始化字符串流
        std::stringstream ss;
        // 把信息放到字节流里
        ss << "hello world " << count;
        // msg中的data是存储信息的，直接赋值
        msg.data = ss.str();

        // 发布消息
        // 输出一个字符串变量,ROS_INFO其实就是printf
        // c_str()：将C++的string转化为C的字符串数组，c_str()生成一个const char *指针，指向字符串的首地址。
        ROS_INFO("%s", msg.data.c_str());
        // 通过调用对象的 publish( )函数发布数据
        chatter_pub.publish(msg);

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

    return 0;
}

如何实现一个发布者:

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

订阅者：

/**
 * 该例程将订阅chatter话题，消息类型String
 */
 
// ros头文件
#include "ros/ros.h"
// 后面会用到ROS中string的消息类型
#include "std_msgs/String.h"

// 接收到订阅的消息后，会进入消息回调函数
// 定义一个指针常量的引用msg
void chatterCallback(const std_msgs::String::ConstPtr& msg)
{
    // 将接收到的消息打印出来
    ROS_INFO("I heard: [%s]", msg->data.c_str());
}

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

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

    // 创建一个Subscriber，订阅名为chatter的topic，注册回调函数chatterCallback
    ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);

    // 循环等待回调函数
    // spin本身的循环条件就是ros::ok()
    ros::spin();

    return 0;
}

如何实现一个订阅者：

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

3.1.2 配置编译规则

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

打开CMakeList.txt，添加：

# 设置需要编译的代码和生成的可执行文件
# 让string_publish.cpp这个cpp文件编译生成string_publish
add_executable(string_publisher src/string_publisher.cpp)
# 设置string_publisher与库catkin_LIBRARIES的连接
target_link_libraries(string_publisher ${catkin_LIBRARIES})

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

3.1.3 编译与运行

编译的过程就是把xxx.cpp变成xxx可运行程序的过程。

打开Terminal：

$ cd ~/catkin_ws
$ catkin_make

# 配置系统环境变量后这步可以省略
$ source devel/setup.bash

$ roscore
$ rosrun learning_communication string_publisher
$ rosrun learning_communication string_subscriber

3.2 发布/订阅自定义数据类型”person”

基于话题模型通信，信息类型是person（自定义的“person”的类型）。话题模型如下:

话题模型(发布/订阅)

3.2.1 自定义话题消息

定义msg文件：
1. 创建msg文件夹
2. touch PersonMsg.msg创建PersonMsg.msg文件,并输入:

string name
uint8 age
uint8 sex

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

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

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

在CMakeLists.txt对应的注释下添加编译选项：
1. find_package(…… message_generation)
2. 添加：add_message_files(FILES PersonMsg.msg)和generate_messages(DEPENDENCIES std_msgs)（前一句话是编译生成头文件，后一句话是调用了ros的标准数据类型）
3. catkin_package(…… message_runtime)
编译接口: catkin_make
编译成功后，devel/include/learning_communication中会动态产生了一个PersonMsg.h头文件。

3.2.2 创建源代码

发布者：

/**
 * 该例程将发布/person_info话题，learning_communication::PersonMsg
 */
 
#include <ros/ros.h>
#include "learning_communication/PersonMsg.h"

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

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

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

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

    int count = 0;
    while (ros::ok())
    {
        // 初始化learning_communication::Person类型的消息
    	learning_communication::PersonMsg person_msg;
        // 之前的例子中string使用了流来赋值，这个例子直接赋值
		person_msg.name = "Tom";
		person_msg.age  = 18;
        // 调用了uint8 male    = 1，也就是把sex赋值为1。
		person_msg.sex  = learning_communication::PersonMsg::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;
}

接收者：

/**
 * 该例程将订阅/person_info话题，自定义消息类型learning_communication::PersonMsg
 */
 
#include <ros/ros.h>
#include "learning_communication/PersonMsg.h"

// 接收到订阅的消息后，会进入消息回调函数
// learning_communication::PersonMsg就是编译好后自动生成的那个头文件
void personInfoCallback(const learning_communication::PersonMsg::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;
}

3.2.3 配置编译规则

打开CMakeList.txt，

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

因为需要动态产生头文件PersonMsg.h，所以还要

添加动态依赖

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

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

3.2.4 编译与运行

$ cd ~/catkin_ws
$ catkin_make

# 配置系统环境变量后这步可以省略
$ source devel/setup.bash

$ roscore
$ rosrun learning_communication person_publisher
$ rosrun learning_communication person_subscriber

4. ROS Service通信编程

4.1 请求/服务”Hello world”

基于服务模型通信，信息类型是string。服务模型如下:

服务模型（服务端/客户端）

4.1.1 创建源代码

服务器：

/**
 * 该例程将提供print_string服务，std_srvs::SetBool
 */
 
#include "ros/ros.h"
// 标准定义的服务类型
#include "std_srvs/SetBool.h"

// service回调函数，输入参数req，输出参数res
bool print(std_srvs::SetBool::Request  &req,
         std_srvs::SetBool::Response &res)
{
    // 打印字符串
    if(req.data)
	{
		ROS_INFO("Hello ROS!");
		res.success = true;
		res.message = "Print Successully";
	}
	else
	{
		res.success = false;
		res.message = "Print Failed";
	}

    return true;
}

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

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

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

    // 循环等待回调函数
    ROS_INFO("Ready to print hello string.");
    ros::spin();

    return 0;
}

其中，SetBool类包含request和response（三个横线上和三个横线下），具有三个参数：

SetBool

bool data：客户端request内容。

bool succesee和string message：服务端response内容。

客户端：

/**
 * 该例程将请求print_string服务，std_srvs::SetBool
 */
 
#include "ros/ros.h"
#include "std_srvs/SetBool.h"

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

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

    // 创建一个client，service消息类型是std_srvs::SetBool
    ros::ServiceClient client = n.serviceClient<std_srvs::SetBool>("print_string");

    // 创建std_srvs::SetBool类型的service消息
    std_srvs::SetBool srv;
    srv.request.data = true;

    // 发布service请求，等待应答结果
    // client.call(srv)阻塞函数
    if (client.call(srv))
    {
        ROS_INFO("Response : [%s] %s", srv.response.success?"True":"False", 
									   srv.response.message.c_str());
    }
    else
    {
        ROS_ERROR("Failed to call service print_string");
        return 1;
    }

    return 0;
}

如何实现一个服务器：

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

4.1.2 配置编译规则

打开CMakeList.txt，添加：

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

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

4.1.3 编译与运行

$ cd ~/catkin_ws
$ catkin_make

# 配置系统环境变量后这步可以省略
$ source devel/setup.bash

$ roscore
$ rosrun learning_communication string_server 
$ rosrun learning_communication string_client

4.2 请求/服务自定义数据类型”person”

基于服务模型通信，信息类型是person。服务模型如下:

服务模型（服务端/客户端）

4.2.1 自定义服务类型

定义srv文件：
1. 创建srv文件夹
2. touch PersonSrv.srv创建PersonSrv.srv文件,并输入:

string name
uint8 age
uint8 sex

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

---
string result

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

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

在CMakeLists.txt对应的注释下添加编译选项：
1. find_package(…… message_generation)
2. 添加：add_service_files(FILES PersonSrv.srv)和generate_messages(DEPENDENCIES std_msgs)（前一句话是编译生成头文件，后一句话是调用了ros的标准数据类型）
3. catkin_package(…… message_runtime)
编译接口: catkin_make
编译成功后，devel/include/learning_communication中会动态产生了一个PersonSrv.h，PersonSrvRequest.h，PersonSrvResponse.h三个头文件。

4.2.2 创建源代码

服务器:

/**
 * 该例程将执行/show_person服务，服务数据类型learning_communication::PersonSrv
 */
 
#include <ros/ros.h>
#include "learning_communication/PersonSrv.h"

// service回调函数，输入参数req，输出参数res
bool personCallback(learning_communication::PersonSrv::Request  &req,
         			learning_communication::PersonSrv::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_communication::PersonSrv
 */

#include <ros/ros.h>
#include "learning_communication/PersonSrv.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_communication::PersonSrv>("/show_person");

    // 初始化learning_communication::Person的请求数据
	learning_communication::PersonSrv srv;
	srv.request.name = "Tom";
	srv.request.age  = 20;
	srv.request.sex  = learning_communication::PersonSrv::Request::male;

    // 请求服务调用
	ROS_INFO("Call service to show person[name:%s, age:%d, sex:%d]", 
			 srv.request.name.c_str(), srv.request.age, srv.request.sex);

	person_client.call(srv);

	// 显示服务调用结果
	ROS_INFO("Show person result : %s", srv.response.result.c_str());

	return 0;
};

4.2.3 配置编译规则

打开CMakeList.txt，添加：

注意添加了动态依赖。

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.2.4 编译与运行

$ cd ~/catkin_ws
$ catkin_make

# 配置系统环境变量后这步可以省略
$ source devel/setup.bash

$ roscore
$ rosrun learning_communication person_server
$ rosrun learning_communication person_client