From 24d7448784efa42a16aabcca67428142d820f033 Mon Sep 17 00:00:00 2001 From: Bill Fan Date: Fri, 1 Dec 2023 12:50:25 -0500 Subject: [PATCH] Corrected the covariance of odometry --- neato_node2/neato_node2/neato_node.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/neato_node2/neato_node2/neato_node.py b/neato_node2/neato_node2/neato_node.py index 994fe6d..a0f69b9 100644 --- a/neato_node2/neato_node2/neato_node.py +++ b/neato_node2/neato_node2/neato_node.py @@ -148,12 +148,12 @@ def main_run_loop(self): self.odom.pose.pose.position.y = self.y self.odom.pose.pose.position.z = 0.0 self.odom.pose.pose.orientation = quaternion - self.odom.pose.covariance = np.array([10**-1, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 10**-1, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 10**-1, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 10**5, 0.0, 0.0, - 0.0, 0.0, 0.0, 0, 10**5, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 10**5], dtype=np.float64) + self.odom.pose.covariance = np.array([10**-4, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 10**-4, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 10**-4, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, + 0.0, 0.0, 0.0, 0, 0.1, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.1], dtype=np.float64) self.odom.twist.twist.linear.x = dx/dt self.odom.twist.twist.angular.z = dth/dt transform = TransformStamped()