def turn2(d, r, init_time, init_yaw, current_time, current_yaw):
status = True
msg = "Turned successfully"
if init_yaw == None:
rospy.logerr(
"Initialization time is None, the startup was not correctly done!")
msg = "Initialization time is None, the startup was not correctly done!"
return False, msg
else:
orient = Orientation()
correct_yaw = orient.getCorrectedYaw(init_time, init_yaw, current_time,
current_yaw)
print " Current_yaw ->" + str(correct_yaw)
if d == 'NORTH':
pass
elif d == 'SOUTH':
init_yaw = init_yaw + radians(180)
elif d == 'EAST':
init_yaw = init_yaw + radians(90)
elif d == 'WEST':
init_yaw = init_yaw - radians(90)
else:
rospy.logerr("Direction is not correct")
print "Direction_yaw ->" + str(init_yaw)
if correct_yaw > init_yaw:
angle = correct_yaw - init_yaw
return turn(degrees(angle), -1)
else:
angle = init_yaw - correct_yaw
return turn(degrees(angle), 1)
def turn2(d, r, init_time, init_yaw, current_time, current_yaw):
status=True
msg="Turned successfully"
if init_yaw == None:
rospy.logerr("Initialization time is None, the startup was not correctly done!")
msg="Initialization time is None, the startup was not correctly done!"
return False, msg
else:
orient = Orientation()
correct_yaw = orient.getCorrectedYaw(init_time, init_yaw, current_time, current_yaw)
print " Current_yaw ->" + str(correct_yaw)
if d == 'NORTH':
pass
elif d == 'SOUTH':
init_yaw = init_yaw + radians(180)
elif d == 'EAST':
init_yaw = init_yaw + radians(90)
elif d == 'WEST':
init_yaw = init_yaw - radians(90)
else:
rospy.logerr("Direction is not correct")
print "Direction_yaw ->" + str(init_yaw)
if correct_yaw > init_yaw:
angle = correct_yaw - init_yaw
return turn(degrees(angle), -1)
else:
angle = init_yaw - correct_yaw
return turn(degrees(angle), 1)
#!/usr/bin/python
from orientation import Orientation
import rospy
if __name__ == '__main__':
rospy.init_node("euler_orientation_test")
orient = Orientation()
while True:
print "Raw yaw -> ", str(orient.getRawYaw())
print "Corrected yaw -> ", str(orient.getCorrectedYaw())
rospy.sleep(3)
#!/usr/bin/python
from orientation import Orientation
import rospy
if __name__ == '__main__':
rospy.init_node("euler_orientation_test")
orient = Orientation()
while True:
print "Raw yaw -> ", str(orient.getRawYaw())
print "Corrected yaw -> ", str(orient.getCorrectedYaw())
rospy.sleep(3)
