def execute(self, userdata):
place_label = userdata.pose_in['name']
self.last_location = place_label
self.init_driven_counter_and_placed_items(place_label)
res = self.get_objects_on_plate()
has_object_to_place = False
for obj in res.objects_on_plate:
if obj.destination_loc == place_label:
has_object_to_place = True
obj_dict = backplate_object_msg_to_dict(obj)
userdata.object_out = obj_dict
userdata.backplate_pose_out = obj_dict['backplate_location']
pose = Pose()
side = get_pickup_side(place_label)
if side == 'left':
pose.position.y = place_offsets_position[
self.visitedPlaces[place_label]['placedItems'] % 3]
else:
pose.position.y = place_offsets_position[
2 - self.visitedPlaces[place_label]['placedItems'] % 3]
pose.orientation.w = 1
userdata.point_out = pose
if not has_object_to_place:
self.visitedPlaces[place_label]['alreadyDriven'] = 0
# self.visitedPlaces[place_label]['placedItems'] += 1
return 'nothing_left'
self.driveToPos(place_label)
self.visitedPlaces[place_label]['placedItems'] += 1
return 'place'
def execute(self, userdata):
pose = userdata.pose_in
side = get_pickup_side(pose['name'])
rotate = rotate_only(side=side, turned=False)
go([rotate])
return 'done'
def execute(self, userdata):
pose = userdata.pose_in
side = get_pickup_side(pose['name'])
rotate = rotate_only(side=side, turned=False)
go([rotate])
return 'done'
def get_distance_to_platform(self, pose):
side = get_pickup_side(pose['name'])
cur_dist = None
try:
cur_dist = self.dist_platform('side')
# print poseLoc
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def get_distance_to_platform(self, pose):
side = get_pickup_side(pose['name'])
cur_dist = None
try:
cur_dist = self.dist_platform('side')
# print poseLoc
except rospy.ServiceException, e:
print "Service call failed: %s" % e
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
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
def execute(self, userdata):
pose = userdata.pose_in
offset = userdata.point_in
side = get_pickup_side(pose['name'])
cur_dist = None
try:
cur_dist = self.dist_platform('side')
# print poseLoc
except rospy.ServiceException, e:
print "Service call failed: %s" % e
return 'failed'
def execute(self, userdata):
# locations = rosparam.get('locations')
# pose = locations[0]
pose = userdata.pose_in
side = get_pickup_side(pose['name'])
rotate = rotate_only(side)
#pre_grip[0] = rotate[0]
#up = [x for x in cam_test]
#up[0] = configuration[0]
if go([rotate]):
return 'success'
else:
return 'failed'
def execute(self, userdata):
# locations = rosparam.get('locations')
# pose = locations[0]
pose = userdata.pose_in
side = get_pickup_side(pose['name'])
rotate = rotate_only(side)
#pre_grip[0] = rotate[0]
#up = [x for x in cam_test]
#up[0] = configuration[0]
if go([rotate]):
return 'success'
else:
return 'failed'
def execute(self, userdata):
pose = userdata.pose_in
# position = rospy.get_param(pose)
# print 'go', pose
side = get_pickup_side(pose['name'])
#print 'go'
rotate = rotate_only(side)
pre_grip[0] = rotate[0]
# gripperClose(False)
# rotate,
go([pre_grip], blocking=self.blocking)
return 'success'
def execute(self, userdata):
pose = userdata.pose_in
# position = rospy.get_param(pose)
# print 'go', pose
side = get_pickup_side(pose['name'])
#print 'go'
rotate = rotate_only(side)
pre_grip[0] = rotate[0]
# gripperClose(False)
# rotate,
go([pre_grip], blocking=self.blocking)
return 'success'
def execute(self, userdata):
pose = userdata.pose_in
side = get_pickup_side(pose['name'])
goal = OdomFineAdjustGoal()
if side == "left":
goal.target.x = -self.speed
else:
goal.target.y = -1.5 * self.speed
goal.max_dist = self.distance
goal.duration = 5
print goal
if NO_FINE_ADJUST:
rospy.sleep(2)
# return 'failed'
return 'done'
# self.odom_fine_client.send_goal_and_wait(goal)
self.odom_fine_client.send_goal(goal)
r = rospy.Rate(10)
traveled_dist = 0
first_pose = self.get_own_pose()
while not self.odom_fine_client.get_state() == GoalStatus.SUCCEEDED:
res = self.get_angle(side)
if abs(res.ang) > math.pi / 180. * 5:
self.odom_fine_client.cancel_all_goals()
# result = self.odom_fine_client.()
# print result
own_pose = self.get_own_pose()
dist = self.dist(first_pose.pose.position,
own_pose.pose.position)
traveled_dist = dist
goal_align = AlignLineDistanceGoal()
goal_align.side = side
goal_align.dist = DISTANCE_TO_PLATFORM
# goal.eps_lin = self.eps_lin
# goal.eps_theta = self.eps_theta
# goal.counts = self.counter
self.align_client.send_goal_and_wait(goal_align)
#set new goal distance
goal.max_dist = self.distance - traveled_dist
self.odom_fine_client.send_goal(goal)
r.sleep()
return 'done'
def execute(self, userdata):
pose = userdata.pose_in
side = get_pickup_side(pose['name'])
goal = AlignLineDistanceGoal()
goal.side = side
goal.dist = DISTANCE_TO_PLATFORM
print goal
if NO_FINE_ADJUST:
rospy.sleep(2)
# return 'failed'
return 'reached'
self.align_client.send_goal_and_wait(goal)
if self.align_client.get_state() == GoalStatus.SUCCEEDED:
return 'reached'
else:
return 'failed'
def driveToPos(self, place_label):
index_to_drive = min(
(int(self.visitedPlaces[place_label]['placedItems'] / 3)) + 1,
len(self.move_x))
already_driven = self.visitedPlaces[place_label]['alreadyDriven']
side = get_pickup_side(place_label)
goal_x = OdomFineAdjustGoal()
if side == "left":
goal_x.target.x = -self.speed
else:
goal_x.target.y = -self.speed
goal_x.max_dist = sum(self.move_x[already_driven:index_to_drive])
if goal_x.max_dist > 0:
self.visitedPlaces[place_label]['alreadyDriven'] = index_to_drive
self.odom_fine_client.send_goal(goal_x)
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'
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'
def execute(self, userdata):
pose = userdata.pose_in
# position = rospy.get_param(pose)
userdata.pose_out = pose
side = get_pickup_side(pose['name'])
rotate = rotate_only(side)
# pre_grip[0] = rotate[0]
gripper_close(False)
new_grip_point = [x for x in grip_point]
new_grip_point[0] += 0.05
new_grip_point[2] += 0.015
conf = np.array(call_ik_solver(new_grip_point, side=side))
conf[4] = get_angle(conf[4], math.pi / 2)
if go([rotate, conf]):
return 'success'
else:
return 'failed'
def execute(self, userdata):
pose = userdata.pose_in
# position = rospy.get_param(pose)
userdata.pose_out = pose
side = get_pickup_side(pose['name'])
rotate = rotate_only(side)
# pre_grip[0] = rotate[0]
gripper_close(False)
new_grip_point = [x for x in grip_point]
new_grip_point[0] += 0.05
new_grip_point[2] += 0.015
conf = np.array(call_ik_solver(new_grip_point, side=side))
conf[4] = get_angle(conf[4], math.pi / 2)
if go([rotate, conf]):
return 'success'
else:
return 'failed'
def execute(self, userdata):
pose = userdata.pose_in
side = get_pickup_side(pose['name'])
goal = OdomFineAdjustGoal()
if side == "left":
goal.target.y = self.speed
else:
goal.target.x = self.speed
goal.max_dist = self.distance
goal.duration = 5
print goal
if NO_FINE_ADJUST:
rospy.sleep(2)
# return 'failed'
return 'done'
self.odom_fine_client.send_goal_and_wait(goal)
return 'done'
def execute(self, userdata):
confs = []
if not self.no_rotate:
pose = userdata.pose_in
side = get_pickup_side(pose['name'])
# print joints
tucked = rotate_only(side='left')
#tucked[0] = configuration[0]
#tucked[1] = joints['arm_joint_2']['min']
#tucked[2] = joints['arm_joint_3']['max']
#tucked[3] = joints['arm_joint_4']['min']
#tucked[4] = configuration[4]
confs.append(tucked)
tucked_for_drive = rotate_only(side='left')
# tucked[0] = conf[0]
tucked_for_drive[1] = joints['arm_joint_2']['min']
tucked_for_drive[2] = joints['arm_joint_3']['max']
tucked_for_drive[3] = joints['arm_joint_4']['min']
tucked_for_drive[4] = joints['arm_joint_5']['straight']
confs.append(tucked_for_drive)
go(confs, blocking=self.blocking)
return 'done'
def execute(self, userdata):
confs = []
if not self.no_rotate:
pose = userdata.pose_in
side = get_pickup_side(pose['name'])
# print joints
tucked = rotate_only(side='left')
#tucked[0] = configuration[0]
#tucked[1] = joints['arm_joint_2']['min']
#tucked[2] = joints['arm_joint_3']['max']
#tucked[3] = joints['arm_joint_4']['min']
#tucked[4] = configuration[4]
confs.append(tucked)
tucked_for_drive = rotate_only(side='left')
# tucked[0] = conf[0]
tucked_for_drive[1] = joints['arm_joint_2']['min']
tucked_for_drive[2] = joints['arm_joint_3']['max']
tucked_for_drive[3] = joints['arm_joint_4']['min']
tucked_for_drive[4] = joints['arm_joint_5']['straight']
confs.append(tucked_for_drive)
go(confs, blocking=self.blocking)
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):
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):
pose = userdata.pose_in
userdata.pose_out = pose
obj = userdata.object_in
userdata.object_out = obj
param_obj = rospy.get_param(obj['name'])
hole = param_obj['hole']
# param_pose = rospy.get_param(pose)
side = get_pickup_side(pose['name'])
# param_pose['side']
# best = self.getBestObject(objects)
goal = OdomFineAdjustGoal()
if side == "left":
goal.target.x = -0.04
else:
goal.target.y = -0.05
# goal.threshold.x = 0.05
#goal.threshold.y = 0.05
#goal.threshold.theta = 1 # this doesn't matter
goal.max_dist = 0.6
goal.duration = 11
if not NO_FINE_ADJUST:
self.odom_fine_client.send_goal(goal)
if NO_VISION:
self.current_hole = PoseStampedLabeled()
self.current_hole.label = hole
print "NO VISION JUST ACCEPT HOLE"
rospy.sleep(1)
else:
self.is_active = True
r = rospy.Rate(20)
while not self.odom_fine_client.get_state(
) == GoalStatus.SUCCEEDED and not self.acceptHole(hole):
# move
r.sleep()
if self.odom_fine_client.get_state() == GoalStatus.SUCCEEDED:
print "MOVED"
self.is_active = False
#userdata.pose_out = "D0"
# TODO set to normal place after done
return "nothing_found"
# stop
for i in xrange(5):
self.odom_fine_client.cancel_all_goals()
# res = self.odom_fine_client.get_result()
rospy.sleep(1.0)
self.is_active = False
if self.current_hole.label in [
'M30'
] and self.current_object.label in ['M20']:
self.current_hole.label = 'M20'
if not self.current_hole.label == hole:
return "failed"
#userdata.dist_out = res.dist
userdata.point_out = self.current_hole.pose.pose
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'
rospy.wait_for_service("/vision/object_manager/get_best")
hole = self.verify_object(req).object
except rospy.ServiceException, e:
print "Service call failed: %s" % e
return 'failed'
if hole.label == 'None':
return 'failed'
object_name = holes_to_obj[hole.label]
print object_name
height = get_place_height_for_obj(object_name)
p = hole.pose.pose.position
test_point = [grip_point[0] + p.x, p.y, grip_point[2] + height]
side = get_pickup_side(pose['name'])
conf = call_ik_solver(test_point, side=side)
if len(conf) == 0:
return 'too_far'
object_out = None
for o in objects_on_plate:
if o.label == object_name:
object_out = o
break
print object_out
userdata.object_out = backplate_object_msg_to_dict(object_out)
userdata.backplate_pose_out = object_out.place_loc
userdata.point_out = hole.pose.pose
print object_out, hole.label
rospy.wait_for_service("/vision/object_manager/get_best")
hole = self.verify_object(req).object
except rospy.ServiceException, e:
print "Service call failed: %s" % e
return 'failed'
if hole.label == 'None':
return 'failed'
object_name = holes_to_obj[hole.label]
print object_name
height = get_place_height_for_obj(object_name)
p = hole.pose.pose.position
test_point = [grip_point[0] + p.x, p.y, grip_point[2] + height]
side = get_pickup_side(pose['name'])
conf = call_ik_solver(test_point, side=side)
if len(conf) == 0:
return 'too_far'
object_out = None
for o in objects_on_plate:
if o.label == object_name:
object_out = o
break
print object_out
userdata.object_out = backplate_object_msg_to_dict(object_out)
userdata.backplate_pose_out = object_out.place_loc
userdata.point_out = hole.pose.pose
print object_out, hole.label
def execute(self, userdata):
pose = userdata.pose_in
userdata.pose_out = pose
obj = userdata.object_in
userdata.object_out = obj
param_obj = rospy.get_param(obj['name'])
hole = param_obj['hole']
# param_pose = rospy.get_param(pose)
side = get_pickup_side(pose['name'])
# param_pose['side']
# best = self.getBestObject(objects)
goal = OdomFineAdjustGoal()
if side == "left":
goal.target.x = -0.04
else:
goal.target.y = -0.05
# goal.threshold.x = 0.05
#goal.threshold.y = 0.05
#goal.threshold.theta = 1 # this doesn't matter
goal.max_dist = 0.6
goal.duration = 11
if not NO_FINE_ADJUST:
self.odom_fine_client.send_goal(goal)
if NO_VISION:
self.current_hole = PoseStampedLabeled()
self.current_hole.label = hole
print "NO VISION JUST ACCEPT HOLE"
rospy.sleep(1)
else:
self.is_active = True
r = rospy.Rate(20)
while not self.odom_fine_client.get_state() == GoalStatus.SUCCEEDED and not self.acceptHole(hole):
# move
r.sleep()
if self.odom_fine_client.get_state() == GoalStatus.SUCCEEDED:
print "MOVED"
self.is_active = False
#userdata.pose_out = "D0"
# TODO set to normal place after done
return "nothing_found"
# stop
for i in xrange(5):
self.odom_fine_client.cancel_all_goals()
# res = self.odom_fine_client.get_result()
rospy.sleep(1.0)
self.is_active = False
if self.current_hole.label in ['M30'] and self.current_object.label in ['M20']:
self.current_hole.label = 'M20'
if not self.current_hole.label == hole:
return "failed"
#userdata.dist_out = res.dist
userdata.point_out = self.current_hole.pose.pose
return "success"