Access Cache Data Error

asked 2019-10-16 02:28:49 -0500

haloted
32 ●5 ●8 ●12

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 ...

(more)

answered 2019-10-16 02:51:41 -0500

gvdhoorn
86574 ●283 ●1432 ●1054 http://cor.tudelft.nl/

updated 2019-10-16 02:55:35 -0500

tl;dr: you create a special message_filters.Subscriber, which you pass to message_filters.Cache. This will cause the Cache to be updated with every incoming message. In addition, you register your own callback (ie: self.ReturnRateData) that also gets called for every new incoming message.

But that callback is a regular ROS subscribers callback, so it receives the message that was received by the special Subscriber you created earlier.

In your callback signature, you call that argument Cache, and treat it as if it is the message_filters.Cache instance (by calling getElemBeforeTime(..) on it).

That won't work, as it's actually a geometry_msgs.PoseStamped message (that was just received).

You'll have to keep a reference to cache around and access that in your ReturnRateData(..) (perhaps store your Cache instance as a member variable of your OptitrackReceive class).

Longer: you have this:

def __init__(self):
  [..]
  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 ReturnRateData(self, Cache):
  self.rate_previous = Cache.getElemBeforeTime(Cache.getOldestTime())
  self.rate_current = Cache.getElemAfterTime(Cache.getLatestTime())
  [..]

The Cache argument that ReturnRateData(..) receives is not a message_filters.Cache, but a PoseStamped.

This is described on the wiki page of message_filters (here):

sub = message_filters.Subscriber('my_topic', sensor_msgs.msg.Image)
cache = message_filters.Cache(sub, 100)
In this example, the Cache stores the last 100 messages received on my_topic, and myCallback is called on the addition of every new message. The user can then make calls like cache.getInterval(start, end) to extract part of the cache.

And from the Message Filters API documentation (here):

message_filters.Subscriber.registerCallback(cb, *args)

Register a callback function cb to be called when this filter has output. The filter calls the function cb with a filter-dependent list of arguments, followed by the call-supplied arguments args.

edit flag offensive delete link

Comments

@gvdhoorn: Thanks for the reply. I solved my problem with the following code:

self.sub = message_filters.Subscriber('/vrpn_client_node/TestTed/pose',PoseStamped)

self.cache = message_filters.Cache(self.sub, 2,allow_headerless=False)

self.cache.registerCallback(self.ReturnRateData)

haloted ( 2019-10-16 23:10:09 -0500 )edit

And:

self.rate_previous = self.cache.getElemBeforeTime(self.cache.getOldestTime())

self.rate_current = self.cache.getElemAfterTime(self.cache.getLatestTime())

haloted ( 2019-10-16 23:11:17 -0500 )edit

add a comment

1 Answer

Comments

Question Tools

Stats

Related questions

Access Cache Data Error edit

1 Answer

Comments

Question Tools

Stats

Related questions

Access Cache Data Error