Access Cache Data Error
Based on the code I have written for question #Q335276, when I try to use the Cache methods, they weren't there. I am not sure why I am getting the error message as shown below:
[ERROR] [1571210224.306722]: bad callback: <bound method Subscriber.callback of <message_filters.Subscriber object at 0x7f226a827208>>
Traceback (most recent call last):
File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/topics.py", line 750, in _invoke_callback
cb(msg)
File "/opt/ros/kinetic/lib/python2.7/dist-packages/message_filters/__init__.py", line 75, in callback
self.signalMessage(msg)
File "/opt/ros/kinetic/lib/python2.7/dist-packages/message_filters/__init__.py", line 57, in signalMessage
cb(*(msg + args))
File "/opt/ros/kinetic/lib/python2.7/dist-packages/message_filters/__init__.py", line 134, in add
self.signalMessage(msg)
File "/opt/ros/kinetic/lib/python2.7/dist-packages/message_filters/__init__.py", line 57, in signalMessage
cb(*(msg + args))
File "/home/ted/catkin_ws/src/transmit_thrust/scripts/OptitrackReceive.py", line 119, in ReturnRateData
self.rate_previous = Cache.getElemBeforeTime(Cache.getOldestTime())
AttributeError: 'PoseStamped' object has no attribute 'getElemBeforeTime'
Here is my code I am using for the Cache method:
import rospy
from geometry_msgs.msg import PoseStamped
from std_msgs.msg import String
# from transmit_thrust.msg import RotorsAction
from transmit_thrust.msg import ThrustAction
from mavros_msgs.msg import ActuatorControl
# The message filters are used to calculate consecutive frames of messages received
import message_filters
class OptitrackReceive():
def __init__(self):
self.pos_original = PoseStamped()
self.pos_converted = PoseStamped()
self.rate_previous = PoseStamped()
self.rate_current = PoseStamped()
self.rotMatrix = np.zeros((3,3))
self.rotMatrixComp = np.zeros((3,3))
self.eulerAngle = np.zeros((3))
self.linear_velocity = np.zeros((3))
self.angular_velocity = np.zeros((3))
rospy.Subscriber('/vrpn_client_node/TestTed/pose',
PoseStamped, self.transform)
rospy.Subscriber('/vrpn_client_node/TestTed/pose',
PoseStamped, self.frameConversion)
rospy.Subscriber('/vrpn_client_node/TestTed/pose',
PoseStamped, self.ReturnRotMatrix)
self.sub = message_filters.Subscriber('/vrpn_client_node/TestTed/pose',PoseStamped)
cache = message_filters.Cache(self.sub, 2,allow_headerless=False)
cache.registerCallback(self.ReturnRateData)
def frameConversion(self, data):
Original = np.zeros((3,1))
# Flip the y axis
Original[0] = data.pose.position.x
Original[1] = data.pose.position.y
Original[2] = data.pose.position.z
# Rotate counter clockwise around z by 90
Ry = tf3d.euler.euler2mat(0,-math.pi*0.5,0,'sxyz')
Original = np.dot(Ry,Original)
data.pose.position.x = Original[0]
data.pose.position.y = Original[1]
data.pose.position.z = Original[2]
self.pos_converted = data
def transform(self, data):
data.pose.position.x, data.pose.position.z = data.pose.position.z, data.pose.position.x
self.pos_original = data
#print(data)
def ReturnRotMatrix(self, data):
# The RotMatrix have to be from quaternions and therefore be converted to quaternions
# To be done
euler = self.ReturnEulerAngle(data)
self.eulerAngle = np.rad2deg(euler)
self.rotMatrix = tf3d.taitbryan.euler2mat(euler[2],euler[1],euler[0])
self.rotMatrixComp = tf3d.quaternions.quat2mat([data.pose.orientation.w, data.pose.orientation.x, data.pose.orientation.y, data.pose.orientation.z])
def ReturnEulerAngle(self, data):
# This converts the data quaternion type to euler angle type
Orientation_list = np.zeros((4))
Orientation_list[0] = data.pose.orientation.w
Orientation_list[1] = data.pose ...