Ask Your Question
0

Bad Python code for custom message node - eating up cpu?

asked 2016-06-13 23:31:32 -0600

b2256 gravatar image

updated 2016-06-13 23:37:34 -0600

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). image description

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

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
1

answered 2016-06-14 02:30:25 -0600

F.Brosseau gravatar image

Why the rospy.Rate is inside the loop ? Is the 15 Hz rate working ?

According to this link http://wiki.ros.org/rospy/Overview/Time, this line should be before the while.

edit flag offensive delete link more

Comments

Yes, the rate was working. I made the change per your suggestion with no change to performance. I will keep looking for other procedural errors. Something is causing this. Thank you for responding.

b2256 gravatar imageb2256 ( 2016-06-14 08:08:05 -0600 )edit
0

answered 2017-03-15 05:22:28 -0600

Fawzi gravatar image

Did you find a solution for this problem?

edit flag offensive delete link more

Comments

Although not an answer, my solution was to post-process raw bagfiles. Not clean, but seems to overcome this problem.

b2256 gravatar imageb2256 ( 2017-04-10 11:17:10 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2016-06-13 23:31:32 -0600

Seen: 262 times

Last updated: Jun 14 '16