ROS2探索总结(十二)—— ROS2 Python编程基础

  • 内容
  • 评论
  • 相关

~欢迎关注~
微信公众号:古月居(guyue_home)
新浪微博:古月春旭(
https://weibo.com/hcx196
知乎专栏:古月居(
https://zhuanlan.zhihu.com/guyuehome
原文链接:
ROS2 Python编程基础

 

ROS最重要的特性之一就是多语言的支持,可以使用C++、Python等语言进行程序的开发,ROS2会继续强化这个特性,对更多语言提供丰富的支持。

这里我们尝试ROS2当中的Python编程,实现话题和服务通信。

代码链接:
https://github.com/ros2/demos
官方教程:
https://index.ros.org/doc/ros2/Tutorials/Python-Programming

在进行操作前,请先参考以下链接,完成ROS 2的安装和例程编译:

ROS 2发布第三个正式版——Crystal Clemmys

一、话题通信

代码编译后,在终端设置环境变量,并使用如下命令运行listener和talker:

  1. $ ros2 run demo_nodes_py listener
  2. $ ros2 run demo_nodes_py talker

运行成功后,可以看到如下效果:

Screenshot from 2019-01-13 23-26-40

以上talker和listener的具体实现,我们可以来分析下代码,分析内容请见代码中的注释:

  • talker.py
    1. # 引用python接口
    2. import rclpy
    3. from rclpy.node import Node
    4. from std_msgs.msg import String
    5.  
    6. class Talker(Node):
    7.  
    8.    def __init__(self):
    9.        super().__init__('talker')
    10.        self.i = 0
    11.  
    12.        # 创建一个Publisher,发布名为chatter的topic,消息类型为String
    13.        self.pub = self.create_publisher(String, 'chatter')
    14.  
    15.        # 设置定时器,周期调用定时器回调函数timer_callback
    16.        timer_period = 1.0
    17.        self.tmr = self.create_timer(timer_period, self.timer_callback)
    18.  
    19.    def timer_callback(self):
    20.        # 创建String类型的消息
    21.        msg = String()
    22.        msg.data = 'Hello World: {0}'.format(self.i)
    23.        self.i += 1
    24.        self.get_logger().info('Publishing: "{0}"'.format(msg.data))
    25.  
    26.        # 发布消息
    27.        self.pub.publish(msg)
    28.  
    29.  
    30. def main(args=None):
    31.    # ROS节点初始化
    32.    rclpy.init(args=args)
    33.  
    34.    # 创建节点及发布器
    35.    node = Talker()
    36.  
    37.    # 循环
    38.    rclpy.spin(node)
    39.  
    40.    # 销毁节点,退出程序
    41.    node.destroy_node()
    42.    rclpy.shutdown()
    43.  
    44.  
    45. if __name__ == '__main__':
    46.    main()

 

  • listener.py
  1. # 引用python接口
  2. import rclpy
  3. from rclpy.node import Node
  4. from std_msgs.msg import String
  5.  
  6. class Listener(Node):
  7.  
  8.    def __init__(self):
  9.        super().__init__('listener')
  10.  
  11.        # 创建一个subscriber,订阅名为chatter的topic
  12.        # 消息类型是String,回调函数为chatter_callback
  13.        self.sub = self.create_subscription(String, 'chatter', self.chatter_callback)
  14.  
  15.    def chatter_callback(self, msg):
  16.        # 将收到的消息数据打印出来
  17.        self.get_logger().info('I heard: [%s]' % msg.data)
  18.  
  19. def main(args=None):
  20.    # ROS节点初始化
  21.    rclpy.init(args=args)
  22.  
  23.    # 创建节点及订阅器
  24.    node = Listener()
  25.  
  26.    # 循环查询消息队列,收到消息后进入回调函数
  27.    rclpy.spin(node)
  28.  
  29.    # 销毁节点,退出程序
  30.    node.destroy_node()
  31.    rclpy.shutdown()
  32.  
  33. if __name__ == '__main__':
  34.    main()

二、服务通信

接下来我们看下ROS另外一种核心通信机制——服务的实现,运行以下命令实现ROS 1教程当中经典的加法求和服务例程:

  1. $ ros2 run demo_nodes_py add_two_ints_server
  2. $ ros2 run demo_nodes_py add_two_ints_client

可以看到如下效果:

Screenshot from 2019-01-13 23-27-57

add_two_ints_server和add_two_ints_client的具体实现,继续来分析代码:

  • add_two_ints_server.py
  1. # 引用python接口
  2. from example_interfaces.srv import AddTwoInts
  3. import rclpy
  4. from rclpy.node import Node
  5.  
  6. class AddTwoIntsServer(Node):
  7.  
  8.    def __init__(self):
  9.        super().__init__('add_two_ints_server')
  10.  
  11.        # 创建一个service服务端,提供名为AddTwoInts的service,
  12.        # 消息类型为AddTwoInts,回调函数为add_two_ints_callback
  13.        self.srv = self.create_service(AddTwoInts, 'add_two_ints', self.add_two_ints_callback)
  14.  
  15.    def add_two_ints_callback(self, request, response):
  16.        # 处理服务功能,将求和结果放入应答数据中
  17.        response.sum = request.a + request.b
  18.        self.get_logger().info('Incoming request\na: %d b: %d' % (request.a, request.b))
  19.  
  20.        return response
  21.  
  22. def main(args=None):
  23.    # ROS节点初始化
  24.    rclpy.init(args=args)
  25.  
  26.    # 创建服务端
  27.    node = AddTwoIntsServer()
  28.  
  29.    # 循环
  30.    rclpy.spin(node)
  31.  
  32.    # 销毁节点,退出程序
  33.    node.destroy_node()
  34.    rclpy.shutdown()
  35.  
  36. if __name__ == '__main__':
  37.    main()
  • add_two_ints_client.py
  1. # 引用python接口
  2. from example_interfaces.srv import AddTwoInts
  3. import rclpy
  4.  
  5. def main(args=None):
  6.    # ROS节点初始化
  7.    rclpy.init(args=args)
  8.  
  9.    # 创建节点
  10.    node = rclpy.create_node('add_two_ints_client')
  11.  
  12.    # 创建客户端
  13.    cli = node.create_client(AddTwoInts, 'add_two_ints')
  14.  
  15.    # 等待服务端
  16.    while not cli.wait_for_service(timeout_sec=1.0):
  17.        print('service not available, waiting again...')
  18.  
  19.    # 创建请求数据
  20.    req = AddTwoInts.Request()
  21.    req.a = 2
  22.    req.b = 3
  23.  
  24.    # 异步请求(非阻塞)
  25.    future = cli.call_async(req)
  26.  
  27.    # 等待服务端应答
  28.    rclpy.spin_until_future_complete(node, future)
  29.    if future.result() is not None:
  30.        node.get_logger().info('Result of add_two_ints: %d' % future.result().sum)
  31.    else:
  32.        node.get_logger().error('Exception while calling service: %r' % future.exception())
  33.  
  34.    # 销毁节点,退出程序
  35.    node.destroy_node()
  36.    rclpy.shutdown()
  37.  
  38. if __name__ == '__main__':
  39.    main()

三、QoS配置

ROS 2中的通信中间件改用DDS,而QoS是DDS中非常重要的一环,控制了各方面与底层的通讯机制,主要从时间限制、可靠性、持续性、历史记录几个方面,满足用户针对不同场景的数据应用需求。

以话题通信为例,我们看下Python程序中该如何配置QoS:

  • talker_qos.py
  1. import argparse
  2. import sys
  3.  
  4. # 引用python接口
  5. import rclpy
  6. from rclpy.node import Node
  7. from rclpy.qos import qos_profile_default, qos_profile_sensor_data
  8. from rclpy.qos import QoSReliabilityPolicy
  9.  
  10. from std_msgs.msg import String
  11.  
  12. class TalkerQos(Node):
  13.  
  14.    def __init__(self, qos_profile):
  15.        super().__init__('talker_qos')
  16.        self.i = 0
  17.  
  18.        # 传输模式:Reliable、Best effort
  19.        if qos_profile.reliability is QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_RELIABLE:
  20.            self.get_logger().info('Reliable talker')
  21.        else:
  22.            self.get_logger().info('Best effort talker')
  23.  
  24.        # 创建一个Publisher,发布名为chatter的topic,消息类型为String,设置qos参数
  25.        self.pub = self.create_publisher(String, 'chatter', qos_profile=qos_profile)
  26.  
  27.        # 设置定时器,周期调用定时器回调函数timer_callback
  28.        timer_period = 1.0
  29.        self.tmr = self.create_timer(timer_period, self.timer_callback)
  30.  
  31.    def timer_callback(self):
  32.        # 创建String类型的消息
  33.        msg = String()
  34.        msg.data = 'Hello World: {0}'.format(self.i)
  35.        self.i += 1
  36.        self.get_logger().info('Publishing: "{0}"'.format(msg.data))
  37.  
  38.        # 发布消息
  39.        self.pub.publish(msg)
  40.  
  41. def main(argv=sys.argv[1:]):
  42.    # 解析输入参数
  43.    parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter)
  44.  
  45.    # 是否使用relibale可靠传输模式
  46.    parser.add_argument(
  47.        '--reliable', dest='reliable', action='store_true',
  48.        help='set qos profile to reliable')
  49.    parser.set_defaults(reliable=False)
  50.  
  51.    # 循环发布次数
  52.    parser.add_argument(
  53.        '-n', '--number_of_cycles', type=int, default=20,
  54.        help='number of sending attempts')
  55.  
  56.    # 其他输入参数  
  57.    parser.add_argument(
  58.        'argv', nargs=argparse.REMAINDER,
  59.        help='Pass arbitrary arguments to the executable')
  60.    args = parser.parse_args(argv)
  61.  
  62.    # ROS节点初始化
  63.    rclpy.init(args=args.argv)
  64.  
  65.    # 设置qos参数
  66.    if args.reliable:
  67.        custom_qos_profile = qos_profile_default
  68.    else:
  69.        custom_qos_profile = qos_profile_sensor_data
  70.  
  71.    # 创建节点及发布器
  72.    node = TalkerQos(custom_qos_profile)
  73.  
  74.    # 循环
  75.    cycle_count = 0
  76.    while rclpy.ok() and cycle_count < args.number_of_cycles:
  77.        rclpy.spin_once(node)
  78.        cycle_count += 1
  79.  
  80.    # 销毁节点并退出
  81.    node.destroy_node()
  82.    rclpy.shutdown()
  83.  
  84. if __name__ == '__main__':
  85.    main()
  • listener_qos.py
  1. import argparse
  2. import sys
  3.  
  4. # 引用python接口
  5. import rclpy
  6. from rclpy.node import Node
  7. from rclpy.qos import qos_profile_default, qos_profile_sensor_data
  8. from rclpy.qos import QoSReliabilityPolicy
  9.  
  10. from std_msgs.msg import String
  11.  
  12. class ListenerQos(Node):
  13.  
  14.    def __init__(self, qos_profile):
  15.        super().__init__('listener_qos')
  16.  
  17.        # 传输模式:Reliable、Best effort
  18.        if qos_profile.reliability is QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_RELIABLE:
  19.            self.get_logger().info('Reliable listener')
  20.        else:
  21.            self.get_logger().info('Best effort listener')
  22.  
  23.        # 创建一个subscriber,订阅名为chatter的topic
  24.        # 消息类型是String,回调函数为chatter_callback,设置qos参数
  25.        self.sub = self.create_subscription(
  26.            String, 'chatter', self.chatter_callback, qos_profile=qos_profile)
  27.  
  28.    def chatter_callback(self, msg):
  29.        # 将收到的消息数据打印出来
  30.        self.get_logger().info('I heard: [%s]' % msg.data)
  31.  
  32.  
  33. def main(argv=sys.argv[1:]):
  34.    # 解析输入参数
  35.    parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter)
  36.  
  37.    # 是否使用relibale可靠传输模式
  38.    parser.add_argument(
  39.        '--reliable', dest='reliable', action='store_true',
  40.        help='set qos profile to reliable')
  41.    parser.set_defaults(reliable=False)
  42.  
  43.    # 循环订阅次数
  44.    parser.add_argument(
  45.        '-n', '--number_of_cycles', type=int, default=20,
  46.        help='max number of spin_once iterations')
  47.  
  48.  # 其他输入参数
  49.    parser.add_argument(
  50.        'argv', nargs=argparse.REMAINDER,
  51.        help='Pass arbitrary arguments to the executable')
  52.    args = parser.parse_args(argv)
  53.  
  54.    # ROS节点初始化
  55.    rclpy.init(args=args.argv)
  56.  
  57.    # 设置qos参数
  58.    if args.reliable:
  59.        custom_qos_profile = qos_profile_default
  60.    else:
  61.        custom_qos_profile = qos_profile_sensor_data
  62.  
  63.    # 创建节点及订阅器
  64.    node = ListenerQos(custom_qos_profile)
  65.  
  66.    # 循环
  67.    cycle_count = 0
  68.    while rclpy.ok() and cycle_count < args.number_of_cycles:
  69.        rclpy.spin_once(node)
  70.        cycle_count += 1
  71.  
  72.    # 销毁节点并退出
  73.    node.destroy_node()
  74.    rclpy.shutdown()
  75.  
  76. if __name__ == '__main__':
  77.    main()

