From 724110f58d49ecabd658643921d06627ca64dc98 Mon Sep 17 00:00:00 2001 From: fdila Date: Tue, 14 Apr 2020 14:22:58 +0200 Subject: [PATCH] fix odometry error when going straight --- .../catkin_ws/src/serial_bridge/scripts/serial_receiver.py | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/utils/catkin_ws/src/serial_bridge/scripts/serial_receiver.py b/utils/catkin_ws/src/serial_bridge/scripts/serial_receiver.py index 86459e9..e39e225 100755 --- a/utils/catkin_ws/src/serial_bridge/scripts/serial_receiver.py +++ b/utils/catkin_ws/src/serial_bridge/scripts/serial_receiver.py @@ -94,11 +94,10 @@ def serial_receiver(): odom_values = rotation_matrix.dot(translate_icc_matrix) + translate_back_matrix elif(d_time > 0): - odom_values = numpy.array([odom_values[0]+cos(odom_values[2])*(lin_vel/d_time), - odom_values[1]+sin(odom_values[2])*(lin_vel/d_time), + odom_values = numpy.array([odom_values[0]+(cos(odom_values[2])*(lin_vel*d_time)), + odom_values[1]+(sin(odom_values[2])*(lin_vel*d_time)), odom_values[2]]) - - + current_time = rospy.Time.now() odom_quat = tf.transformations.quaternion_from_euler(0, 0, odom_values[2]) odom_broadcaster.sendTransform((odom_values[0], odom_values[1], 0.), odom_quat, current_time, "base_link", "odom") -- 2.52.0