Robot missing its goal - wrong publish rate

edit

Hi guys,

I thought my odometry was bad, but after several tests I realized its not (or at least I have fixed it). So when moving my robot 1m forward the robots odometry in x-direction is +1 - same thing with rotation.

I tried a code to let my robot rotate by a specific angle by using odometry data (this) But it will not be able to reach it. So if he should reach an angle of 90 degrees, it will rotate from about 0-180-0-180 and so on. I guess that the frequency of getting the odometry data is to low: With a publishing rate of 10 (in the code) it publishes 10 times the same angle and will miss the 'right' angle (of course this is not right as its rotation is a constant movement). When I reduce the publishing rate to 1.0 or something the robot is moving slower and will not miss its goal like so much (it will only rotate with 10 degrees around it)

I guess I have to change a publishing frequency of odometry or something, but don't know where..

I hope somebody can help me with this

Code

#!/usr/bin/env python
import rospy
from nav_msgs.msg import Odometry
from tf.transformations import euler_from_quaternion, quaternion_from_euler
from geometry_msgs.msg import Twist
import math

roll = pitch = yaw = 0.0
target = 90
kp=0.5
def get_rotation (msg):
    global roll, pitch, yaw
    orientation_q = msg.pose.pose.orientation
    orientation_list = [orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w]
    (roll, pitch, yaw) = euler_from_quaternion (orientation_list)
    print yaw

rospy.init_node('rotate_robot')

sub = rospy.Subscriber ('/odom', Odometry, get_rotation)
pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
r = rospy.Rate(10)
command =Twist()

while not rospy.is_shutdown():
    #quat = quaternion_from_euler (roll, pitch,yaw)
    #print quat
    target_rad = target*math.pi/180
    command.angular.z = kp * (target_rad-yaw)
    pub.publish(command)
    print("target={} current:{}".format(target_rad,yaw))
    r.sleep()

Output with rate 10

target=1.57079632679 current:1.56676118476
target=1.57079632679 current:1.56676118476
target=1.57079632679 current:1.56676118476
target=1.57079632679 current:1.56676118476
target=1.57079632679 current:1.56676118476
target=1.57079632679 current:1.56676118476
target=1.57079632679 current:1.56676118476
target=1.57079632679 current:1.56676118476
target=1.57079632679 current:1.56676118476
target=1.57079632679 current:1.56676118476
1.84468741763
target=1.57079632679 current:1.84468741763
target=1.57079632679 current:1.84468741763
target=1.57079632679 current:1.84468741763
target=1.57079632679 current:1.84468741763
target=1.57079632679 current:1.84468741763
target=1.57079632679 current:1.84468741763
target=1.57079632679 current:1.84468741763
target=1.57079632679 current:1.84468741763
target=1.57079632679 current:1.84468741763
target=1.57079632679 current:1.84468741763
2.07234475519
target=1.57079632679 current:2.07234475519
target=1.57079632679 current:2.07234475519
target=1.57079632679 current:2.07234475519
target=1.57079632679 current:2.07234475519
target=1.57079632679 current:2.07234475519
target=1.57079632679 current:2.07234475519
target=1.57079632679 current:2.07234475519
target=1.57079632679 current:2.07234475519
target=1.57079632679 current:2.07234475519
target=1.57079632679 current:2.07234475519 ...