第3章 ROS通信机制进阶——常用API
3.1 常用API
首先，建议参考官方API文档或参考源码:
- ROS节点的初始化相关API;
- NodeHandle 的基本使用相关API;
- 话题的发布方，订阅方对象相关API;
- 服务的服务端，客户端对象相关API;
- 时间相关API;
- 日志输出相关API。
另请参考
3.1.1 初始化
C++
初始化
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
|
void init(int &argc, char **argv, const std::string& name, uint32_t options = 0);
ros::init(argc,argv,"name",ros::init_options::xxx)
|
传参数：
查看参数
节点名称需要保证唯一，会导致一个问题：同一个节点不能重复启动。
结果：ROS中当有重名的节点启动时，之前的节点会被关闭。
需求：特定场景下，需要一个节点多次启动且能正常运行，怎么办？
1
| ros::init(argc,argv,"name",ros::init_options::AnonymousName)
|
解决：设置启动项ros::init_options::AnonymousName
当创建ROS节点时，会在用户自定义的节点名称后缀随机数，从而避免重名问题。
Python
初始化
1
2
3
4
5
6
7
8
9
| def init_node(name, argv=None, anonymous=False, log_level=None, disable_rostime=False, disable_rosout=False, disable_signals=False, xmlrpc_port=
"""
在ROS msater中注册节点
@param name: 节点名称，必须保证节点名称唯一，节点名称中不能使用命名空间(不能包含 '/')
@type name: str
@param anonymous: 取值为 true 时，为节点名称后缀随机编号
@type anonymous: bool
"""
ros::init("name",anonymous=True) // 源码里如果没有设置argv会直接使用系统赋值
|
传参同上
3.1.2 话题与服务相关对象
C++
在 roscpp 中，话题和服务的相关对象一般由 NodeHandle 创建。 NodeHandle有一个重要作用是可以用于设置命名空间。
1.发布对象
对象获取：
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
|
template <class M>
Publisher advertise(const std::string& topic, uint32_t queue_size, bool latch = false)
|
1
2
3
4
5
|
template <typename M>
void publish(const M& message) const
|
2.订阅对象
对象获取：
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
|
template
Subscriber subscribe(const std::string& topic, uint32_t queue_size, void(*fp)(const boost::shared_ptr))
|
3.服务对象
对象获取：
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
|
template
ServiceServer advertiseService(const std::string& service, bool(*srv_func)(MReq&, MRes&))
|
4.客户端对象
对象获取：
1
2
3
4
5
6
7
8
9
|
template
ServiceClient serviceClient(const std::string& service_name, bool persistent = false, const M_string& header_values = M_string())
|
请求发送函数：
1
2
3
4
5
6
|
template<class Service>
bool call(Service& service)
|
等待服务函数1：
1
2
3
4
5
6
7
8
|
ROSCPP_DECL bool waitForService(const std::string& service_name, ros::Duration timeout = ros::Duration(-1));
|
等待服务函数2：
1
2
3
4
5
6
7
|
bool waitForExistence(ros::Duration timeout = ros::Duration(-1));
|
Python
1.发布对象
对象获取：
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
| class Publisher(Topic):
"""
在ROS master注册为相关话题的发布方
"""
def __init__(self, name, data_class, subscriber_listener=None,tcp_nodelay=False, latch=False,
headers=None, queue_size=None)
"""
Constructor
@param name: 话题名称
@type name: str
@param data_class: 消息类型
@param latch: 如果为 true,该话题发布的最后一条消息将被保存，并且后期当有订阅者连接时会将该消息发送给订阅者
@type latch: bool
@param queue_size: 等待发送给订阅者的最大消息数量
@type queue_size: int
"""
|
1
2
3
4
| def publish(self, *args, **kwds):
"""
发布消息
"""
|
2.订阅对象
对象获取：
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
| class Subscriber(Topic):
"""
类注册为指定主题的订阅者，其中消息是给定类型的。
"""
def __init__(self, name, data_class, callback=None, callback_args=None,
queue_size=None, buff_size=DEFAULT_BUFF_SIZE, tcp_nodelay=False):
"""
Constructor.
@param name: 话题名称
@type name: str
@param data_class: 消息类型
@type data_class: L{Message} class
@param callback: 处理订阅到的消息的回调函数
@type callback: fn(msg, cb_args)
@param queue_size: 消息队列长度，超出长度时，头部的消息将被弃用
"""
|
3.服务对象
对象获取：
1
2
3
4
5
6
7
8
9
10
11
12
13
14
| class Service(ServiceImpl):
"""
声明一个ROS服务
使用示例::
s = Service('getmapservice', GetMap, get_map_handler)
"""
def __init__(self, name, service_class, handler,
buff_size=DEFAULT_BUFF_SIZE, error_handler=None):
"""
@param name: 服务主题名称 ``str``
@param service_class:服务消息类型
@param handler: 回调函数，处理请求数据，并返回响应数据
@type handler: fn(req)->resp
"""
|
4.客户端对象
对象获取：
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
| class ServiceProxy(_Service):
"""
创建一个ROS服务的句柄
示例用法::
add_two_ints = ServiceProxy('add_two_ints', AddTwoInts)
resp = add_two_ints(1, 2)
"""
def __init__(self, name, service_class, persistent=False, headers=None):
"""
ctor.
@param name: 服务主题名称
@type name: str
@param service_class: 服务消息类型
@type service_class: Service class
"""
|
请求发送函数：
1
2
3
4
| def call(self, *args, **kwds):
"""
发送请求，返回值为响应数据
"""
|
等待服务函数：
1
2
3
4
5
6
7
8
| def wait_for_service(service, timeout=None):
"""
调用该函数时，程序会处于阻塞状态直到服务可用
@param service: 被等待的服务话题名称
@type service: str
@param timeout: 超时时间
@type timeout: double|rospy.Duration
"""
|
3.1.3 回旋函数
C++
在ROS程序中，频繁的使用了 ros::spin() 和 ros::spinOnce() 两个回旋函数，可以用于处理回调函数。
1.spinOnce()
1
2
3
4
5
6
7
8
|
ROSCPP_DECL void spinOnce();
|
2.spin()
1
2
3
4
|
ROSCPP_DECL void spin()
|
相同点：二者都用于处理回调函数；
不同点：ros::spin() 是进入了循环执行回调函数，而 ros::spinOnce() 只会执行一次回调函数(没有循环)，在 ros::spin() 后的语句不会执行到，而 ros::spinOnce() 后的语句可以执行。
Python
1
2
3
4
| def spin():
"""
进入循环处理回调
"""
|
3.1.4 时间
ROS中时间相关的API是极其常用，比如:获取当前时刻、持续时间的设置、执行频率、休眠、定时器… 都与时间相关。
C++
1.时刻
获取时刻，或是设置指定时刻:
1
2
3
4
5
6
7
8
9
10
| ros::init(argc,argv,"hello_time");
ros::NodeHandle nh;
ros::Time right_now = ros::Time::now();
ROS_INFO("当前时刻:%.2f",right_now.toSec());
ROS_INFO("当前时刻:%d",right_now.sec);
ros::Time someTime(100,100000000);
ROS_INFO("时刻:%.2f",someTime.toSec());
ros::Time someTime2(100.3);
ROS_INFO("时刻:%.2f",someTime2.toSec());
|
2.持续时间
设置一个时间区间(间隔):
1
2
3
4
5
| ROS_INFO("当前时刻:%.2f",ros::Time::now().toSec());
ros::Duration du(10);
du.sleep();
ROS_INFO("持续时间:%.2f",du.toSec());
ROS_INFO("当前时刻:%.2f",ros::Time::now().toSec());
|
3.持续时间与时刻运算
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
| ROS_INFO("时间运算");
ros::Time now = ros::Time::now();
ros::Duration du1(10);
ros::Duration du2(20);
ROS_INFO("当前时刻:%.2f",now.toSec());
ros::Time after_now = now + du1;
ros::Time before_now = now - du1;
ROS_INFO("当前时刻之后:%.2f",after_now.toSec());
ROS_INFO("当前时刻之前:%.2f",before_now.toSec());
ros::Duration du3 = du1 + du2;
ros::Duration du4 = du1 - du2;
ROS_INFO("du3 = %.2f",du3.toSec());
ROS_INFO("du4 = %.2f",du4.toSec());
|
4.设置运行频率
1
2
3
4
5
6
| ros::Rate rate(1);
while (true)
{
ROS_INFO("-----------code----------");
rate.sleep();
}
|
5.定时器
ROS 中内置了专门的定时器，可以实现与 ros::Rate 类似的效果:
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
| ros::NodeHandle nh;
bool oneshot = false,bool autostart = true) const;
ros::spin();
|
定时器的回调函数:
1
2
3
4
| void doSomeThing(const ros::TimerEvent &event){
ROS_INFO("-------------");
ROS_INFO("event:%s",std::to_string(event.current_real.toSec()).c_str());
}
|
Python
1.时刻
获取时刻，或是设置指定时刻:
1
2
3
4
5
6
7
8
9
10
11
12
13
|
right_now = rospy.Time.now()
rospy.loginfo("当前时刻:%.2f",right_now.to_sec())
rospy.loginfo("当前时刻:%.2f",right_now.to_nsec())
some_time1 = rospy.Time(1234.567891011)
some_time2 = rospy.Time(1234,567891011)
rospy.loginfo("设置时刻1:%.2f",some_time1.to_sec())
rospy.loginfo("设置时刻2:%.2f",some_time2.to_sec())
some_time3 = rospy.Time.from_sec(543.21)
rospy.loginfo("设置时刻3:%.2f",some_time3.to_sec())
|
2.持续时间
1
2
3
4
5
6
|
rospy.loginfo("持续时间测试开始.....")
du = rospy.Duration(3.3)
rospy.loginfo("du1 持续时间:%.2f",du.to_sec())
rospy.sleep(du)
rospy.loginfo("持续时间测试结束.....")
|
3.持续时间与时刻运算
1
2
3
4
5
6
7
8
9
10
11
12
| rospy.loginfo("时间运算")
now = rospy.Time.now()
du1 = rospy.Duration(10)
du2 = rospy.Duration(20)
rospy.loginfo("当前时刻:%.2f",now.to_sec())
before_now = now - du1
after_now = now + du1
dd = du1 + du2
rospy.loginfo("之前时刻:%.2f",before_now.to_sec())
rospy.loginfo("之后时刻:%.2f",after_now.to_sec())
rospy.loginfo("持续时间相加:%.2f",dd.to_sec())
|
4.运行频率
1
2
3
4
5
|
rate = rospy.Rate(0.5)
while not rospy.is_shutdown():
rate.sleep()
rospy.loginfo("+++++++++++++++")
|
5.定时器
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
|
"""
def __init__(self, period, callback, oneshot=False, reset=False):
Constructor.
@param period: 回调函数的时间间隔
@type period: rospy.Duration
@param callback: 回调函数
@type callback: function taking rospy.TimerEvent
@param oneshot: 设置为True，就只执行一次，否则循环执行
@type oneshot: bool
@param reset: if True, timer is reset when rostime moved backward. [default: False]
@type reset: bool
"""
rospy.Timer(rospy.Duration(1),doMsg)
rospy.spin()
|
回调函数：
1
2
3
| def doMsg(event):
rospy.loginfo("+++++++++++")
rospy.loginfo("当前时刻:%s",str(event.current_real))
|
3.1.5 其他函数
在发布实现时，一般会循环发布消息，循环的判断条件一般由节点状态来控制，C++中可以通过 ros::ok() 来判断节点状态是否正常，而 python 中则通过 rospy.is_shutdown() 来实现判断，导致节点退出的原因主要有如下几种:
- 点接收到了关闭信息，比如常用的 ctrl + c 快捷键就是关闭节点的信号；
- 同名节点启动，导致现有节点退出；
- 程序中的其他部分调用了节点关闭相关的API(C++中是ros::shutdown()，python中是 rospy.signal_shutdown())
另外，日志相关的函数也是极其常用的，在ROS中日志被划分成如下级别:
- DEBUG(调试):只在调试时使用，此类消息不会输出到控制台；
- INFO(信息):标准消息，一般用于说明系统内正在执行的操作；
- WARN(警告):提醒一些异常情况，但程序仍然可以执行；
- ERROR(错误):提示错误信息，此类错误会影响程序运行；
- FATAL(严重错误):此类错误将阻止节点继续运行。
C++
1.节点状态判断
2.节点关闭函数
3.日志函数
1
2
3
4
5
| ROS_DEBUG("hello,DEBUG");
ROS_INFO("hello,INFO");
ROS_WARN("Hello,WARN");
ROS_ERROR("hello,ERROR");
ROS_FATAL("hello,FATAL");
|
Python
1.节点状态判断
1
2
3
4
5
| def is_shutdown():
"""
@return: True 如果节点已经被关闭
@rtype: bool
"""
|
2.节点关闭函数
1
2
3
4
5
6
| def signal_shutdown(reason):
"""
关闭节点
@param reason: 节点关闭的原因，是一个字符串
@type reason: str
"""
|
1
2
3
4
5
6
| def on_shutdown(h):
"""
节点被关闭时调用的函数
@param h: 关闭时调用的回调函数，此函数无参
@type h: fn()
"""
|
3.日志函数
1
2
3
4
5
| rospy.logdebug("hello,debug")
rospy.loginfo("hello,info")
rospy.logwarn("hello,warn")
rospy.logerr("hello,error")
rospy.logfatal("hello,fatal")
|
