Question

我正在尝试绘制一些ROS传感器数据，但实际上并没有用。基本上我下面的代码所做的是创建一个订户，并订阅Imu数据。然后将数据乘以2，并将值传递给animate函数。

我知道它可以工作Imu部分工作，但是我需要实时绘制数据的帮助。当我尝试绘图时，该绘图甚至从未显示出来，或者它是空白的。

import rclpy
from rclpy.node import Node
import math
from sensor_msgs.msg import Imu
import numpy as np
from numpy import linalg as LNG
from rclpy.qos import qos_profile_sensor_data
import time
import matplotlib.pyplot as plt
import matplotlib.animation as animation


fig = plt.figure()
ax = fig.add_subplot(111)

x = []
y = []
def animate(i, imu):
    x.append(time.time())
    y.append(imu)
    plt.plot(x, y, color='r')

class State_Estimator(Node):
    def __init__(self):
        super().__init__('state_estimator')
        self.get_logger().info('Start Receiving Imu Data')
        self.IMU_Sub = self.create_subscription(IMU, '/imu/data', self.quaternion_callback,
                                                qos_profile=qos_profile_sensor_data)

        self.get_logger().info("Finished Initialization of Node: {}".format(self.get_name()))
        


    def quaternion_callback(self, msg):

        q_IMU                = self.quaternion      = msg.orientation      
        
        while True:
           double_Imu  = 2 * q_IMU
           q_IMU       = double_Imu
         
        return q_IMU

anim = animation.FuncAnimation(fig, animate, fargs = q_IMU, interval=1000) 
plt.show()


def main(args = None):
    rclpy.init(args = args)  
    state_estimator = State_Estimator()  



    try:
        while (1):
            rclpy.spin(state_estimator)
    except Exception as e:
        print("Exception: {}".format(e))
    # state_estimator.destroy_node()

    rclpy.spin(state_estimator)

    rclpy.shutdown()


if __name__ == '__main__':
    main()

需要帮助使用Matplotlib绘制传感器数据

0 个答案: