Scale error in twist angular velocities

[qw8z5fUB]

Have observed angular velocities published by twist pub are too small by a factor of 100 in my case. This is while running the tracker at 100hz. I believe this driver is missing a division by dt.

Looking in VrpnTrackerRos::handle_twist, I see the vrpn_TRACKERVELCB vel_quat field is currently converted from quaternion to Euler then published without further modification.

Note in the the definition of the vrpn_TRACKERVELCB struct:

typedef struct _vrpn_TRACKERVELCB {
    struct timeval msg_time;  // Time of the report
    vrpn_int32 sensor;        // Which sensor is reporting
    vrpn_float64 vel[3];      // Velocity of the sensor
    vrpn_float64 vel_quat[4]; // Rotation of the sensor per vel_quat_dt
    vrpn_float64 vel_quat_dt; // delta time (in secs) for vel_quat
} vrpn_TRACKERVELCB;

The vel_quat field provides change in rotation over a time interval, and the vel_quat_dt provides the length of that time interval. Hence the user must divide vel_quat by vel_quat_dt to determine rotation rate per second. This driver does not perform that division, hence the scaling error on published angular velocity.

I'd suggest the following (untested!) change is appropriate:

diff --git a/src/vrpn_client_ros.cpp b/src/vrpn_client_ros.cpp
index 7a03870..899ee45 100644
--- a/src/vrpn_client_ros.cpp
+++ b/src/vrpn_client_ros.cpp
@@ -265,15 +265,15 @@ namespace vrpn_client_ros
       tracker->twist_msg_.twist.linear.y = tracker_twist.vel[1];
       tracker->twist_msg_.twist.linear.z = tracker_twist.vel[2];
 
-      double roll, pitch, yaw;
+      double d_roll, d_pitch, d_yaw;
       tf2::Matrix3x3 rot_mat(
           tf2::Quaternion(tracker_twist.vel_quat[0], tracker_twist.vel_quat[1], tracker_twist.vel_quat[2],
                           tracker_twist.vel_quat[3]));
-      rot_mat.getRPY(roll, pitch, yaw);
+      rot_mat.getRPY(d_roll, d_pitch, d_yaw);
 
-      tracker->twist_msg_.twist.angular.x = roll;
-      tracker->twist_msg_.twist.angular.y = pitch;
-      tracker->twist_msg_.twist.angular.z = yaw;
+      tracker->twist_msg_.twist.angular.x = d_roll / tracker_twist.vel_quat_dt;
+      tracker->twist_msg_.twist.angular.y = d_pitch / tracker_twist.vel_quat_dt;
+      tracker->twist_msg_.twist.angular.z = d_yaw / tracker_twist.vel_quat_dt;
 
       twist_pub->publish(tracker->twist_msg_);
     }