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 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 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): 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 # 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): 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
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"