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()
#函數不被用作模塊調用