[ROS2-python 기초] 여러 topic을 구독하여 csv 파일 만들기

정예슬·2023년 2월 10일
0

ROS

목록 보기
3/4
post-thumbnail
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile
from std_msgs.msg import Int32
import time
import pandas as pd
import numpy as np


class csvSub(Node) :
   def __init__(self) :
      super().__init__('csv_sub')
      qos_profile = QoSProfile(depth=10)
      self.subscriber_1 = self.create_subscription(
      Int32,
      '/test1',
      self.listener_callback1,
      qos_profile
      )
      
      self.subscriber_2 = self.create_subscription(
      Int32,
      '/test2',
      self.listener_callback2,
      qos_profile
      )

      self.subscriber_3 = self.create_subscription(
      Int32,
      '/test3',
      self.listener_callback3,
      qos_profile
      )

      self.starttime=time.time()
      self.systime = 0
	  self.data1 = 0
      self.data2 = 0
      self.data3 = 0
      self.columns = ['time', 'data1', 'data2', 'data3']
      self.df = pd.DataFrame([], columns=self.columns)

   def listener_callback1(self, msg) :
      self.data1 = msg.data

   def listener_callback2(self, msg) :
      self.data2 = msg.data

   def listener_callback3(self, msg) :
      self.data3 = msg.data
      self.systime = round(time.time() - self.starttime, 5)
      temp = pd.DataFrame([[self.systime, self.data1, self.data2, 
                           self.data3]], columns=self.columns)
      self.df = pd.concat([self.df, temp])      
      print(self.df.tail(1)) 
 

def main(args=None) :
   rclpy.init(args=args)
   node = csvSub()
   try :
      rclpy.spin(node)
   except KeyboardInterrupt :
      node.get_logger().info('STOPPED BY KEYBOARD')
   finally :
      node.destroy_node()
      rclpy.shutdown()
      node.df.to_csv(f"./result.csv", index=None)
if __name__ == '__main__' :
   main()
profile
춘식이랑 함께하는 개발일지

0개의 댓글