def execute(self, userdata):
global leg_counter
leg_counter = leg_counter + 1
#print leg_counter
move_hand('open')
pubrel = rospy.Publisher('resetpiece', Int8, queue_size=1, latch=True)
pubrel.publish(1)
rospy.sleep(0.1)
pubrel.publish(0)
hand2 = rospy.Publisher('robot_has_piece',
Int8,
queue_size=1,
latch=True)
hand2.publish(0)
rospy.sleep(0.1)
pubpiecedone = rospy.Publisher('legdone',
Int8,
queue_size=1,
latch=True)
pubpiecedone.publish(0)
pubrel.publish(0)
rospy.sleep(0.2)
if leg_counter >= 4:
return 'outcome2'
return 'outcome1'
def execute(self, userdata):
#global cov
#cov.start()
#Path planning towards the piece
theplans = interface([-0.5,0.0,-0.75,0.0,1.39,0.0,0.0,-0.5,0.0])
for i,plan in enumerate(theplans):
set_robot_joints(plan)
theplans = interface([-1.15,0.05,-0.75,-1000.0,1.39,-1000.0,-1000.0,-0.5,-1000.0])
for i,plan in enumerate(theplans):
set_robot_joints(plan)
theplans = interface([-1.15,0.05,-0.48,-1000.0,1.39,-1000.0,-1000.0,-0.5,-1000.0])
for i,plan in enumerate(theplans):
set_robot_joints(plan)
move_hand('close')
rospy.sleep(0.5)
hand = rospy.Publisher('robot_gripper', Int8, queue_size=1,latch=True)
hand.publish(1)
rospy.sleep(0.5)
hand.publish(0)
pubpress = rospy.Publisher('pressure_e1', Int8, queue_size=1,latch=True)
pubpress.publish(1)
rospy.sleep(0.2)
#Path planning towards goal location
theplans = interface([0.0,0.05,-0.75,0.0,1.39,0.0,0.0,-0.5,0.0])
#print theplans
for i,plan in enumerate(theplans):
set_robot_joints(plan)
rospy.sleep(1)
hand2 = rospy.Publisher('robot_has_piece', Int8, queue_size=1,latch=True)
hand2.publish(1)
rospy.sleep(0.1)
#cov.stop()
#cov.save()
return 'outcome1'
def execute(self, userdata):
theplans = interface(
[0.0, 0.05, -0.75, 0.0, 1.39, 0.0, 0.0, -0.5, 0.0])
for i, plan in enumerate(theplans):
set_robot_joints(plan)
move_hand('close')
rospy.sleep(0.5)
hand = rospy.Publisher('robot_gripper', Int8, queue_size=1, latch=True)
hand.publish(1)
rospy.sleep(0.5)
hand.publish(0)
pubpress = rospy.Publisher('pressure_e1',
Int8,
queue_size=1,
latch=True)
pubpress.publish(1)
rospy.sleep(0.2)
rospy.sleep(1)
hand2 = rospy.Publisher('robot_has_piece',
Int8,
queue_size=1,
latch=True)
hand2.publish(1)
rospy.sleep(0.1)
return 'outcome1'
def execute(self, userdata):
#global cov
#cov.start()
# If online the assert_normal value will be set by a publisher subscriber
# interaction to force the drop pathway
if assert_normal == 1:
trigger_drop_pathway = 1
else:
# Randomly assign the value
trigger_drop_pathway = random.randint(0, 10)
theplans = interface(
[-0.5, 0.0, -0.75, 0.0, 1.39, 0.0, 0.0, -0.5, 0.0])
for i, plan in enumerate(theplans):
set_robot_joints(plan)
theplans = interface([
-1.15, 0.05, -0.75, -1000.0, 1.39, -1000.0, -1000.0, -0.5, -1000.0
])
for i, plan in enumerate(theplans):
set_robot_joints(plan)
theplans = interface([
-1.15, 0.05, -0.48, -1000.0, 1.39, -1000.0, -1000.0, -0.5, -1000.0
])
for i, plan in enumerate(theplans):
set_robot_joints(plan)
move_hand('close')
rospy.sleep(0.5)
hand = rospy.Publisher('robot_gripper', Int8, queue_size=1, latch=True)
hand.publish(1)
rospy.sleep(0.5)
hand.publish(0)
pubpress = rospy.Publisher('pressure_e1',
Int8,
queue_size=1,
latch=True)
pubpress.publish(1)
rospy.sleep(0.2)
#Path planning towards goal location
theplans = interface(
[0.0, 0.05, -0.75, 0.0, 1.39, 0.0, 0.0, -0.5, 0.0])
#print theplans
for i, plan in enumerate(theplans):
set_robot_joints(plan)
rospy.sleep(1)
hand2 = rospy.Publisher('robot_has_piece',
Int8,
queue_size=1,
latch=True)
hand2.publish(1)
rospy.sleep(0.1)
# Introduce state fork
if trigger_drop_pathway == 1:
return 'outcome2'
else:
return 'outcome1'
def execute(self, userdata):
#global cov
#cov.start()
#Path planning towards the piece
#x = random.randint(0, 10)
trigger_drop_pathway = 2
theplans = interface(
[-0.5, 0.0, -0.75, 0.0, 1.39, 0.0, 0.0, -0.5, 0.0])
for i, plan in enumerate(theplans):
set_robot_joints(plan)
theplans = interface([
-1.15, 0.05, -0.75, -1000.0, 1.39, -1000.0, -1000.0, -0.5, -1000.0
])
for i, plan in enumerate(theplans):
set_robot_joints(plan)
theplans = interface([
-1.15, 0.05, -0.48, -1000.0, 1.39, -1000.0, -1000.0, -0.5, -1000.0
])
for i, plan in enumerate(theplans):
set_robot_joints(plan)
move_hand('close')
rospy.sleep(0.5)
hand = rospy.Publisher('robot_gripper', Int8, queue_size=1, latch=True)
hand.publish(1)
rospy.sleep(0.5)
hand.publish(0)
pubpress = rospy.Publisher('pressure_e1',
Int8,
queue_size=1,
latch=True)
pubpress.publish(1)
rospy.sleep(0.2)
#Path planning towards goal location
theplans = interface(
[0.0, 0.05, -0.75, 0.0, 1.39, 0.0, 0.0, -0.5, 0.0])
#print theplans
for i, plan in enumerate(theplans):
set_robot_joints(plan)
rospy.sleep(1)
hand2 = rospy.Publisher('robot_has_piece',
Int8,
queue_size=1,
latch=True)
hand2.publish(1)
rospy.sleep(0.1)
if trigger_drop_pathway == 2:
#Will I need to thread this
return 'outcome2'
#cov.stop()
#cov.save()
return 'outcome1'
def Reset():
move_hand('close')
# Hand raised:
targ = [0.5,0.05,-0.77,0.0,0.0,1.4,0.0,-0.17,0.0]
targ = fixTarget(targ)
theplans = interface(targ)
followPlans(theplans)
rospy.sleep(4)
def execute(self, userdata):
#Announce start of Reset
pubReset = rospy.Publisher('robot_reset', Robot, queue_size=1,latch=True)
pubReset.publish(1)
rospy.sleep(0.1)
#Target position of reset: centre of hand in x=0.0, y=0.2, z=0.585
set_robot_joints([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
move_hand('open')
rospy.sleep(0.5)
#Select random initial position for the robot
set_robot_joints([0.0, 0.0, 0.0, 0.0, random.uniform(-0.5,0.5), 0.0, 0.0, random.uniform(-0.5,0.5), 0.0, 0.0, random.uniform(-0.5,0.5),0.0])
return 'outcome1'
def execute(self, userdata):
global leg_counter
leg_counter = leg_counter + 1
move_hand('open')
dropped = rospy.Publisher('dropped_piece',
Int8,
queue_size=1,
latch=True)
dropped.publish(1)
rospy.sleep(0.2)
dropped.publish(0)
return 'outcome1'
def execute(self, userdata):
#global cov
#cov.start()
print 'Piece is released'
global leg_counter
leg_counter = leg_counter + 1
global good_legs
good_legs = good_legs + 1
print leg_counter
move_hand('open')
rospy.sleep(0.5)
pubpress = rospy.Publisher('pressure_e1', Int8, queue_size=1,latch=True)
pubpress.publish(0)
rospy.sleep(0.1)
pubrel = rospy.Publisher('resetpiece', Int8, queue_size=1,latch=True)
pubrel.publish(1)
rospy.sleep(0.05)
pubrel.publish(0)
hand2 = rospy.Publisher('robot_has_piece', Int8, queue_size=1,latch=True)
hand2.publish(0)
rospy.sleep(0.1)
pubpiecedone = rospy.Publisher('legdone', Int8, queue_size=1,latch=True)
pubpiecedone.publish(1)
#pubrel.publish(0)
rospy.sleep(0.1)
pubpiecedone.publish(0)
rospy.sleep(0.1)
if leg_counter < 4:
#cov.stop()
#cov.save()
return 'outcome1'
else:
leg_counter=0
if good_legs >= 4:
pubtabledone = rospy.Publisher('tabledone', Int8, queue_size=1,latch=True)
pubtabledone.publish(1)
rospy.sleep(0.1)
pubtableontime = rospy.Publisher('tableontime', Int8, queue_size=1,latch=True)
if (time.time()-table_timeout)<=300:
pubtableontime.publish(1)
rospy.sleep(0.1)
print 'Table on time'
else:
pubtableontime.publish(0)
rospy.sleep(0.1)
print 'Table not on time'
#cov.stop()
#cov.save()
return 'outcome2'
def execute(self, userdata):
print 'Piece is released'
move_hand('open')
rospy.sleep(0.5)
pubpress = rospy.Publisher('pressure_e1', Int8, queue_size=1,latch=True)
pubpress.publish(0)
rospy.sleep(0.1)
pubrel = rospy.Publisher('resetpiece', Int8, queue_size=1,latch=True)
pubrel.publish(1)
rospy.sleep(0.1)
print 'Interaction done'
pubdone = rospy.Publisher('done', Int8, queue_size=1,latch=True)
pubdone.publish(1)
rospy.sleep(0.1)
return 'outcome1'
def execute(self, userdata):
#Reset position with hand open
#global cov
#cov.start()
theplans = interface([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
for i,plan in enumerate(theplans):
set_robot_joints(plan)
rospy.sleep(1)
set_robot_joints([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
move_hand('open')
rospy.sleep(0.5)
global table_timeout
table_timeout = time.time()
#cov.stop()
#cov.save()
return 'outcome1'
def execute(self, userdata):
global leg_counter
leg_counter = leg_counter + 1
move_hand('open')
if assert_normal != 1:
subprocess.Popen([
"python",
"/home/harrison/catkin_ws/src/table_simulator/scripts/bdi_test_generator/drop_loop_run.py",
str(file_number)
])
legDrop = rospy.Publisher('Leg_Drop', Int8, queue_size=1, latch=True)
legDrop.publish(1)
rospy.sleep(1)
legDrop.publish(0)
return 'outcome1'
def execute(self, userdata):
#Reset position with hand open
#global cov
#cov.start()
theplans = interface([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
for i, plan in enumerate(theplans):
set_robot_joints(plan)
rospy.sleep(1)
set_robot_joints(
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
move_hand('open')
rospy.sleep(0.5)
global table_timeout
table_timeout = time.time()
#cov.stop()
#cov.save()
return 'outcome1'
def execute(self, userdata):
global leg_counter
leg_counter = leg_counter + 1
#print leg_counter
move_hand('open')
pubrel = rospy.Publisher('resetpiece', Int8, queue_size=1,latch=True)
pubrel.publish(1)
rospy.sleep(0.1)
pubrel.publish(0)
hand2 = rospy.Publisher('robot_has_piece', Int8, queue_size=1,latch=True)
hand2.publish(0)
rospy.sleep(0.1)
pubpiecedone = rospy.Publisher('legdone', Int8, queue_size=1,latch=True)
pubpiecedone.publish(0)
pubrel.publish(0)
rospy.sleep(0.2)
if leg_counter >= 4:
return 'outcome2'
return 'outcome1'
def Move():
# Move hand laterally to a point above the piece
targ = [1.5,0.05,-0.77,0.0,0.0,1.4,0.0,-0.17,1000.0]
targ = fixTarget(targ)
theplans = interface(targ)
followPlans(theplans)
move_hand('open')
rospy.sleep(2)
# Bring the hand down to the piece
targ = [1.5,0.05,-0.67,1000.0,0,1.4,0.0,-0.17,1000.0]
targ = fixTarget(targ)
theplans = interface(targ)
followPlans(theplans)
rospy.sleep(3)
# Close the hand
move_hand('close')
rospy.sleep(1)
# Raise the hand
targ = [1.5,0.05,-0.77,0.0,0.0,1.4,0.0,-0.17,1000.0]
targ = fixTarget(targ)
theplans = interface(targ)
followPlans(theplans)
rospy.sleep(1)
# Path planning towards goal location
targ = [0.0,0.05,-0.77,0.0,0.0,1.4,0.0,-0.17,0.0]
targ = fixTarget(targ)
theplans = interface(targ)
followPlans(theplans)
rospy.sleep(5)
def Release():
move_hand('open')
rospy.sleep(1)
def execute(self, userdata):
#global cov
#cov.start()
print 'Piece is released'
global leg_counter
leg_counter = leg_counter + 1
global good_legs
good_legs = good_legs + 1
print leg_counter
move_hand('open')
rospy.sleep(0.5)
pubpress = rospy.Publisher('pressure_e1',
Int8,
queue_size=1,
latch=True)
pubpress.publish(0)
rospy.sleep(0.1)
pubrel = rospy.Publisher('resetpiece', Int8, queue_size=1, latch=True)
pubrel.publish(1)
rospy.sleep(0.05)
pubrel.publish(0)
hand2 = rospy.Publisher('robot_has_piece',
Int8,
queue_size=1,
latch=True)
hand2.publish(0)
rospy.sleep(0.1)
pubpiecedone = rospy.Publisher('legdone',
Int8,
queue_size=1,
latch=True)
pubpiecedone.publish(1)
#pubrel.publish(0)
rospy.sleep(0.1)
pubpiecedone.publish(0)
rospy.sleep(0.1)
if leg_counter < 4:
#cov.stop()
#cov.save()
return 'outcome1'
else:
leg_counter = 0
if good_legs >= 4:
pubtabledone = rospy.Publisher('tabledone',
Int8,
queue_size=1,
latch=True)
pubtabledone.publish(1)
rospy.sleep(0.1)
pubtableontime = rospy.Publisher('tableontime',
Int8,
queue_size=1,
latch=True)
if (time.time() - table_timeout) <= 300:
pubtableontime.publish(1)
rospy.sleep(0.1)
print 'Table on time'
else:
pubtableontime.publish(0)
rospy.sleep(0.1)
print 'Table not on time'
#cov.stop()
#cov.save()
return 'outcome2'
