From: fdila Date: Tue, 14 Apr 2020 12:22:58 +0000 (+0200) Subject: fix odometry error when going straight X-Git-Url: http://git.leonardobizzoni.com/?a=commitdiff_plain;h=724110f58d49ecabd658643921d06627ca64dc98;p=pioneer-stm32 fix odometry error when going straight --- 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")