Velocity of drone in body frame

asked 2019-06-04 09:22:11 -0500

mauro gravatar image

updated 2019-06-05 02:37:33 -0500


I would like to use the tf package to calculate the velocity of a drone with respect to the world, expressed in the body coordinates of the drone. I thought I could use the lookupTwistFull method for that but I don't get the results I expect.

When I use the following bit of code, it seems like I'm getting the velocity of the drone expressed in the world frame. I'm testing this by having someone hold the drone and walking along the world axis while I check the plots (using rqt_multiplot) of the forward and sidewards velocity.

refpoint = (0,0,0)
trans_vel, rot_vel = self._listener.lookupTwistFull(drone_frame, world_frame, drone_frame, refpoint
                                        , drone_frame, rospy.Time(0), rospy.Duration(0.1))

I though this would be in accordance with the specifications in the tf documentation:

lookupTwistFull(tracking_frame, observation_frame, reference_frame, reference_point, reference_point_frame, time, averaging_interval)

Can someone tell me, if I'm specifying the arguments of lookupTwistFull correctly in order to get the velocity in the drone frame or if I misunderstood the documentation?

I'm super thankful for any help =)

Edit: I found an open issue concerning this topic on Github. Can anyone confirm this?

edit retag flag offensive close merge delete