One possibility is to use tf's **Transformer::lookupTwist** to calculate the velocity. If you want to do it "by hand" you can compute the current (online) linear velocity expressed in the robot's body frame as:

v[k] = {o[k-1]}^(-1) * (p[k] - p[k-1]) / (t[k] - t[k-1]),

using the variables

v[k]: current linear velocity (delayed by 1/200 seconds),

{o[k-1]}^(-1): previous orientation inverted (unit quaternion or rotation matrix),

p[k], p[k-1]: positions of this and previous frame,

t[k], t[k-1]: timestamp of this and previous frame.

This is, you de-rotate the translation vector from the last to the current transform and divide it by the time difference.

The offline version of this is

v[k] = {o[k]}^(-1) * (p[k+1] - p[k-1]) / (t[k+1] - t[k-1]),

which is the symmetric derivative.

Depending on how well your robot is tracked, the velocity signal might be noisy, so you might want to use a filter.

Can you specify this a bit? What exactly are you given from the mocap sytstem (position, orientation, timestamp)? Do you want to have the velocity in the robot body frame? I did the same lately for an Optitrack system, so maybe I could give you some hints.

The system gives x,y,z position of robot's center,orientation of robot and timestamps(secs).These messages are published at the rate of 100 Hz.As my robot is ground based one,I am using only x and y position(not orientation) to find the velocity. I want to find velocity of the robot's center.