文章目录
1 Client library与roscpp
1.1 Client Library简介
ROS 为机器人开发者们提供了不同语言的编程接口,比如 C++ 接口叫做 roscpp,Python 接口叫做 rospy,Java 接口叫做 rosjava。 尽管语言不同,但这些接口都可以用来创建 topic、service、param,实现 ROS 的通信功能。Clinet Lirary 有点类似开发中的 Helper Class,把一些常用的基本功能做了封装。
从开发客户端库(所谓客户端库,简单的理解就是一套接口)的角度看,一个客户端库,至少需要能够包括master注册、名称管理、消息收发等功能。这样才能给开发者提供对ROS通信架构进行配置的方法。
整个ROS包括的packages如下,
1.2 roscpp
roscpp位于 /opt/ros/kinetic
之下,用C++实现了ROS通信。在ROS中,C++的代码是通过catkin这个编译系统(扩展的CMake)来进行编译构建的。可以简单理解为,roscpp就是一个C++库,我们创建一个Cmake工程,在其中include了roscpp等ROS的libraries,这样就可以在工程中使用ROS提供的函数了。
通常我们要调用ROS的C++接口,首先就需要 #include <ros/ros.h>
。
roscpp的主要部分包括:
- ros::init():解析传入的ROS参数,创建node第一步需要用到的函数
- ros::NodeHandle:和topic、service、param等交互的公共接口
- ros::master : 包含从master查询信息的函数
- ros::this_node:包含查询这个进程(node)的函数
- ros::service:包含查询服务的函数
- ros::param:包含查询参数服务器的函数,而不需要用到NodeHandle
- ros::names:包含处理ROS图资源名称的函数
以上功能可以分为以下几类:
- Initialization and Shutdown 初始与关闭
- Topics 话题
- Services 服务
- Parameter Server 参数服务器
- Timers 定时器
- NodeHandles 节点句柄
- Callbacks and Spinning 回调和自旋(或者翻译叫轮询?)
- Logging 日志
- Names and Node Information 名称管理
- Time 时钟
- Exception 异常
看了这么多接口,是不是感觉很复杂,但我们日常开发并不会用到所有的功能,只需对要有一些印象,掌握几个比较常见和重要的用法就足够了。
2 节点初始化、关闭与NodeHandle
当执行一个ROS程序,就被加载到了内存中,就成为了一个进程,在ROS里叫做节点。每一个ROS的节点尽管功能不同,但都有必不可少的一些步骤,比如初始化、销毁,需要通行的场景通常都还需要节点的句柄。
1、初始化节点
对于一个C++写的ROS程序,主要有两层工作:
- 调用了
ros::init()
函数,从而初始化节点的名称和其他信息,一般我们ROS程序一开始都会以这种方式开始。 - 创建
ros::NodeHandle
对象,也就是节点的句柄,它可以用来创建Publisher、Subscriber以及做其他事情。
NodeHandle就是对节点资源的描述,有了它,你就可以操作这个节点,比如为程序提供服务、监听某个topic上的消息、访问和修改param等等。
2、关闭节点
通常我们要关闭一个节点可以直接在终端上按 Ctrl+C
,系统会自动触发SIGINT句柄来关闭这个进程。 你也可以通过调用 ros::shutdown()
来手动关闭节点,但通常我们很少这样做。如果需要判断节点状态,可以通过 ros::ok()
来判断节点状态是否正常。
下面这段代码是最常见的一个ROS程序的执行步骤,通常要启动节点,获取句柄,而关闭的工作系统自动帮我们完成,如果有特殊需要你也可以自定义。
#include <ros/ros.h>
int main(int argc,char ** argv)
{
ros::init(argc, argv, "your_node_name");
ros::NodeHandle nh;
// …… 节点功能
ros::spin(); // 用于出发topic、service的响应队列
return 0;
}
3、NodeHandle常用成员函数
NodeHandle是Node的句柄,用来对当前节点进行各种操作。在ROS中,NodeHandle是一个定义好的类,通过 include<ros/ros.h>
,我们可以创建这个类,以及使用它的成员函数。
// 创建话题的publisher
ros::Publisher advertise(const string &topic, uint32_t queue_size, bool latch=false);
// 第一个参数为发布话题的名称
// 第二个参数是消息队列的最大长度,如果发布的消息超过这个长度而没有被接收,那么旧消息就会出队。通常设为一个较小的数即可
/* 第三个参数是是否锁存。某些话题并不是会以某个频率发布,比如/map这个topic,
只有在初次订阅或者地图更新两种情况下,/map才会发布消息。这里就用到了锁存。 */
// 创建话题的subscriber
ros::Subsriber subscribe(const string &topic, uint32_t queue_size, void(*)(M));
// 第一个参数是订阅话题的名称
// 第二个参数是订阅队列的长度,如果接收到的消息没来得及处理,那么新消息入队,旧消息就会出队
//第三个参数是回调函数指针,指向回调函数来处理接收到的消息
// 创建服务的server,提供服务
ros::ServiceServer advertiseService(const string &service, bool(*srv_func)(Mreq &, Mres&));
// 第一个参数是service名称
// 第二个参数是服务函数的指针,指向服务函数。指向的函数应该有两个参数,分别接受请求和响应。
// 创建服务的client
ros::ServiceClient serviceClient(const string &service_name, bool persistent=false);
//第一个函数式service名称
//第二个参数用于设置服务的连接是否持续,如果为true,client将会保持与远程主机的连接,这样后续的请求会快一些。通常我们设为flase
// 查询某个参数的值
bool getParam(const string &key, std::string &s);
bool getParam(const std::string &key, double &d) const;
bool getParam(const std::string &key, int &i) const;
// 从参数服务器上获取key对应的值,已重载了多个类型
// 给参数赋值
void setParam(const std::string &key, const std::string &s) const;
void setParam(const std::string &key, const char *s) const;
void setParam(const std::string &key, int i) const;
// 给key对应的val赋值,重载了多个类型的val
可以看出,NodeHandle对象在ROS C++程序里非常重要,各种类型的通信都需要用NodeHandle来创建完成。具体可以参考:话题与服务相关对象。
3 Topic in roscpp
3.1 Topic通信
Topic是ROS里一种异步通信的模型,一般是节点间分工明确,有的只负责发送,有的只负责接收处理。对于绝大多数的机器人应用场景,比如传感器数据收发,速度控制指令的收发,Topic模型是最适合的通信方式。
为了讲明白topic通信的编程思路,我们以 learing_topic
为例,该程序包含以下部分:自定义了一个类型为person的消息(包括姓名、年龄和性别信息),一个node以一定频率发布的person消息,另一个node接收并处理。源代码见: learning_topic。
3.2 创建Person消息
自定义Person消息,在msg路径下创建 Person.msg
:
string name
uint8 age
uint8 sex
uint8 unknow = 0
uint8 male = 1
uint8 female =2
这里可以将定义的Person类型的消息,理解成一个C语言中的结构体,在程序中对一个Person消息进行创建修改的方法和对结构体的操作一样。创建了.msg文件之后,需要修改 CMakeLists.txt
和 package.xml
,从而让系统能够编译自定义消息。在 CMakeLists.txt
中添加编译选项:
find_package(catkin REQUIRED COMPONENTS
geometry_msgs
roscpp
rospy
std_msgs
message_generation # 需要添加 message_generation,必须有 std_msgs
)
add_messages_files(
FILES
Person.msg
) # catkin在cmake之上新增的命令,指定从哪个消息文件生成
# 添加生成消息时的依赖
generate_messages(
DEPENDENCIES
std_msgs
) # catkin新增的命令,用于生成消息
# DEPENDENCIES后面指定生成msg需要依赖其他什么消息,由于Person.msg用到了uint8这种ROS标准消息,因此需要再把std_msgs作为依赖
# 添加执行时的依赖
ctakin_packages(
# INCLUDE_DIRS include
# LIBRARIES learning_topic
CATKIN_DEPENDS geometry_msgs roscpp rospy std_msgs message_runtime
# 官网没有在 catkin_package 中配置 message_runtime,经测试配置也可以
# DEPENDS system_lib
)
在 package.xml
中添加功能包依赖:
<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>
<!-- exce_depend 以前对应的是 run_depend 现在非法 -->
当完成了以上所有工作,就可以回到工作空间,然后编译。编译完成之后会在devel路径下生成 Person.msg
对应的头文件,头文件按照C++的语法规则定义了 learning_topic::Person
类型的数据。
要在代码中使用自定义消息类型,只要 #include <learning_topic/Person.h>
,然后声明,按照对结构体操作的方式修改内容即可。
learning_topic::Person person_msg;
personmsg.name = "xiaoming"
person_msg.age = 20;
person_msg.sex = learning_topic::Person::male;
3.3 消息发布节点(publisher)
定义完了消息,就可以开始写ROS代码了。通常我们会把消息收发的两端分成两个节点来写,一个节点就是一个完整的C++程序。详见: learning_topic/src/person_publisher.cpp
/****
*****需求:该例程将发布/person_info话题,自定义消息类型learning_topic::Person
*****发布者实现:
***************1.包含头文件
***************2.初始化ROS节点
***************3.创建ROS句柄
***************4.创建发布对象,并向ROS Master注册节点信息,包括发布的话题名和话题中的消息类型
***************5.创建消息数据
***************6.按照一定频率循环发布消息
*/
#include <ros/ros.h>
#include "learning_topic/Person.h" // 自定义msg产生的头文件
int main(int arga, char **argv)
{
ros::init(argc, argv, "person_publisher"); // 用于解析ROS参数,第三个参数为本节点名
ros::NodeHandle n; // 实例化句柄,初始化node
// 创建一个Publisher,发布名为/person_info的话题,消息类型为learning_topic::Person,队列长度10
ros::Publisher person_info_pub = n.advertise<learning_topic::Person>("/person_info", 10);
ros::Rate loop_rate(1); // 设置循环频率,1Hz
int count = 0;
while(ros::ok()) // 循环发布msg
{
// 初始化learning_topic::Person类型的消息
learning_topic::Person person_msg;
personmsg.name = "xiaoming"
person_msg.age = 20;
person_msg.sex = learning_topic::Person::male;
person_info_pub.publisher(person_msg); // 以1Hz的频率发布消息
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(); // 根据前面的定义的loop_rate,设置1s的暂停
}
return 0;
}
3.4 消息接收节点(subscriber)
详见 learning_topic/src/person_subscriber.cpp
/****
*****需求:该例程将发布/person_info话题,自定义消息类型learning_topic::Person
*****订阅者实现:
***************1.包含头文件
***************2.初始化ROS节点
***************3.创建ROS句柄
***************4.创建订阅对象,并向ROS Master注册节点信息,包括订阅的话题名和话题中的消息类型
***************5.循环等待话题消息,接收到消息后进入回调函数
***************6.在回调函数中完成消息处理
*/
#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::init(argc, argv, "person_subscriber"); // 初始化ROS节点
ros::NodeHandle n; // 创建节点句柄
ros::Subscriber person_info_sub = n.subscriber("/person_info", 10, personInfoCallback);
// 创建一个Subscriber,订阅名为/person_info的topic,注册回调函数personInfoCallback
ros::spin(); // ros::spin()循环等待回调函数,用于调用所有可触发的回调函数,将进入循环,不会返回,
// 类似于在循环里反复调用spinOnce(),而ros::spinOnce()只会去触发一次
return 0;
}
在topic接收方,有一个比较重要的概念,就是回调(CallBack),在本例中,回调就是预先给 /person_info
话题传来的消息准备一个回调函数,事先定义好回调函数的操作。只有当有消息来时,回调函数才会被触发执行。具体去触发的命令就是 ros::spin()
,它会反复的查看有没有消息来,如果有就会让回调函数去处理。
千万不要认为,只要指定了回调函数,系统就回去自动触发,你必须
ros::spin()
或ros::spinOnce()
才能真正使回调函数生效。
补充:回调函数与spin()方法,具体可以参考:回调函数、spin()。
3.5 CMakeLists.txt文件修改
配置 CMakeLists.txt
中的编译规则,首先需要设置编译的代码和生成的可执行文件;其次,设置链接里库;最后,添加依赖项,具体操作如下:
add_executable(person_publisher src/person_publisher.cpp) # 生成可执行文件person_publisher
target_link_libraries(person_publisher ${catkin_LIBRARIES}) # 链接
add_dependencies(person_publisher ${PROJECT_NAME}_generate_messages_cpp)
# 在编译person_publisher前,必须先编译完成自定义的消息,必须添加add_dependencies,否则找不到自定义的msg产生的头文件
add_executable(person_subscriber src/person_subscriber.cpp) # 生成可执行文件person_subscriber
target_link_libraries(person_subscriber ${catkin_LIBRARIES}) # 链接
add_dependencies(person_subscriber ${PROJECT_NAME}_generate_messages_cpp)
到此为止,之后经过 catkin_make
,一个自定义消息+发布接收的基本模型就完成了。
4 service in roscpp
4.1 service通信
Service是一种请求—反馈的通信机制。请求的一方通常被称为客户端,提供服务的一方叫做服务器端。Service机制相比于Topic的不同之处在于:
- 消息的传输是双向的,有反馈的,而不是单一的流向。
- 消息往往不会以固定频率传输,不连续,而是在需要时才会向服务器发起请求。
在ROS中如何请求或者提供一个服务,我们来看 learning_service
的代码:一个节点发出服务请求(姓名,年龄,性别),另一个节点进行服务响应,答复请求。
4.2 创建Person服务
创建 learning_service/srv/Person.srv
文件,内容包括:
string name
uint8 age
uint8 sex
uint8 unknow = 0
uint8 male = 1
uint8 female = 2
# 短横线上边部分是服务请求的数据
---
# 短横线下面是服务回传的内容
string result
srv格式的文件创建后,也需要修改 CMakeLissts.txt
,
find_package(catkin REQUIRED COMPONENTS
geometry_msgs
roscpp
rospy
std_msgs
message_generation # 需要添加 message_generation,必须有 std_msgs
)
add_service_files(
FILES
Person.srv
) # catkin在cmake之上新增的命令,指定从哪个消息文件生成
# 添加生成消息时的依赖
generate_messages(
DEPENDENCIES
std_msgs
) # catkin新增的命令,用于生成消息
# DEPENDENCIES后面指定生成msg需要依赖其他什么消息,由于Person.msg用到了uint8这种ROS标准消息,因此需要再把std_msgs作为依赖
# 添加执行时的依赖
ctakin_packages(
# INCLUDE_DIRS include
# LIBRARIES learning_topic
CATKIN_DEPENDS geometry_msgs roscpp rospy std_msgs message_runtime
# 官网没有在 catkin_package 中配置 message_runtime,经测试配置也可以
# DEPENDS system_lib
)
在 package.xml
中添加编译依赖与执行依赖:
<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>
<!-- exce_depend 以前对应的是 run_depend 现在非法 -->
然后进行 catkin_make
,系统就会自动生成在代码中可用的Person类型。在代码中使用,只需要 #include <service_demo/Person.h>
,然后即可创建该类型的srv。
learning_service::Person srv; // srv分为srv.request和srv.response两部分
srv.request.name = "Tom"; // 不能用srv.name或者srv.age来访问
srv.request.age = 20;
srv.request.sex = learning_service::Person::Request::male;
新生成的Person类型的服务,其结构体的风格更为明显,可以这么理解,一个Person服务结构体中嵌套了两个结构体,分别是请求和响应:
struct Person
{
struct Request
{
string name;
int age;
}request;
struct Response
{
string result;
}response;
}
4.3 创建提供服务节点(server)
learning_service/src/person_server.cpp
内容如下:
/****
*****需求:该例程将执行/show_person服务,服务数据类型learning_service::Person
*****服务端实现:
***************1.包含头文件
***************2.初始化ROS节点
***************3.创建ROS句柄
***************4.创建服务对象
***************5.循环等待服务请求,进入回调函数
***************6.在回调函数中完成服务功能的处理,并反馈应答数据
*/
#include <ros/ros.h>
#include "learning_service/Person.h"
// service回调函数,输入参数req,输出参数res, bool 返回值由于标志是否处理成功
bool personCallback(learning_service::Person::Request &req, learning_service::Person::Response &res)
{
ROS_INFO("Person: name:%s age:%d sex:%d", req.name.c_str(), req.age, req.sex); // 显示请求数据
res.result = "OK"; // 处理请求,结果写入response,设置反馈数据
return true; // 返回true,正确处理了请求
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "person_server"); // 解析参数,命名节点
ros::NodeHandle n; // 创建节点句柄,实例化node
ros::ServiceServer person_service = n.advertiseService("/show_person", personCallback);
// 创建一个名为/show_person的server,注册回调函数personCallback
ROS_INFO("Ready to show person information."); // 循环等待回调函数
ros::spin(); // 由于请求有多个,需要调用 ros::spin()
return 0;
}
4.4 创建服务请求节点(client)
/****
*****需求:该例程将执行/show_person服务,服务数据类型learning_service::Person
*****客户端实现:
***************1.包含头文件
***************2.初始化ROS节点
***************3.创建ROS句柄
***************4.创建客户端对象
***************5.发布服务请求数据
***************6.等待Server处理之后的应答结果
*/
#include <ros/ros.h>
#include "learning_service/Person.h"
int main(int argc, char **argv)
{
ros::init(argc, argv, "person_client"); // 初始化ROS节点,命名为“person_client”
ros::NodeHandle node; // 创建节点句柄
// 等待服务启动成功,发现/show_person服务后,创建一个服务客户端,连接名为/show_person的service
ros::service::waitForService("/show_person");
ros::ServiceClient person_client = node.serviceClient<learning_service::Person>("/show_person"); // 定义service客户端,service名字为“/show_person”,service类型为learning-service
// 初始化learning_service::Person的请求数据
learning_service::Person srv;
srv.request.name = "Tom"; // 不能用srv.name或者srv.age来访问
srv.request.age = 20;
srv.request.sex = learning_service::Person::Request::male;
// 请求服务调用
ROS_INFO("Call service to show person[name:%s, age:%d, sex:%d]", srv.request.name.c_str(), srv.request.age, srv.request.sex);
// 显示服务调用结果
person_client.call(srv);
ROS_INFO("Show person result: %s", srv.response.result.c_str());
return 0;
}
注意: 建立client需要用 nh.serviceClient<service_demo::Greeting>("greetings")
,指明服务的类型和服务的名称。而调用时可以直接用 client.call(srv)
,返回结果不是response,而是是否成功调用远程服务。
4.5 CMakeLists.txt文件修改
配置 CMakeLists.txt
中的编译规则,和话题模型的配置基本类似,首先需要设置编译的代码和生成的可执行文件;其次,设置链接里库;最后,添加依赖项,具体操作如下:
add_executable(person_server src/person_server.cpp) # 生成可执行文件person_server
target_link_libraries(person_server ${catkin_LIBRARIES}) # 链接
add_dependencies(person_server ${PROJECT_NAME}_gencpp)
# 在编译person_server前,必须先编译完成自定义的服务,必须添加add_dependencies,否则找不到自定义的srv产生的头文件
add_executable(person_client src/person_client.cpp) # 生成可执行文件person_client
target_link_libraries(person_client ${catkin_LIBRARIES}) # 链接
add_dependencies(person_client ${PROJECT_NAME}_gencpp)
到此为止,之后经过 catkin_make
,一个自定义的服务模型就完成了。PS:如果先启动客户端,那么会导致运行失败。
5 Param in roscpp
5.1 Parameter Server
严格来说,param并不能称作一种通信方式,因为它往往只是用来存储一些静态的设置,而不是动态变化的。关于param的API,roscpp为我们提供了两套,一套是放在 ros::paramnamespace
下,另一套是在 ros::NodeHandle
下,这两套API的操作完全一样,用哪一个都可以。
具体使用可以参考:参数操作、Parameter in roscpp
6 时钟
6.1 Timer与Duration
ROS里经常用到的一个功能就是时钟,比如计算机器人移动距离、设定一些程序的等待时间、设定计时器等等。roscpp里有两种时间的表示方法,一种是时刻(ros::Time),一种是时长(ros::Duration)。无论是Time还是Duration都具有相同的表示方法:
int32 sec
int32 nsec
Time/Duration都由秒和纳秒组成。 要使用Time和Duration,需要 #include <ros/time.h>
和 #include <ros/duration.h>
。具体可以参考:Time与Duration、时间。
6.2 sleep
通常在机器人任务执行中可能有需要等待的场景,这时就要用到sleep功能,roscpp中提供了两种sleep的方法:
ros::Duration(0.5).sleep(); // 用Duration对象的sleep方法休眠
ros::Rate r(10); // 频率10Hz
while(ros::ok())
{
r.sleep(); //定义好sleep的频率,Rate对象会自动让整个循环以10hz休眠,即使有任务执行占用了时间
}
6.3 Timer
Rate的功能是指定一个频率,让某些动作按照这个频率来循环执行。与之类似的是ROS中的定时器Timer,它是通过设定回调函数和触发时间来实现某些动作的反复执行,创建方法和topic中的subscriber很像。
void callback1(const ros::TimeEvent&)
{
ROS_INFO("Callback 1 triggered");
}
void callback2(const ros::TimeEvent&)
{
ROS_INFO("Callback 2 triggered");
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "person_subscriber");
ros::NodeHandle n;
ros::Timer timer1 = n.createTimer(ros::Duration(0.1), callback1); //timer1每0.1s触发一次callback1函数
ros::Timer timer2 = n.createTimer(ros::Duration(1.0), callback2); //timer1每1.0s触发一次callback2函数
ros::spin(); // 不要忘了spin,只有spin了才能真正去触发回调函数
return 0;
}
7 日志和异常
7.1 Log
ROS为开发者和用户提供了一套日志记录和输出系统,这套系统的实现方式是基于topic,也就是每个节点都会把一些日志信息发到一个统一的topic上去,这个topic就是 /rosout
。 rosout本身也是一个node,它专门负责进行日志的记录。我们在启动master的时候,系统就会附带启动rosout。
在roscpp中进行日志的输出,需要先 include <ros/console.h>
,这个头文件包括了五个级别的日志输出接口,分别是:
- DEBUG(调试):只在调试时使用,此类消息不会输出到控制台;
- INFO(信息):标准消息,一般用于说明系统内正在执行的操作;
- WARN(警告):提醒一些异常情况,但程序仍然可以执行;
- ERROR(错误):提示错误信息,此类错误会影响程序运行;
- FATAL(严重错误):此类错误将阻止节点继续运行。
具体可以参考:http://wiki.ros.org/roscpp/Overview/Logging
7.2 Exception
roscpp中有两种异常类型,当有以下两种错误时,就会抛出异常:
ros::InvalidNodeNameException // 当无效的基础名称传给ros::init(),通常是名称中有/,就会触发
ros::InvalidNameException // 当无效名称传给了roscpp