0
$\begingroup$

Rosanswers logo

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/u3kznp4oiygbuan/AADAD_QmU_LieGK3y_sbxR65a?dl=0

   #!/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


Originally posted by b2256 on ROS Answers with karma: 162 on 2016-06-13

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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.


Originally posted by F.Brosseau with karma: 379 on 2016-06-14

This answer was ACCEPTED on the original site

Post score: 1


Original comments

Comment by b2256 on 2016-06-14:
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.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.