def gripper(pub_cmd, loop_rate, io_0):
global SPIN_RATE
global thetas
global current_io_0
global current_position
error = 0
spin_count = 0
at_goal = 0
current_io_0 = io_0
driver_msg = command()
driver_msg.destination = current_position
driver_msg.v = 1.0
driver_msg.a = 1.0
driver_msg.io_0 = io_0
pub_cmd.publish(driver_msg)
while (at_goal == 0):
if( abs(thetas[0]-driver_msg.destination[0]) < 0.0005 and \
abs(thetas[1]-driver_msg.destination[1]) < 0.0005 and \
abs(thetas[2]-driver_msg.destination[2]) < 0.0005 and \
abs(thetas[3]-driver_msg.destination[3]) < 0.0005 and \
abs(thetas[4]-driver_msg.destination[4]) < 0.0005 and \
abs(thetas[5]-driver_msg.destination[5]) < 0.0005 ):
#rospy.loginfo("Goal is reached!")
at_goal = 1
loop_rate.sleep()
if (spin_count > SPIN_RATE * 5):
pub_cmd.publish(driver_msg)
# rospy.loginfo("Just published again driver_msg")
spin_count = 0
spin_count = spin_count + 1
return error
def move_arm(pub_cmd, loop_rate, dest, vel, accel):
global thetas
global SPIN_RATE
error = 0
spin_count = 0
at_goal = 0
driver_msg = command()
driver_msg.destination = dest
driver_msg.v = vel
driver_msg.a = accel
driver_msg.io_0 = current_io_0
pub_cmd.publish(driver_msg)
loop_rate.sleep()
while (at_goal == 0):
if( abs(thetas[0]-driver_msg.destination[0]) < 0.0005 and \
abs(thetas[1]-driver_msg.destination[1]) < 0.0005 and \
abs(thetas[2]-driver_msg.destination[2]) < 0.0005 and \
abs(thetas[3]-driver_msg.destination[3]) < 0.0005 and \
abs(thetas[4]-driver_msg.destination[4]) < 0.0005 and \
abs(thetas[5]-driver_msg.destination[5]) < 0.0005 ):
at_goal = 1
#rospy.loginfo("Goal is reached!")
loop_rate.sleep()
if (spin_count > SPIN_RATE * 5):
pub_cmd.publish(driver_msg)
rospy.loginfo("Just published again driver_msg")
spin_count = 0
spin_count = spin_count + 1
return error
