ROS2 한 노드에서 publish와 subscribe 동시에 동작시키기 (python)

ROS2를 공부하다가 한 노드에서 publish와 subscribe가 동시에 동작되는 노드를 작성해보았다.
ROS1을 이용한 프로젝트를 진행할 때 한 노드에서 publish와 subscribe가 동작해야하는 경우가 많았으므로 ROS2에서도 구현하고자 하였다.

pkg 생성

$ cd ~/robot_ws/src
$ ros2 pkg create my_test_pkg_py --build-type ament_python --dependencies rclpy std_msgs

pkg를 생성하기 위해 본인의 workspace에 들어가고 pkg를 생성해준다.
간단한 동작을 python을 통해 구현할 것이므로 rclpy와 std_msgs를 dependencies에 추가하였다.

코드 작성 전 구상하기

node1과 node2가 서로 topic을 publish하고 subscribe할 수 있도록 구상하였다.
또한 동작 트리거를 입력하기 위해 master_node를 생성하여 전체 명령을 입력할 수 있도록 하였다.
구상하여 제작한 rqt_graph는 다음과 같다.

order topic은 string으로 1, 2, stop을 보낸다.

• 1: node1부터 동작을 시작한다.
• 2: node2부터 동작을 시작한다.
• stop: node1과 node2의 동작을 멈춘다.

코드 작성

master_order.py

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile
from std_msgs.msg import String

class MasterOrder(Node):
    def __init__(self):
        super().__init__('master_order')
        qos_profile = QoSProfile(depth=10)
        self.master_order = self.create_publisher(String, 'order', qos_profile)

    def publish_order(self):
        msg = String()
        msg.data = self.order
        self.master_order.publish(msg)
        self.get_logger().info(f'Order message: {msg.data}')

    def input_order(self):
        self.order = input("Order: ")


def main(args=None):
    rclpy.init(args=args)
    master_order = MasterOrder()

    while True:
        try:
            master_order.input_order()
            if master_order.order == '1' or master_order.order == '2':
                master_order.publish_order()
            elif master_order.order == 'stop':
                master_order.get_logger().info('Order stop')
                master_order.publish_order()
            else:
                master_order.get_logger().info('Try again')
                continue
        except KeyboardInterrupt:
            master_order.get_logger().info('Keyboard Interrupt (SIGINT)')
            break

    master_order.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

input_order 변수를 이용하여 class 내부에 명령어를 저장해준다.
while문 내부에서 입력된 order를 확인해준다.

node1.py

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile
from std_msgs.msg import String

class Node1(Node):
    def __init__(self):
        super().__init__('node1')
        qos_profile = QoSProfile(depth=10)
        self.node1_pub = self.create_publisher(String, 'node1_pub', qos_profile)
        self.sub_order = self.create_subscription(String, 'order', self.sub_order_msg, qos_profile)
        self.sub_node2 = self.create_subscription(String, 'node2_pub', self.sub_node2_msg, qos_profile)

    def node1_pub_message(self):
        msg = String()
        msg.data = 'node1 talk'
        self.node1_pub.publish(msg)

    def sub_order_msg(self, msg):
        self.order_msg = msg.data
        if msg.data == 'stop':
            self.get_logger().info('node1 stop')
        elif msg.data == '1':
            self.get_logger().info('node1 active')
            self.node1_pub_message()

    def sub_node2_msg(self, msg):
        if self.order_msg != 'stop':
            self.get_logger().info(f'node1 listen- {msg.data}')
            self.node1_pub_message()


def main(args=None):
    rclpy.init(args=args)
    node1 = Node1()
    try:
        rclpy.spin(node1)
    except KeyboardInterrupt:
        node1.get_logger().info('Keyboard Interrupt (SIGINT)')
    finally:
        node1.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()

명령어를 subscribe하고 내용을 확인하여 1인 경우 publish를 진행한다.
명령어로 stop이 subscribe 됐을 경우 node1의 동작을 멈춘다.
또한 node2에서 topic을 subscribe하면 받은 데이터를 보여주고 데이터를 publish 해준다.

node2 또한 같은 동작을 수행한다.
node의 숫자만 다르고 내용은 크게 다를게 없으므로 설명은 생략하도록 하겠다.

node2.py

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile
from std_msgs.msg import String

class Node2(Node):
    def __init__(self):
        super().__init__('node2')
        qos_profile = QoSProfile(depth=10)
        self.node2_pub = self.create_publisher(String, 'node2_pub', qos_profile)
        self.sub_order = self.create_subscription(String, 'order', self.sub_order_msg, qos_profile)
        self.sub_node1 = self.create_subscription(String, 'node1_pub', self.sub_node1_msg, qos_profile)

    def node2_pub_message(self):
        msg = String()
        msg.data = 'node2 talk'
        self.node2_pub.publish(msg)

    def sub_order_msg(self, msg):
        self.order_msg = msg.data
        if msg.data == 'stop':
            self.get_logger().info('node2 stop')
        elif msg.data == '2':
            self.get_logger().info('node2 active')
            self.node2_pub_message()

    def sub_node1_msg(self, msg):
        if self.order_msg != 'stop':
            self.get_logger().info(f'node2 listen- {msg.data}')
            self.node2_pub_message()


def main(args=None):
    rclpy.init(args=args)
    node2 = Node2()
    try:
        rclpy.spin(node2)
    except KeyboardInterrupt:
        node2.get_logger().info('Keyboard Interrupt (SIGINT)')
    finally:
        node2.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()

setup.py 수정

setup.py 하단 'console_scripts'를 수정한다. master_order, node1, node2의 코드를 인식시켜준다.

'console_scripts': [
	'master_order = my_test_pkg_py.master_order:main',
    'node1 = my_test_pkg_py.node1:main',
    'node2 = my_test_pkg_py.node2:main',
]

동작 확인

생성한 pkg 하나만 colcon build할 경우 colcon build --symlink-install --packages-select를 이용하여 build 해준다.

$ cd ~/robot_ws
$ colcon build --symlink-install --packages-select my_test_pkg_py
$ source ~/.bashrc

터미널 3개를 열고 동작을 확인한다. 아래의 명령어는 각 터미널에 1개씩 입력한다.

$ ros2 run my_test_pkg_py master_order
$ ros2 run my_test_pkg_py node1
$ ros2 run my_test_pkg_py node2

master_order를 실행한 터미널에 1, 2, stop을 입력하여 동작을 확인한다.
rqt_graph를 이용하면 위에 올렸던 rqt_graph의 사진을 확인할 수 있다.