python ros 订阅imu数据，实时显示欧拉角

 

#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
import math
from sensor_msgs.msg import Imu
from geometry_msgs.msg import Pose, Quaternion,PoseWithCovarianceStamped
import PyKDL

def quat_to_angle(quat):
  rot = PyKDL.Rotation.Quaternion(quat.x, quat.y, quat.z, quat.w)
  return map(normalize_angle,rot.GetRPY())

def normalize_angle(angle):
    res = angle
    while res > math.pi:
        res -= 2.0*math.pi
    while res < -math.pi:
        res += 2.0*math.pi
    return res

def callback(data):
    #rpy
    print(quat_to_angle(data.orientation))
    #回调函数 收到的参数.data是通信的数据 默认通过这样的 def callback(data) 取出data.data数据

def getangle(orientation):
    x=orientation.x
    y=orientation.y
    z=orientation.z
    w=orientation.w

    f=2*(w*y-z*z)

    r = math.atan2(2*(w*x+y*z),1-2*(x*x+y*y))
    p=0
    if(-1<=f<=1):
        p = math.asin(f)
    y = math.atan2(2*(w*z+x*y),1-2*(z*z+y*y))

    angleR = r*180/math.pi
    angleP = p*180/math.pi
    angleY = y*180/math.pi

    return {"angleR":angleR,"angleP":angleP,"angleY":angleY}


def listener():

    # In ROS, nodes are uniquely named. If two nodes with the same
    # node are launched, the previous one is kicked off. The
    # anonymous=True flag means that rospy will choose a unique
    # name for our 'listener' node so that multiple listeners can
    # run simultaneously.
    rospy.init_node('listener', anonymous=True)
    
    #启动节点同时为节点命名， 若anoymous为真则节点会自动补充名字，实际名字以 listener_322345等表示
    #若为假，则系统不会补充名字，采用用户命名。但是一次只能有一个同名节点，若后面有一个相同listener
    #名字的节点则后面的节点启动会注销前面的相同节点名。
    
    rospy.Subscriber("xxx_imu_driver/imu",Imu, callback)
    #rospy.Subscriber("robot_pose",Pose, callback)
    
    
    #启动订阅，订阅主题，及标准字符串格式，同时调用回调函数，当有数据时调用函数，取出数据
    
    # spin() simply keeps python from exiting until this node is stopped
    
    #循环程序
    rospy.spin()

if __name__ == '__main__':
    listener()
#函数不被用作模块调用