Loading files
This file is pinned to the selected release. Download the original to open it in your design tool.
25e0ce228This file is pinned to the selected release. Download the original to open it in your design tool.
#!/usr/bin/env python
import rospy
import math
from std_msgs.msg import Float32
from sensor_msgs.msg import Imu
from tf.transformations import euler_from_quaternion
rad2degrees = 180.0/math.pi
degrees2rad = math.pi / 180.0
class Imu2Angles:
def __init__(self):
self.rate = rospy.get_param('~rate', 100.0)
self.imu_name = rospy.get_param('~imu_name', 'imu_steer')
self.topic_name = rospy.get_param('~topic_name', 'topic_name')
self.roll = 0.0
self.pitch = 0.0
self.yaw = 0.0
self.pub_imu_roll_msg = Float32()
self.pub_imu_pitch_msg = Float32()
self.pub_imu_yaw_msg = Float32()
self.pub_imu_roll = rospy.Publisher('/' + self.imu_name + '/roll', Float32, queue_size=1)
self.pub_imu_pitch = rospy.Publisher('/' + self.imu_name + '/pitch', Float32, queue_size=1)
self.pub_imu_yaw = rospy.Publisher('/' + self.imu_name + '/yaw', Float32, queue_size=1)
self.sub = rospy.Subscriber(self.topic_name, Imu, self.process_imu_message, queue_size=1)
rospy.spin()
def process_imu_message(self, imuMsg):
quaternion = (
imuMsg.orientation.x,
imuMsg.orientation.y,
imuMsg.orientation.z,
imuMsg.orientation.w)
(self.roll, self.pitch, self.yaw) = euler_from_quaternion(quaternion)
self.roll = self.roll * rad2degrees
self.pitch = self.pitch * rad2degrees
self.yaw = self.yaw * rad2degrees
self.publish_angles()
def publish_angles(self):
self.pub_imu_roll_msg = Float32()
self.pub_imu_roll_msg.data = self.roll
self.pub_imu_roll.publish(self.pub_imu_roll_msg)
self.pub_imu_pitch_msg = Float32()
self.pub_imu_pitch_msg.data = self.pitch
self.pub_imu_pitch.publish(self.pub_imu_pitch_msg)
self.pub_imu_yaw_msg = Float32()
self.pub_imu_yaw_msg.data = self.yaw
self.pub_imu_yaw.publish(self.pub_imu_yaw_msg)
# Main function.
if __name__ == '__main__':
# Initialize the node and name it.
rospy.init_node('Imu2Angles')
try:
obj = Imu2Angles()
except:
pass