Ask Your Question

Revision history [back]

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 stamped type (all geometry_msgs have a stamped and not stamped type) and also to not call the Callback if the buffer is not full.

So your subscriber can look like this :

sub = message_filters.Subscriber("ros_vrpn_client topic", positiongeometry)
cache = message_filters.Cache(sub, 2,allow_headerless=True)
cache.registerCallback(Callback)

So in your function Callback, you can do this :

def Callback(cache):
    global previous
    global current
    previous = cache.getElemBeforeTime(cache.getOldestTime()) #get the oldest message
    current= cache.getElemAfterTime(cache.getLastestTime()) #get the newest message
    #If you have a non stamped message, add the stamp as a global variable
    global previous_stamp
    global current_stamp
    previous_stamp = cache.getOldestTime()
    current_stamp= cache.getLastestTime()

And you can calculate the exact average velocity in the ReturnLinearVelocity() :

def ReturnLinearVelocity():
    #Instead of current_stamp, you can use the current.header.stamp and current.point.x
    #   if you use a geometry_msgs/PointStamped for example
    vel_x = (current.x - previous.x) / (current_stamp-previous_stamp)
    vel_y = (current.y - previous.y) / (current_stamp-previous_stamp)
    vel_z = (current.z - previous.z) / (current_stamp-previous_stamp)
    return [vel_x, vel_y, vel_z]

and also you can change your code a bit, putting the array of velocities in global and doing the linear velocity computation inside the Callback() function directly to store only the array of velocity, but it's your code you know what to do !