반응형
  1. 시작하기 전
    1. 앞선 글인 String토픽 생성을 기반으로 작성하였다.
    2. 기존 String전송과 차이점만 적어놓았으므로 처음이라면 아래의 글을 참고 하기 바란다.
      1. ROS2 패키지만들기-1(패키지 생성) (https://hostramus.tistory.com/112)
      2. ROS2 패키지만들기-2(노드생성 publisher) (https://hostramus.tistory.com/113)
      3. ROS2 패키지만들기-3(노드생성 Subscriber) (https://hostramus.tistory.com/114)
      4. ROS2 패키지만들기-4(확인 및 실행) (https://hostramus.tistory.com/115)
  2.  목표
    1. 2개의 정수를 퍼블리시하는 토픽 만들기
    2. std_msgs.msg의 Int8MultiArray 사용해보기
    3. 2개의 정수를 퍼블리시 하기
    4. 2개의 정수를 서브스크라이브하여 합계 계산하기
  3. 퍼블리셔 코드 (Publisher Code)
    import rclpy
    from rclpy import node
    from rclpy.node import Node
    from rclpy.qos import QoSProfile
    from std_msgs.msg import Int8MultiArray
    
    class HelloworldPublisher222(Node):
      def __init__(self):
        super().__init__('helloworld_publisher222')
        qos_profile = QoSProfile(depth=10)
        self.count = 0
        self.helloworld_publisher = self.create_publisher(Int8MultiArray, 'helloworld', qos_profile)
        self.timer = self.create_timer(1, self.publish_helloworld_msg)
    
      def publish_helloworld_msg(self):
        msg = Int8MultiArray()
        msg.data = [self.count, self.count+1]
        self.helloworld_publisher.publish(msg)
        self.get_logger().info('pub22 message: {0}'.format(msg.data))
        self.count += 1
    
    def main(args=None):
      rclpy.init(args=args)
      node = HelloworldPublisher222()
      try:
        rclpy.spin(node)
      except KeyboardInterrupt:
        node.get_logger().info('Keyboard Interrupt (SIGINT)')
      finally:
        node.destroy_node()
        rclpy.shutdown()
    
    if __name__ == '__main__':
      main()
    ​
  4. 코드 분석 (Publisher)
    1. from std_msgs.msg import Int8MultiArray
       
      1. 5행의 메세지 종류를 Int8MultiArray로 import한다.
      2. int8은 8비트정수로 -127~127까지 범위다
      3. int16, int32, int64중 원하는 크기로 사용한다.
    2. self.helloworld_publisher = self.create_publisher(Int8MultiArray, 'helloworld', qos_profile)
      1. 12행의 self.create_publisher에서 퍼블리시 할 형식을 기존 String대신 Int8Multiarray로 설정한다.
      2. 다른 것은 모두 그대로 놔둔다.
    3. msg = Int8MultiArray()
      msg.data = [self.count, self.count+1]
      1. 16행의 msg에 Int8MultiArray 인스턴스를 생성한다.
      2. 17행에서 msg.data의 값을 array형태로 만들어 넣는다.
        1. 여기에서는 count를 사용하였다.
  5. 서브스크라이버 코드 (Subscriber Code)
    import rclpy
    from rclpy.node import Node
    from rclpy.qos import QoSProfile
    from std_msgs.msg import Int8MultiArray
    
    class HelloworldSubscriber222(Node):
      def __init__(self):
        super().__init__('Helloworld_Subscriber222')
        qos_profile = QoSProfile(depth=10)
        self.helloworld_subscriber = self.create_subscription(Int8MultiArray, 'helloworld', self.subscribe_topic_message, qos_profile)
    
      def subscribe_topic_message(self, msg):
        b = msg.data[0]
        c = msg.data[1]
        self.get_logger().info('Receiced message: {0}'.format(msg.data))
        self.get_logger().info(str(b) + " + " + str(c) + " = " + str(b+c))
    
    def main(args=None):
      rclpy.init(args=args)
      node = HelloworldSubscriber222()
      try:
        rclpy.spin(node)
      except KeyboardInterrupt:
        node.get_logger().info('Keyboard Interrupt (SIGINT)')
      finally:
        node.destroy_node()
        rclpy.shutdown()
    
    if __name__ == '__main__':
      main()​
  6.  코드 분석 (Subscriber)
    1. from std_msgs.msg import Int8MultiArray​
       
      1. 4행에서 퍼블리셔와 같이 메세지 형식을 Int8MultiArray로 맞춰서 import한다.
    2. self.helloworld_subscriber = self.create_subscription(Int8MultiArray, 'helloworld', self.subscribe_topic_message, qos_profile)
      1.  10행에서 self.create_subscription에서 Int8MultiArray로 맞춰준다.
    3.   def subscribe_topic_message(self, msg):
          b = msg.data[0]
          c = msg.data[1]
          self.get_logger().info('Receiced message: {0}'.format(msg.data))
          self.get_logger().info(str(b) + " + " + str(c) + " = " + str(b+c))
      1. msg.data는 Array형태이다.
        1. b와 c에 msg.data[0]과 msg.data[1]의 데이터를 받는다.
      2. self.get_logger().info(str(b) + " + " + str(c) + " = " + str(b+c))
        1. self.get_logger().info()는 스트링타입을 인수로 받는다.
        2. 따라서 정수인 b,c를 str()을 이용하여 변환해준다.
  7. 실행
    1. 퍼블리셔 실행 화면
      1. self.count의 값이 array형태로 퍼블리시 되는것이 확인 된다.
      2. 위의 코드상 int8형식이기에 127이 넘어가는 순간 에러가 발생한다.
    2. 서브스크라이버 실행 화면
      1. 서브스크라이버는 받은 데이터(msg.data)를 한번 출력 후에 두 정수의 합계를 출력한다.

 

아래 출처의 내용을 수정 변경하였습니다.

개인적 공부를 위해 올린 글로 공부하실분은 아래 링크의 글을 참고하시기 바랍니다.

출처 : 오로카 카페 (cafe.naver.com/openrt/24450)

  1.  
반응형

+ Recent posts