ROS 2针对四种类型的通信,提供了QoS的默认配置:

  • Default QoS settings for publishers and subscriptions
    In order to make the transition from ROS1 to ROS2, exercising a similar network behavior is desirable. By default, publishers and subscriptions are reliable in ROS2, have volatile durability, and “keep last” history.
  • Services
    In the same vein as publishers and subscriptions, services are reliable. It is especially important for services to use volatile durability, as otherwise service servers that re-start may receive outdated requests.
  • Sensor data
    For sensor data, in most cases it’s more important to receive readings in a timely fashion, rather than ensuring that all of them arrive. That is, developers want the latest samples as soon as they are captured, at the expense of maybe losing some. For that reason the sensor data profile uses best effort reliability and a smaller queue depth.
  • Parameters
    Parameters are based on services, and as such have a similar profile. The difference is that parameters use a much larger queue depth so that requests do not get lost when, for example, the parameter client is unable to reach the parameter service server.

参考:http://design.ros2.org/articles/qos.html

以上例程运行时,默认使用的是Best effort模式:

Screenshot from 2019-01-14 15-52-41

可以通过命令输入切换成Reliable模式:

Screenshot from 2019-01-14 15-54-46

还可以设置发布或订阅的次数:

Screenshot from 2019-01-14 15-57-14

 

更多内容欢迎关注:

微信公众号:古月居 (guyue_home)

新浪微博:古月春旭 (https://weibo.com/hcx196)

知乎专栏:古月居 (https://zhuanlan.zhihu.com/guyuehome)


原创文章,转载请注明: 转载自古月居

本文链接地址: ROS2探索总结(十二)—— ROS2 Python编程基础

微信 OR 支付宝 扫描二维码
为本文作者 打个赏
pay_weixinpay_weixin