def execute(self, userdata):
param_objects = rospy.get_param(userdata.pose_in['name'])['objects']
objects_to_pick = [x['name'] for x in param_objects]
check = self.check_rv20()
publish_status('I am checking if it is <br> a R20 or V20.')
if check.result in objects_to_pick:
obj = userdata.object_in
for o in param_objects:
if o['name'] == check.result:
object_out = o
break
object_out['label'] = obj['label'].replace('RV20', check.result)
userdata.object_out = object_out
res = self.get_best_backplate_location(
obj_name=object_out['label'], objects_to_pick=objects_to_pick)
userdata.backplate_pose_out = res.place_loc
return 'correct'
userdata.object_out = userdata.object_in
userdata.backplate_pose_out = []
return 'incorrect'
def go(self, req):
reset()
print "Starting SMACH"
publish_status("I got the specifications <br> and will start now.")
self.sm.execute()
publish_status("I am finished.")
return EmptyResponse()
def go(self, req):
reset()
print "Starting SMACH"
publish_status("I got the specifications <br> and will start now.")
self.sm.execute()
publish_status("I am finished.")
return EmptyResponse()
def execute(self, userdata):
backplate_pose = userdata.backplate_pose_in
obj = userdata.object_in
place_on_platform(obj, backplate_pose)
publish_status('I am placing ' + obj['name'] + '<br> on my back plate.')
obj['backplate_location'] = backplate_pose
obj_msg = dict_to_backplate_object_msg(obj)
self.add_to_platform(obj_msg)
return 'success'
def execute(self, userdata):
backplate_pose = userdata.backplate_pose_in
obj = userdata.object_in
pick_from_platform(obj, backplate_pose)
publish_status('I am getting the ' + obj['name'] + '<br> from my back plate.')
obj['backplate_location'] = backplate_pose
obj_msg = dict_to_backplate_object_msg(obj)
self.remove_from_platform(obj=obj_msg)
return 'success'
def execute(self, userdata):
backplate_pose = userdata.backplate_pose_in
obj = userdata.object_in
place_on_platform(obj, backplate_pose)
publish_status('I am placing ' + obj['name'] +
'<br> on my back plate.')
obj['backplate_location'] = backplate_pose
obj_msg = dict_to_backplate_object_msg(obj)
self.add_to_platform(obj_msg)
return 'success'
def execute(self, userdata):
backplate_pose = userdata.backplate_pose_in
obj = userdata.object_in
pick_from_platform(obj, backplate_pose)
publish_status('I am getting the ' + obj['name'] +
'<br> from my back plate.')
obj['backplate_location'] = backplate_pose
obj_msg = dict_to_backplate_object_msg(obj)
self.remove_from_platform(obj=obj_msg)
return 'success'
class SwitchObjectHoleManagerOnState(smach.State):
def __init__(self, holes=False):
smach.State.__init__(self, outcomes=['done'], input_keys=['pose_in'])
init_globals_detect()
self.topic_str = ''
self.holes = holes
if not holes:
self.topic_str = '/vision/object_manager/switch_on'
self.reset = rospy.ServiceProxy('/vision/object_manager/reset',
Empty)
else:
self.topic_str = '/vision/hole_manager/switch_on'
self.reset = rospy.ServiceProxy('/vision/hole_manager/reset',
Empty)
self.switch_on = rospy.ServiceProxy(self.topic_str,
SwitchOnForLocation)
self.vision_rgb_switch_side = rospy.ServiceProxy(
"/vision/rgb/switch_side", SetSide)
self.vision_depth_switch_side = rospy.ServiceProxy(
"/vision/depth/switch_side", SetSide)
def execute(self, userdata):
pose = userdata.pose_in
side = get_pickup_side(pose['name'])
try:
self.reset()
rospy.wait_for_service(self.topic_str)
self.vision_rgb_switch_side(side=side)
self.vision_depth_switch_side(side=side)
self.switch_on(userdata.pose_in['name'])
except rospy.ServiceException, e:
print "Service call failed: %s" % e
rospy.sleep(1.0)
if self.holes:
publish_status('I am scanning the service area <br> for holes.')
else:
publish_status('I am scanning the service area <br> for objects.')
return 'done'
def execute(self, userdata):
param_objects = rospy.get_param(userdata.pose_in['name'])['objects']
objects_to_pick = [x['name'] for x in param_objects]
check = self.check_rv20()
publish_status('I am checking if it is <br> a R20 or V20.')
if check.result in objects_to_pick:
obj = userdata.object_in
for o in param_objects:
if o['name'] == check.result:
object_out = o
break
object_out['label'] = obj['label'].replace('RV20', check.result)
userdata.object_out = object_out
res = self.get_best_backplate_location(obj_name=object_out['label'], objects_to_pick=objects_to_pick)
userdata.backplate_pose_out = res.place_loc
return 'correct'
userdata.object_out = userdata.object_in
userdata.backplate_pose_out = []
return 'incorrect'
def execute(self, userdata):
obj = userdata.object_in
# offset detected by object detection
offset = userdata.point_in
pose = userdata.pose_in
# to which side
side = get_pickup_side(pose['name'])
quat = [offset.orientation.x, offset.orientation.y, offset.orientation.z, offset.orientation.w]
r, p, y = tf.transformations.euler_from_quaternion(quat)
new_grip_point = [x for x in grip_point]
new_grip_point[0] += offset.position.x
new_grip_point[1] -= offset.position.y
new_grip_point[2] += get_pick_height_for_obj(obj['label'])
publish_status('I am grasping object ' + str(obj['name']) + '.')
## TODO check HEIGHT FOR HIGH OBJECTS
if NO_ARM:
return 'success'
conf = None
try:
conf = call_ik_solver(new_grip_point, side=side)
except:
print "got no config from IK Solver"
if len(conf) == 0:
return 'too_far'
else:
conf = np.array(conf)
grip_angle = get_angle(conf[4], y)
straight_angle = conf[4]
conf[4] = grip_angle
height = new_grip_point[2]
new_grip_point[2] += PRE_GRIP_HEIGHT
confs = []
rotate = rotate_only(side=side)
confs.append(rotate)
first = None
gripX = new_grip_point[0]
gripY = new_grip_point[1]
while new_grip_point[2] > height:
new_grip_point[2] -= STEP_SIZE
new_grip_point[2] = max(height, new_grip_point[2])
new_grip_point[0] = gripX
new_grip_point[1] = gripY
conf2 = []
while len(conf2) == 0:
try:
print 'get conf', new_grip_point
conf2 = call_ik_solver(new_grip_point, side=side)
print conf2
except:
#new_grip_point[0] -= 0.01 #get closer to solve pre-grip
print 'got not config'
return 'too_far'
if len(conf2) > 0:
conf2 = np.array(conf2)
print conf2
new_grip_point[0] -= 0.005 #get closer to solve pre-grip
conf2[4] = conf[4]
if first is None:
first = [x for x in conf2]
confs.append(conf2)
gripper_close(False)
confs.append(conf)
if not go(confs):
return 'failed'
if userdata.object_in['label'] == 'M20_100_h':
obj = userdata.object_in
### TODO set correct rotation
obj['rotation'] = -y - (grip_angle - straight_angle)
userdata.object_out = obj
print "rotation is", obj['rotation'], y, grip_angle - straight_angle
gripper_close(True, heavy=True)
else:
gripper_close(True)
if not go([first]):
return 'failed_after_grip'
return 'success'
def execute(self, userdata):
# offset detected by object detection
offset = userdata.point_in
pose = userdata.pose_in
obj = userdata.object_in
side = get_pickup_side(pose['name'])
# to which side
publish_status('I am placing the object <br> on the service area.')
quat = [offset.orientation.x, offset.orientation.y, offset.orientation.z, offset.orientation.w]
r, p, y = tf.transformations.euler_from_quaternion(quat)
new_grip_point = [x for x in grip_point]
new_grip_point[0] += offset.position.x
new_grip_point[1] -= offset.position.y
new_grip_point[2] += get_place_height_for_obj(obj['label'])
if NO_ARM:
return 'success'
conf = None
try:
conf = call_ik_solver(new_grip_point, side=side)
except:
print "got no config from IK Solver"
if len(conf) == 0:
return 'too_far'
else:
conf = np.array(conf)
straight = conf[4]
grip_angle = get_angle(conf[4], y)
conf[4] = grip_angle
new_grip_point[2] += PRE_PLACE_HEIGHT
rotate = rotate_only(side=side)
gripX = new_grip_point[0]
gripY = new_grip_point[1]
new_grip_point[0] = gripX
new_grip_point[1] = gripY
conf2 = []
while len(conf2) == 0:
try:
print 'get conf', new_grip_point
conf2 = call_ik_solver(new_grip_point, side=side)
print conf2
except:
# new_grip_point[0] -= 0.01 #get closer to solve pre-grip
print 'got not config'
return 'too_far'
if len(conf2) > 0:
conf2 = np.array(conf2)
new_grip_point[0] -= 0.005 #get closer to solve pre-grip
conf2[4] = conf[4]
go([rotate])
print obj
if obj['label'] == 'M20_100_h' and obj['type'] == 'PPT':
m20_rot = obj['rotation']
yaw = self.get_rotation(y, straight, m20_rot)
if yaw is None:
print "rotate screw!"
yaw = self.get_rotation(y, straight, math.pi - abs(m20_rot))
temp_y = get_angle(straight, y - math.pi / 2.)
scnd_tmp = temp_y + math.pi
if scnd_tmp > joints['arm_joint_5']['max']:
scnd_tmp -= 2. * math.pi
conf[4] = temp_y
conf2[4] = temp_y
go([conf2, conf])
gripper_close(False)
go([conf2])
conf[4] = scnd_tmp
conf2[4] = scnd_tmp
go([conf2, conf])
gripper_close(True, heavy=True)
go([conf2])
conf[4] = yaw
conf2[4] = yaw
if not go([conf2, conf]):
return 'failed'
gripper_close(False)
#rotate??
if False:
rotate_wiggle = [x for x in conf]
rotate_wiggle[4] -= 0.1
rotate_wiggle2 = [x for x in conf]
rotate_wiggle2[4] += 0.1
if not go([rotate_wiggle, rotate_wiggle2, conf]):
return 'failed'
if not go([conf2]):
return 'failed_after_place'
return 'success'
def execute(self, userdata):
objects_on_plate = get_objects_on_plate()
pickup_location_object_map = self.get_pickup_location_to_object_map()
# sort after length of objects
pickup_locs_sorted = sorted(
pickup_location_object_map,
key=lambda k: len(pickup_location_object_map[k]),
reverse=True)
# toDo loc up location with type end
best_pickup_loc = self.get_end_locarion()
items_on_plate = len(objects_on_plate)
if items_on_plate == 0 and len(pickup_location_object_map) > 0:
# location with most items to pic
best_pickup_loc['name'] = pickup_locs_sorted[0]
best_pickup_loc['type'] = 'Pickup'
publish_status("I am going to " + str(best_pickup_loc['name']) +
' to pickup objects.')
print "Pickup, since nothing on plate"
# place items
elif items_on_plate == 3 or (len(pickup_location_object_map) == 0
and items_on_plate > 0):
best_pickup_loc['name'] = self.get_location_with_most_obj_to_place(
objects_on_plate)
best_pickup_loc['type'] = 'Place'
publish_status("My back plate is full, I am going to " +
str(best_pickup_loc['name']) + ' to place objects.')
print "Place, since plate full or nothing left to pick"
#choose extra item to pick
else:
found = False
for loc_key in pickup_locs_sorted:
objects_to_pickup = pickup_location_object_map[loc_key]
if len(objects_to_pickup) <= 3 - items_on_plate:
found = True
for obj in objects_to_pickup:
req = self.can_place_object(obj['name'])
if not req.can_place:
found = False
break
if found:
best_pickup_loc['name'] = loc_key
best_pickup_loc['type'] = 'Pickup'
publish_status("I am going to " +
str(best_pickup_loc['name']) +
'<br> to pickup objects.')
print "Pickup, can kill other platform"
if not found and items_on_plate > 0:
best_pickup_loc[
'name'] = self.get_location_with_most_obj_to_place(
objects_on_plate)
best_pickup_loc['type'] = 'Place'
publish_status("I am going to " +
str(best_pickup_loc['name']) +
'<br> to place objects.')
print "place since no other platform to kill"
if best_pickup_loc['type'] == 'End':
print 'Go To end'
userdata.pose_out = best_pickup_loc
if self.last_chosen_position is not None and self.last_chosen_position['name'] == \
best_pickup_loc['name']:
self.counter += 1
if self.counter > 2:
# delete location from param server
locations_param = rospy.get_param('locations')
for loc in locations_param:
if loc['name'] == self.last_chosen_position['name']:
locations_param.remove(loc)
break
rospy.set_param('locations', locations_param)
self.counter = 0
else:
self.counter = 0
self.last_chosen_position = best_pickup_loc
return 'done'
def execute(self, userdata):
obj = userdata.object_in
# offset detected by object detection
offset = userdata.point_in
pose = userdata.pose_in
# to which side
side = get_pickup_side(pose['name'])
quat = [
offset.orientation.x, offset.orientation.y, offset.orientation.z,
offset.orientation.w
]
r, p, y = tf.transformations.euler_from_quaternion(quat)
new_grip_point = [x for x in grip_point]
new_grip_point[0] += offset.position.x
new_grip_point[1] -= offset.position.y
new_grip_point[2] += get_pick_height_for_obj(obj['label'])
publish_status('I am grasping object ' + str(obj['name']) + '.')
## TODO check HEIGHT FOR HIGH OBJECTS
if NO_ARM:
return 'success'
conf = None
try:
conf = call_ik_solver(new_grip_point, side=side)
except:
print "got no config from IK Solver"
if len(conf) == 0:
return 'too_far'
else:
conf = np.array(conf)
grip_angle = get_angle(conf[4], y)
straight_angle = conf[4]
conf[4] = grip_angle
height = new_grip_point[2]
new_grip_point[2] += PRE_GRIP_HEIGHT
confs = []
rotate = rotate_only(side=side)
confs.append(rotate)
first = None
gripX = new_grip_point[0]
gripY = new_grip_point[1]
while new_grip_point[2] > height:
new_grip_point[2] -= STEP_SIZE
new_grip_point[2] = max(height, new_grip_point[2])
new_grip_point[0] = gripX
new_grip_point[1] = gripY
conf2 = []
while len(conf2) == 0:
try:
print 'get conf', new_grip_point
conf2 = call_ik_solver(new_grip_point, side=side)
print conf2
except:
#new_grip_point[0] -= 0.01 #get closer to solve pre-grip
print 'got not config'
return 'too_far'
if len(conf2) > 0:
conf2 = np.array(conf2)
print conf2
new_grip_point[0] -= 0.005 #get closer to solve pre-grip
conf2[4] = conf[4]
if first is None:
first = [x for x in conf2]
confs.append(conf2)
gripper_close(False)
confs.append(conf)
if not go(confs):
return 'failed'
if userdata.object_in['label'] == 'M20_100_h':
obj = userdata.object_in
### TODO set correct rotation
obj['rotation'] = -y - (grip_angle - straight_angle)
userdata.object_out = obj
print "rotation is", obj[
'rotation'], y, grip_angle - straight_angle
gripper_close(True, heavy=True)
else:
gripper_close(True)
if not go([first]):
return 'failed_after_grip'
return 'success'
def execute(self, userdata):
# offset detected by object detection
offset = userdata.point_in
pose = userdata.pose_in
obj = userdata.object_in
side = get_pickup_side(pose['name'])
# to which side
publish_status('I am placing the object <br> on the service area.')
quat = [
offset.orientation.x, offset.orientation.y, offset.orientation.z,
offset.orientation.w
]
r, p, y = tf.transformations.euler_from_quaternion(quat)
new_grip_point = [x for x in grip_point]
new_grip_point[0] += offset.position.x
new_grip_point[1] -= offset.position.y
new_grip_point[2] += get_place_height_for_obj(obj['label'])
if NO_ARM:
return 'success'
conf = None
try:
conf = call_ik_solver(new_grip_point, side=side)
except:
print "got no config from IK Solver"
if len(conf) == 0:
return 'too_far'
else:
conf = np.array(conf)
straight = conf[4]
grip_angle = get_angle(conf[4], y)
conf[4] = grip_angle
new_grip_point[2] += PRE_PLACE_HEIGHT
rotate = rotate_only(side=side)
gripX = new_grip_point[0]
gripY = new_grip_point[1]
new_grip_point[0] = gripX
new_grip_point[1] = gripY
conf2 = []
while len(conf2) == 0:
try:
print 'get conf', new_grip_point
conf2 = call_ik_solver(new_grip_point, side=side)
print conf2
except:
# new_grip_point[0] -= 0.01 #get closer to solve pre-grip
print 'got not config'
return 'too_far'
if len(conf2) > 0:
conf2 = np.array(conf2)
new_grip_point[0] -= 0.005 #get closer to solve pre-grip
conf2[4] = conf[4]
go([rotate])
print obj
if obj['label'] == 'M20_100_h' and obj['type'] == 'PPT':
m20_rot = obj['rotation']
yaw = self.get_rotation(y, straight, m20_rot)
if yaw is None:
print "rotate screw!"
yaw = self.get_rotation(y, straight, math.pi - abs(m20_rot))
temp_y = get_angle(straight, y - math.pi / 2.)
scnd_tmp = temp_y + math.pi
if scnd_tmp > joints['arm_joint_5']['max']:
scnd_tmp -= 2. * math.pi
conf[4] = temp_y
conf2[4] = temp_y
go([conf2, conf])
gripper_close(False)
go([conf2])
conf[4] = scnd_tmp
conf2[4] = scnd_tmp
go([conf2, conf])
gripper_close(True, heavy=True)
go([conf2])
conf[4] = yaw
conf2[4] = yaw
if not go([conf2, conf]):
return 'failed'
gripper_close(False)
#rotate??
if False:
rotate_wiggle = [x for x in conf]
rotate_wiggle[4] -= 0.1
rotate_wiggle2 = [x for x in conf]
rotate_wiggle2[4] += 0.1
if not go([rotate_wiggle, rotate_wiggle2, conf]):
return 'failed'
if not go([conf2]):
return 'failed_after_place'
return 'success'
def execute(self, userdata):
objects_on_plate = get_objects_on_plate()
pickup_location_object_map = self.get_pickup_location_to_object_map()
# sort after length of objects
pickup_locs_sorted = sorted(pickup_location_object_map, key=lambda k: len(pickup_location_object_map[k]),
reverse=True)
# toDo loc up location with type end
best_pickup_loc = self.get_end_locarion()
items_on_plate = len(objects_on_plate)
if items_on_plate == 0 and len(pickup_location_object_map) > 0:
# location with most items to pic
best_pickup_loc['name'] = pickup_locs_sorted[0]
best_pickup_loc['type'] = 'Pickup'
publish_status("I am going to " + str(best_pickup_loc['name']) + ' to pickup objects.')
print "Pickup, since nothing on plate"
# place items
elif items_on_plate == 3 or (len(pickup_location_object_map) == 0 and items_on_plate > 0):
best_pickup_loc['name'] = self.get_location_with_most_obj_to_place(objects_on_plate)
best_pickup_loc['type'] = 'Place'
publish_status(
"My back plate is full, I am going to " + str(best_pickup_loc['name']) + ' to place objects.')
print "Place, since plate full or nothing left to pick"
#choose extra item to pick
else:
found = False
for loc_key in pickup_locs_sorted:
objects_to_pickup = pickup_location_object_map[loc_key]
if len(objects_to_pickup) <= 3 - items_on_plate:
found = True
for obj in objects_to_pickup:
req = self.can_place_object(obj['name'])
if not req.can_place:
found = False
break
if found:
best_pickup_loc['name'] = loc_key
best_pickup_loc['type'] = 'Pickup'
publish_status("I am going to " + str(best_pickup_loc['name']) + '<br> to pickup objects.')
print "Pickup, can kill other platform"
if not found and items_on_plate > 0:
best_pickup_loc['name'] = self.get_location_with_most_obj_to_place(objects_on_plate)
best_pickup_loc['type'] = 'Place'
publish_status("I am going to " + str(best_pickup_loc['name']) + '<br> to place objects.')
print "place since no other platform to kill"
if best_pickup_loc['type'] == 'End':
print 'Go To end'
userdata.pose_out = best_pickup_loc
if self.last_chosen_position is not None and self.last_chosen_position['name'] == \
best_pickup_loc['name']:
self.counter += 1
if self.counter > 2:
# delete location from param server
locations_param = rospy.get_param('locations')
for loc in locations_param:
if loc['name'] == self.last_chosen_position['name']:
locations_param.remove(loc)
break
rospy.set_param('locations', locations_param)
self.counter = 0
else:
self.counter = 0
self.last_chosen_position = best_pickup_loc
return 'done'
class GetNextObjectLocationState(smach.State):
def __init__(self, holes=False):
smach.State.__init__(self,
outcomes=[
'success', 'nothing_found', 'nothing_left',
'backplate_full'
],
input_keys=['pose_in'],
output_keys=['point_out'])
init_helpers()
self.holes = holes
self.srv_string = ''
if not holes:
self.srv_string = "/vision/object_manager/get_best"
else:
self.srv_string = "/vision/hole_manager/get_best"
self.get_best_at_location = rospy.ServiceProxy(self.srv_string,
GetObjectAtLocation)
def execute(self, userdata):
pose = userdata.pose_in
side = get_pickup_side(pose['name'])
if not self.holes:
objects_on_plate = get_objects_on_plate()
if len(objects_on_plate) == 3:
return 'backplate_full'
param_pose = rospy.get_param(pose['name'])
param_objects = param_pose['objects']
objects = [x['name'] for x in param_objects]
print "OBJECTS AT LOCATION :", objects
else:
objects_on_plate = get_objects_on_plate()
objects = []
for obj in objects_on_plate:
if obj.destination_loc == pose['name']:
param_obj = rospy.get_param(obj.label)
hole = param_obj['hole']
objects.append(hole)
else:
rospy.logerr('object not for this platform' +
str(obj.label))
# objects = [x['name'] for x in param_objects]
print "HOLES TO FIND AT LOCATION :", objects
if len(objects) == 0:
return 'nothing_left'
best = None
req = GetObjectAtLocationRequest()
req.loc = pose['name']
req.side = side
req.objects_to_pick = objects
try:
rospy.wait_for_service(self.srv_string)
best = self.get_best_at_location(req)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
return 'nothing_found'
if best.object.label == 'None':
print 'best.object.label == None'
return 'nothing_found'
userdata.point_out = best.object.pose.pose
if self.holes:
publish_status('I am driving to the <br> next hole (' +
str(best.object.label) + ').')
else:
publish_status('I am driving to the <br> next object (' +
str(best.object.label) + ').')
return 'success'