def rotateTo(angle,
speed=0.4): # rotate to a certain angle, with default speed 0.4
global rate
rpy = point_control.get_imu()
angle_error = angle - rpy[2]
print "<rotateTo> Input angle:", angle, "current angle: ", rpy[
2], "angle error:", angle_error
# print "angle_error: ", angle_error,"angle_now: ", rpy[2]
while abs(angle_error) > 5:
rpy = point_control.get_imu()
# print "angle_error: ", angle_error,"angle_now: ", rpy[2]
angle_error = angle - rpy[2]
if angle_error > 0:
velocity_control.vel_control(0,
speed) # low speed to rotate to front
else:
velocity_control.vel_control(0, -speed)
# print "angle_error", angle_error
rate.sleep()
print "<rotateTo> Rotate finished at angle: ", rpy[2]
velocity_control.vel_control(0, 0)
def rotateTo(angle): # rotate to a certain angle, with default speed 0.4
rate = rospy.Rate(100)
# global rate
rpy = point_control.get_imu()
angle_error = angle - rpy[2]
if (angle_error > 180):
angle_error -= 360
elif (angle_error < -180):
# print "angle_error,", angle_error
angle_error += 360
# print "angle_error,", angle_error
print "<rotateTo> Input angle:", angle, "current angle: ", rpy[
2], "angle error:", angle_error
# print "angle_error: ", angle_error,"angle_now: ", rpy[2]
while abs(angle_error) > 5:
rpy = point_control.get_imu()
# print "angle_error: ", angle_error,"angle_now: ", rpy[2]
angle_error = angle - rpy[2]
if (angle_error > 180):
angle_error -= 360
elif (angle_error < -180):
angle_error += 360
if angle_error > 0:
speed = max(angle_error * 0.02, 0.4) #lowerspped, 0.4
speed = min(speed, 2.0) # upperspeed, 2
velocity_control.vel_control(0,
speed) # low speed to rotate to front
else:
speed = min(angle_error * 0.02, -0.4)
speed = max(speed, -2.0)
velocity_control.vel_control(0, speed)
# print "angle_error", angle_error
rate.sleep()
print "<rotateTo> Rotate finished at angle: ", rpy[2]
velocity_control.vel_control(0, 0)
carState = state_moveBack
else:
if carState == state_moveToPoint:
print "[state] moveToPoint. "
print "POS:", [targetPointX, targetPointY]
point_control.PointMove(targetPointX - 0.3, targetPointY, 0)
print "Stopped at target point."
carState = state_rotateToFront
else:
if carState == state_rotateToFront:
print "[state] rotate to front. "
rpy = point_control.get_imu()
angle_error = 0 - rpy[2]
while abs(angle_error) > 5:
rpy = point_control.get_imu()
angle_error = 0 - rpy[2]
if angle_error > 0:
velocity_control.vel_control(
0, 0.4) # low speed to rotate to front
else:
velocity_control.vel_control(0, -0.4)
print "angle_error", angle_error
rate.sleep()
velocity_control.vel_control(0, 0)