listener lookupTwist producting wrong velocity for turns.
I'm trying to extract linear velocity from an object's tf frame using lookupTwist.
I simulate an object moving around on a 400m track. I used simple line and circle equations to describe its position at time t at fixed velocity.
Looking at the tf in rviz, it performs correctly and moves in a smooth oval.
Now when I try and extract velocity using lookupTwist, the linear velocities are all messed up. In straight lines the linear velocities are correct, but in the turns the velocities values sky rocket. For example if I set my simulated object to move at 10 m/s, on a turn the velocity would instantly shoot up to 50m/s go back down halfway way through the turn, shoot back up to 50 just before the straight away.
This is because the code I'm using to extract the velocity from a tf. Is there something I should be aware of or is my environment bugged?
geometry_msgs::Twist objectTwist;
tf::TransformListener listener;
listener.lookupTwist("/map", "/sim_object", ros::Time(0), ros::Duration(0.1), objectTwist);
cout << sqrt(objectTwist.linear.x * objectTwist.linear.x + objectTwist.linear.y * objectTwist.linear.y) << endl;