From 73618a1e7e1aa42325b0efe95383490539f6d1df Mon Sep 17 00:00:00 2001 From: Rudolph Peter z003n7ht Date: Sun, 19 Feb 2017 20:23:11 +0100 Subject: [PATCH] Fixed conversion to meters instead of millimeters. --- nodes/cozmo_driver.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/nodes/cozmo_driver.py b/nodes/cozmo_driver.py index 7df2a78..5143c7d 100755 --- 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