Bad Python code for custom message node - eating up cpu?
Wrote a custom message and am attempting to publish (via bagfile playback) for use in another node . The python roll/pitch/yaw is based upon this previous answer(link text ), although poorly executed on my part. (License omitted for brevity) The remaining dependent code is at this Dropbox link: https://www.dropbox.com/sh/u3kznp4oiy...
#!/usr/bin/python
# -*- coding: utf-8 -*-
#subscribes to quaternions, converts quaternions to Euler angles, and then publish the Euler angles. The Euler angles are in a custom message
# Start up ROS pieces.
PKG = 'loc2orbgt'
import roslib; roslib.load_manifest(PKG)
import rospy
import math
import tf
# ROS messages.
from nav_msgs.msg import Odometry
from sensor_msgs.msg import Imu
from loc2orbgt.msg import Eulers
class QuatToEuler():
def __init__(self):
self.got_new_msg = False
self.euler_msg = Eulers()
# Create subscribers and publishers.
#sub_imu = rospy.Subscriber("imu/data", Imu, self.imu_callback)
sub_rlocl = rospy.Subscriber("odometry/filtered", Odometry, self.rlocl_callback)
pub_euler = rospy.Publisher("euler", Eulers,queue_size=1)
# Main while loop.
while not rospy.is_shutdown():
# Publish new data if we got a new message.
if self.got_new_msg:
pub_euler.publish(self.euler_msg)
rate = rospy.Rate(15) # 15hz
self.got_new_msg = False
# Odometry callback function.
def rlocl_callback(self, msg):
# Convert quaternions to Euler angles.
(r, p, y) = tf.transformations.euler_from_quaternion([msg.pose.pose.orientation.x, -msg.pose.pose.orientation.y, -msg.pose.pose.orientation.z, -msg.pose.pose.orientation.w])
#(r, p, y) = [r*180/math.pi, p*180/math.pi, y*180/math.pi]
(r, p, y) = [r*180/math.pi, p*180/math.pi, (y*180/math.pi)+(11.8599971761+90)]
self.fill_euler_msg(msg, r, p, y)
# Fill in Euler angle message.
def fill_euler_msg(self, msg, r, p, y):
self.got_new_msg = True
self.euler_msg.header.stamp = msg.header.stamp
self.euler_msg.roll = r
self.euler_msg.pitch = p
self.euler_msg.yaw = y
# Main function.
if __name__ == '__main__':
# Initialize the node and name it.
rospy.init_node('quat_to_euler')
# Go to class functions that do all the heavy lifting. Do error checking.
try:
quat_to_euler = QuatToEuler()
except rospy.ROSInterruptException: pass
The code (q2_rpy.py) seems to work fine, but I found that the second node ( a time syncro node) would not initiate after being called by the launch file. Upon further investigation, it turns out that the former rpy code was using waaaay too much cpu. (See Screenshot).
Could it be that is the reason for second node not subscribing? Is there a better way regarding loop efficiency? Not sure where to go from here as the second node has a dependency on this one. As always, any insight will be greatly appreciated.
B2256