Difference in movement between the input value and the actual movement
-Robot: waffle Pi -SBC: Raspberry Pi 3
OS installed in SBC: Raspbian OS installed in Remote PC: Ubuntu 16.04 LTS (Xenial Xerus) ROS version: 1.12.14
Hello everyone, I wrote two codes to derive my robot one is for linear movement and the other is for the rotational movement. the problem is that the error is always increased by increasing the traveled distance (rotation /angle). That means if I enter 360 degrees rotation angle the robot will rotate almost 340 degrees (20 degrees error) and this error will higher when rotating for example 720 degrees and so on. When I apply the code using "fake node" there is no errors. Does anyone meet the same problem and have ideas to solve it? Best wishes Below is my code (for the rotation movement):
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
#PI = 3.1415926535897 the basic position
def move():
print("Let's rotate your robot")
speed = input("Input your speed (degrees/sec):")
angle = input("Type your distance (degrees):")
clockwise = input("Clockwise?: ")
PI = 3.1415926535897
angular_speed = speed*2*PI/360
relative_angle = angle*2*PI/360
#vel_msg.linear.x=0
#vel_msg.linear.y=0
#vel_msg.linear.z=0
#vel_msg.angular.x = 0
#vel_msg.angular.y = 0
if clockwise:
vel_msg.angular.z = -abs(angular_speed)
else:
vel_msg.angular.z = abs(angular_speed)
t0 = rospy.Time.now().to_sec()
#rospy.spin() WRONG USE
r=rospy.Rate(100)
current_angle = 0
while(current_angle < relative_angle):
velocity_publisher.publish(vel_msg)
t1 = rospy.Time.now().to_sec()
current_angle = angular_speed*(t1-t0)
r.sleep()
vel_msg.angular.z = 0
velocity_publisher.publish(vel_msg)
#rospy.spin() . if we uncomment tje rospy.spin the program will not stop automatically but only when pressing Ctrl+C
if __name__ == '__main__':
try:
rospy.init_node('move', anonymous=True)
velocity_publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
vel_msg = Twist()
move()
except rospy.ROSInterruptException:
pass
Generally speaking when you test things on "fake nodes", you are using an ideal environment, however the behavior will be slightly different when you try the same code on a real machine because encoders, sensors etc. have their own inherent error.
I think the problem arises when you compute the
current_angle
in that way. The error of the calculations will increase over time because is the natural behavior of such logic. Have you tried to listen between transforms, E.g.: base_link -> Odom and calculate the relative transform?With the relative transform you will be able to compute with high precision the magnitude the robot steered and implement the logic to control when to stop.
However, you will have to take into account that this method also comes with errors, it will depends on the precision of your transform broadcaster.
Maybe this resource is usefull for your problem: Turtlebot Free Space Navigation