ros2中wait_for_message()函数实现方法

ros2中wait_for_message()函数实现方法

1. ​​小鱼​​听着歌说:

今天有鱼粉在群里问小鱼，如何在ros2中实现wait_for_message()函数？

在ros1中可以使用下面的程序,来获取一条指定话题的数据消息

msg = rospy.wait_for_message('话题名字',话题类型)

该函数实现原理为：创建一个订阅，接收一条消息，取消订阅
​​​ https://docs.ros.org/en/diamondback/api/rospy/html/rospy.client-module.html#wait_for_message​​
照着这个原理，我们可以写一个ROS2中相同的函数，但是需要注意ROS2中的rospy不是多线程的了，所以不能和ROS1中该函数实现一样，一直等待消息而不用spin()。

所以在ROS2中我们可以这样来实现这个函数

def wait_for_message(node ,topic_type, topic): 
    class _vfm(object):
        def __init__(self) -> None:
            self.msg = None
            
        def cb(self, msg):
            self.msg = msg

    vfm = _vfm()
    subscription = node.create_subscription(topic_type,topic,vfm.cb,1)
    while rclpy.ok():
        if vfm.msg != None: return vfm.msg
        rclpy.spin_once(node)
        time.sleep(0.001)
    # unsubcription
    subscription.destroy()

完整测试代码如下：

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from std_msgs.msg import String
import time

def wait_for_message(node ,topic_type, topic): 
    class _vfm(object):
        def __init__(self) -> None:
            self.msg = None
            
        def cb(self, msg):
            self.msg = msg

    vfm = _vfm()
    subscription = node.create_subscription(topic_type,topic,vfm.cb,1)
    while rclpy.ok():
        if vfm.msg != None: return vfm.msg
        rclpy.spin_once(node)
        time.sleep(0.001)
    # unsubcription
    subscription.destroy()

def main(args=None):
    rclpy.init(args=args)
    node = Node("test_message_node")
    msg = wait_for_message(node, String, "/test_sub")
    print(f'wait_for_message:{msg}')
    rclpy.spin(node)
    rclpy.shutdown()

if __name__ == '__main__':
    main()

ros2 topic pub /test_sub std_msgs/msg/String '{data: "123"}'