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)