Fixed conversion to meters instead of millimeters.

OTL · Feb 19, 2017 · 88c81ed · 88c81ed
1 parent 4b3909c
commit 88c81ed
Showing 1 changed file with 4 additions and 3 deletions.
diff --git a/nodes/cozmo_driver.py b/nodes/cozmo_driver.py
@@ -389,9 +389,10 @@ def _publish_tf(self, update_rate):
         # Note: Sign for linear velocity is taken from commanded velocities!
         # Note: The angular velocity can also be taken from gyroscopes!
         delta_pose = self._last_pose - self._cozmo.pose
-        self._lin_vel = np.sqrt(delta_pose.position.x**2
-                                + delta_pose.position.y**2
-                                + delta_pose.position.z**2) * update_rate * np.sign(self._cmd_lin_vel)
+        dist = np.sqrt(delta_pose.position.x**2
+                       + delta_pose.position.y**2
+                       + delta_pose.position.z**2) / 1000.0
+        self._lin_vel = dist * update_rate * np.sign(self._cmd_lin_vel)
         self._ang_vel = -delta_pose.rotation.angle_z.radians * update_rate
 
         # publish odom_frame -> footprint_frame