# How to store two data of consecutive frames for computation?

I am using ROS Kinetic on ubuntu 16.04.

The application I am trying to do is to obtain messages from the VRPN optitrack system and use these data to construct a data array that displays variables in sync.
For example, the optitrack system reads in position and attitude data. The language I am using is python and I will write two subscribers, one is to subscribe to the position topic while the other on the attitude topic on the ros vrpn client. The read data are saved in variables for computation. I will define three functions, one will return a rotational matrix, the other will return the linear velocity calculated from consecutive frame of position data, and the other function will calculate angular velocity from consecutive frame of attitude data. The three functions will return a [1x9] rotational matrix representation [1x3] linear velocity representation and [1x3] angular velocity representation.

I will then append these numpy arrays in to an [1x18] array for display as follows: [ 1x9 rot matrix array 1x3 position data 1x3 linear velocity 1x3 angular velocity]

One question is how can I make sure the data are printed in sync? I understand the position data gets updated at the highest frequency and I only needed to queue two messages each time for processing. as the two messages would be the first frame followed by the second frame. The position data in the final display would be the one in the second frame. This could be a silly question because simply the script just needed to take whatever processing time and the variables given to compute results. And syncing this should not be a problem.

My main question in this case is how can I write code to store data from consecutive messages, basically consecutive frames? The following is my example code for calculating linear velocity from two position data of consecutive frames.

import transforms3d.euler as eul

import rospy

def ReturnLinearVelocity(previous, current, frequency):

linearVel =(current - previous)*frequency

return linearVel

def Callback(data):
previous = data
current = data

ReturnLinearVelocity(previous, current, frequency)

rospy.init_node('rq3_proxy')

while not rospy.is_shutdown():
rospy.Subscriber("ros_vrpn_client topic", positiongeometry, Callback)
rospy.sleep(1)

rospy.spin()

edit retag close merge delete

Sort by » oldest newest most voted

Hello,

I don't get the question right but I will try to answers :

How can I store 2 consecutives messages for computing velocities :

Your code is almost OK with some small problems :

1) The while not rospy.is_shutdown(): is useless here because rospy.spin() is doing the same (waiting for rospy to shutdown), check Rospy Initialization and Shutdown for some explanation :

2.1 Testing for shutdown

The most common usage patterns for testing for shutdown in rospy are:

... setup callbacks

while not rospy.is_shutdown(): do some work

and

... setup callbacks

rospy.spin()

The spin() code simply sleeps until the is_shutdown() flag is True. It is mainly used to prevent your Python Main thread from exiting.

There are multiple ways in which a node can receive a shutdown request, so it is important that you use one of the two methods above for ensuring your program terminates properly.

2) You should declare previous and current as global variable (there is better methods than global variable but let's not complicate the answers yet) in the callback and in the code, and you shouldn't modify the variable current and previous in any other place (You are thread safe as long as variable are modified only in 1 thread, here the callback function, they can be read anywhere), also you should initialize them :

current = positiongeometry()
previous = positiongeometry()

def ReturnLinearVelocity(freq):
return (current-previous)*freq

def Callback(data):
global previous
global current
previous = current
current = data


Also, you wrote previous = data, that was probably an error since previous shoud have the previous position (the current before modification), otherwise you whould always have a zero-velocity :) .

3) You should call ReturnLinearVelocity() in a loop after initializing the callbacks, so you want to go for the pattern 1 for testing shutdown in rospy. In the current form, you calculate LinearVelocity before any message are received, hence creating a Name Error Variable previous is not defined (or something like that), you should put the function in a loop (with a timer, you don't want to loop as fast as possible and let your python code eat all you RAM and CPU) check Rospy Publisher and your code should look like that :

rospy.init_node('rq3_proxy')
r = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
print(ReturnLinearVelocity(frequency_of_optitrack))
r.sleep()


Here, frequency_of_optitrack if the frequency at which the position are send. This is not perfect because you are not sure that the frequency will remain the same, so check the complicated answer below :

You can use a message filter to sync 2 frames in a cache, and calculate the difference between them. A Cache message filter work like a rotating buffer, so if you set the size to 2, the first message of the buffer will be the previous position and the last message will be the current position. Cache filter have the plus of keeping the stamp of the message (I don't know if your message positiongeometryhad them) even if the message wasn't a ...

more

1

@Imathieu
Much appreciated for such a detailed explanation and suggestion.

( 2019-10-12 08:36:48 -0500 )edit