add covariance for odom.pose

jsk-ros-pkg · Oct 18, 2019 · 0b10dcb · 0b10dcb
1 parent 6a8accb
commit 0b10dcb
Showing 1 changed file with 4 additions and 3 deletions.
diff --git a/jsk_fetch_robot/jsk_fetch_startup/scripts/odom_corrector.py b/jsk_fetch_robot/jsk_fetch_startup/scripts/odom_corrector.py
@@ -14,12 +14,13 @@ def __init__(self):
             '~output', Odometry, queue_size=1)
         self.covariance = [1e-3, 0, 0, 0, 0, 0,
                            0, 1e-3, 0, 0, 0, 0,
-                           0, 0, 1e-6, 0, 0, 0,
-                           0, 0, 0, 1e-6, 0, 0,
-                           0, 0, 0, 0, 1e-6, 0,
+                           0, 0, 0, 0, 0, 0,
+                           0, 0, 0, 0, 0, 0,
+                           0, 0, 0, 0, 0, 0,
                            0, 0, 0, 0, 0, 1e-3]
 
     def _cb(self, msg):
+        msg.pose.covariance = self.covariance
         msg.twist.covariance = self.covariance
         self.pub.publish(msg)