def main():
rospy.init_node("apNet_demo", anonymous=True)
gripper_controller = GripperController()
ur5_controller = UR5Controller()
# init pose
gripper_controller.open()
ur5_controller.home()
print('Starting APCNet Demo')
vel_controller = APCNetDemo()
while not rospy.is_shutdown():
try:
if vel_controller.state == 'home':
arm_is_home = ur5_controller.home()
rospy.sleep(1)
if arm_is_home:
print('home --> reach')
gripper_controller.open()
vel_controller.state = 'reach'
elif vel_controller.state == 'reach':
vel_controller.joint_command_pub.publish(vel_controller.joint_traj)
rospy.sleep(0.008) # 125 Hz
elif vel_controller.state == 'grasp':
gripper_controller.close()
vel_controller.state = 'place'
print('grasp --> place')
rospy.sleep(1)
elif vel_controller.state == 'place':
is_arrived = ur5_controller.move2rest_pose()
# rospy.sleep(1)
if is_arrived:
print('place --> drop')
gripper_controller.open()
vel_controller.state = 'home'
print('drop --> home')
rospy.sleep(0.5)
except KeyboardInterrupt:
print "Pressed \'Ctrl + c\', Shutting down ..."
sys.exit()
