def _handle_focus(self, req): taskdata = req.taskdata rospy.loginfo('Executing state FocusObject') rospy.loginfo('Trying to focus %s' % taskdata.object_to_focus.object.id) taskdata.focused_object = None centroid = taskdata.object_to_focus.c_centroid centroid.z -= 0.03 poses = calc_grasp_position.make_scan_pose(centroid, *utils.focus_poses[self._ctr]) utils.manipulation.set_planning_time_arm(2) move_method = utils.manipulation.move_to transition = "" for pose in poses: if move_method(pose): taskdata.focused_object = taskdata.object_to_focus rospy.logdebug('Wait for clock') time.sleep(1) rospy.logdebug('Wait for tf again.') rospy.sleep(4) self._ctr = 0 transition = self.RETURN_VAL_SUCSS if transition == "": utils.manipulation.set_planning_time_arm(5) if self._ctr < 6: self._ctr += 1 transition = self.RETURN_VAL_RETRY else: self._ctr = 0 transition = self.RETURN_VAL_FAIL return TaskDataServiceResponse(taskdata = taskdata, result = transition)
def _handle_focus(self, req): taskdata = req.taskdata rospy.loginfo('Executing state FocusObject') rospy.loginfo('Trying to focus %s' % taskdata.object_to_focus.object.id) taskdata.focused_object = None centroid = taskdata.object_to_focus.c_centroid centroid.z -= 0.03 poses = calc_grasp_position.make_scan_pose( centroid, *utils.focus_poses[self._ctr]) utils.manipulation.set_planning_time_arm(2) move_method = utils.manipulation.move_to transition = "" for pose in poses: if move_method(pose): taskdata.focused_object = taskdata.object_to_focus rospy.logdebug('Wait for clock') time.sleep(1) rospy.logdebug('Wait for tf again.') rospy.sleep(4) self._ctr = 0 transition = self.RETURN_VAL_SUCSS if transition == "": utils.manipulation.set_planning_time_arm(5) if self._ctr < 6: self._ctr += 1 transition = self.RETURN_VAL_RETRY else: self._ctr = 0 transition = self.RETURN_VAL_FAIL return TaskDataServiceResponse(taskdata=taskdata, result=transition)
def test11_1(self): m = self.make_free_map() m.get_cell_by_index(9,9).set_unknown() m.get_cell_by_index(9,10).set_unknown() m.get_cell_by_index(9,11).set_unknown() m.get_cell_by_index(10,9).set_unknown() m.get_cell_by_index(10,10).set_unknown() m.get_cell_by_index(10,11).set_unknown() m.get_cell_by_index(11,9).set_unknown() m.get_cell_by_index(11,10).set_unknown() m.get_cell_by_index(11,11).set_unknown() (x, y) = m.index_to_coordinates(10, 10) poses = make_scan_pose(Point(x,y,0), 0.05, 0, n=16) poses = m.filter_invalid_scan_poses(x, y, poses) # print len(poses) self.assertTrue(len(poses) == 0, poses)
def test11_1(self): m = self.make_free_map() m.get_cell_by_index(9, 9).set_unknown() m.get_cell_by_index(9, 10).set_unknown() m.get_cell_by_index(9, 11).set_unknown() m.get_cell_by_index(10, 9).set_unknown() m.get_cell_by_index(10, 10).set_unknown() m.get_cell_by_index(10, 11).set_unknown() m.get_cell_by_index(11, 9).set_unknown() m.get_cell_by_index(11, 10).set_unknown() m.get_cell_by_index(11, 11).set_unknown() (x, y) = m.index_to_coordinates(10, 10) poses = make_scan_pose(Point(x, y, 0), 0.05, 0, n=16) poses = m.filter_invalid_scan_poses(x, y, poses) # print len(poses) self.assertTrue(len(poses) == 0, poses)
def test12_1(self): m = self.make_free_map() # m.get_cell_by_index(9,9).set_unknown() # m.get_cell_by_index(9,10).set_unknown() # m.get_cell_by_index(9,11).set_unknown() m.get_cell_by_index(10,9).set_obstacle() m.get_cell_by_index(10,9).highest_z = 1 m.get_cell_by_index(10,10).set_unknown() m.get_cell_by_index(10,11).set_obstacle() m.get_cell_by_index(10,11).highest_z = 1 # m.get_cell_by_index(11,9).set_unknown() # m.get_cell_by_index(11,10).set_unknown() # m.get_cell_by_index(11,11).set_unknown() (x, y) = m.index_to_coordinates(10, 10) poses = make_scan_pose(Point(x,y,0), 0.15, 0, n=16) # print "" # print len(poses) poses = m.filter_invalid_scan_poses2(x, y, poses) # print len(poses) self.assertTrue(len(poses) == 10, len(poses))
def test12_1(self): m = self.make_free_map() # m.get_cell_by_index(9,9).set_unknown() # m.get_cell_by_index(9,10).set_unknown() # m.get_cell_by_index(9,11).set_unknown() m.get_cell_by_index(10, 9).set_obstacle() m.get_cell_by_index(10, 9).highest_z = 1 m.get_cell_by_index(10, 10).set_unknown() m.get_cell_by_index(10, 11).set_obstacle() m.get_cell_by_index(10, 11).highest_z = 1 # m.get_cell_by_index(11,9).set_unknown() # m.get_cell_by_index(11,10).set_unknown() # m.get_cell_by_index(11,11).set_unknown() (x, y) = m.index_to_coordinates(10, 10) poses = make_scan_pose(Point(x, y, 0), 0.15, 0, n=16) # print "" # print len(poses) poses = m.filter_invalid_scan_poses2(x, y, poses) # print len(poses) self.assertTrue(len(poses) == 10, len(poses))
def execute(self, userdata): rospy.loginfo('Executing state FocusObject') rospy.loginfo('Trying to focus %s' % userdata.object_to_focus.object.id) userdata.focused_object = None centroid = userdata.object_to_focus.c_centroid centroid.z -= 0.03 poses = calc_grasp_position.make_scan_pose(centroid, *utils.focus_poses[self._ctr]) if userdata.enable_movement: move_method = utils.manipulation.move_arm_and_base_to else: utils.manipulation.set_planning_time_arm(2) move_method = utils.manipulation.move_to for pose in poses: if move_method(pose): userdata.focused_object = userdata.object_to_focus rospy.logdebug('Wait for clock') time.sleep(1) rospy.logdebug('Wait for tf again.') rospy.sleep(4) self._ctr = 0 return 'success' utils.manipulation.set_planning_time_arm(5) if self._ctr < 6: self._ctr += 1 return 'retry' else: self._ctr = 0 return 'fail'
def execute(self, userdata): rospy.loginfo('Executing state ScanObstacles') userdata.sec_try_done = False if userdata.sec_try: current_region = self.classified_regions[self.next_cluster - 1][0] userdata.sec_try_done = True else: #get regions if len(self.classified_regions) == 0: obstacle_cluster = utils.map.get_obstacle_regions() rospy.logdebug( str(len(self.classified_regions)) + " regions found.") print '#####################################' self.classified_regions = utils.map.undercover_classifier( obstacle_cluster, userdata.yaml.objects) print self.classified_regions print '#####################################' base = utils.manipulation.get_base_origin().point self.classified_regions.sort(key=lambda x: euclidean_distance( Point(*(utils.map.index_to_coordinates(*x[0].get_avg())) + (0.0, )), base)) # x[0].get_number_of_cells()) if self.next_cluster >= len(self.classified_regions): rospy.loginfo("searched all cluster") return 'noRegionLeft' current_region = self.classified_regions[self.next_cluster][0] rospy.logdebug("current region: " + str(self.next_cluster) + "\n" + str(current_region) + "\nclassified as " + str(self.classified_regions[self.next_cluster][1])) self.next_cluster += 1 region_centroid = Point( *(utils.map.index_to_coordinates(*current_region.get_avg())) + (-0.065, )) dist_to_region = mathemagie.euclidean_distance(Point(0, 0, 0), region_centroid) #if userdata.yaml.task_type == Task.TASK_5: # fixture_position = mathemagie.add_point(userdata.yaml.puzzle_fixture.position, Point(0.115, 0.165, 0)) # dist_to_fixture = mathemagie.euclidean_distance(fixture_position, region_centroid) # if dist_to_fixture < 0.35: # rospy.logdebug("Region classified as puzzle fixture, skipping") # return 'mapScanned' # If the arm cannot move ignore distant regions # TODO find the best max distance if not userdata.enable_movement: rospy.logdebug('Distance of the current region to the arm: %s' % str(dist_to_region)) if dist_to_region > 1.1: rospy.logwarn('Current region is out of reach. Ignoring it.') return 'mapScanned' angle = 1.2 distance = 0.6 + current_region.get_number_of_cells() * 0.008 poses = make_scan_pose(region_centroid, distance, angle, n=16) if not userdata.enable_movement: poses = utils.manipulation.filter_close_poses(poses) poses = utils.map.filter_invalid_scan_poses2(region_centroid.x, region_centroid.y, poses) if userdata.sec_try: current_pose = utils.manipulation.get_eef_position().pose.position current_pose.z = 0 region_to_eef = subtract_point(region_centroid, current_pose) poses.sort(key=lambda pose: abs( get_angle( region_to_eef, subtract_point( Point(pose.pose.position.x, pose.pose.position.y, 0), region_centroid)) - pi / 2)) visualize_poses(poses) if userdata.enable_movement: # move = utils.manipulation.move_arm_and_base_to plan = utils.manipulation.plan_arm_and_base_to else: # move = utils.manipulation.move_to plan = utils.manipulation.plan_arm_to utils.manipulation.blow_up_objects(do_not_blow_up_list=("map")) for pose in poses: # utils.manipulation.set_planning_time_arm(2) if utils.manipulation.move_with_plan_to(plan(pose)): # utils.manipulation.set_planning_time_arm(5) userdata.focused_point = region_centroid rospy.logdebug('Wait for clock') time.sleep(0.5) rospy.sleep(2.5) return 'newImage' utils.manipulation.blow_down_objects() return 'mapScanned'
def execute(self, userdata): rospy.loginfo('Executing state ScanMapArmCam') if self.finished: return 'mapScanned' regions = utils.map.get_unknown_regions() regions.sort(key=lambda r: -r.get_number_of_cells()) for r in regions: r.set_boarder_cells(sorted(utils.map.get_boarder_cell_points(r), cmp=lambda p1, p2: utils.map.is_more_edge(p1.x, p1.y, p2.x, p2.y))) i = 0 r_id = 0 utils.manipulation.set_planning_time_arm_base(5) while len(regions) > 0 and utils.map.get_percent_cleared() < 0.985: if len(regions[r_id].get_boarder_cells()) < self.min_num_of_cells: regions.remove(regions[r_id]) if len(regions) == 0: break r_id = r_id % len(regions) i = 0 continue if i >= 2: r_id += 1 r_id = r_id % len(regions) i = 0 continue next_point = regions[r_id].get_boarder_cells()[-1] regions[r_id].get_boarder_cells().remove(next_point) cell_x = next_point.x cell_y = next_point.y utils.map.mark_cell(cell_x, cell_y, True) next_point.z = 0.2 poses = make_scan_pose(next_point, 0.7, 0.8, n=16) poses = utils.map.filter_invalid_scan_poses(cell_x, cell_y, poses) poses = utils.map.filter_invalid_scan_poses2(cell_x, cell_y, poses) poses.sort(key=lambda pose: -euclidean_distance( Point(*(utils.map.index_to_coordinates(*regions[r_id].get_avg()) + (0,))), Point(pose.pose.position.x, pose.pose.position.y, 0))) visualize_poses(poses) j = 0 move_successfull = False while j < len(poses) and not move_successfull: rospy.logdebug("try scan pose nr. " + str(j)) move_successfull = utils.manipulation.move_arm_and_base_to(poses[j], do_not_blow_up_list=("all")) j += 1 if move_successfull: rospy.sleep(utils.waiting_time_before_scan) arm_base = utils.manipulation.get_base_origin() map_updated = utils.map.add_point_cloud(arm_base.point, scene_cam=False) if not map_updated: rospy.logdebug("no map update") utils.map.mark_cell(next_point.x, next_point.y, False) i += 1 continue rospy.logdebug("published") utils.map.mark_cell(next_point.x, next_point.y, False) co = utils.map.to_collision_object() utils.manipulation.get_planning_scene().add_object(co) utils.manipulation.set_planning_time_arm_base(10) return 'newImage' utils.map.mark_cell(next_point.x, next_point.y, False) i += 1 rospy.loginfo("can't update map any further") utils.map.all_unknowns_to_obstacle() co = utils.map.to_collision_object() utils.manipulation.get_planning_scene().add_object(co) self.finished = True utils.manipulation.set_planning_time_arm_base(10) return 'mapScanned'
def execute(self, userdata): rospy.loginfo('Executing state ScanObstacles') userdata.sec_try_done = False if userdata.sec_try: current_region = self.classified_regions[self.next_cluster-1][0] userdata.sec_try_done = True else: #get regions if len(self.classified_regions) == 0: obstacle_cluster = utils.map.get_obstacle_regions() rospy.logdebug(str(len(self.classified_regions)) + " regions found.") print '#####################################' self.classified_regions = utils.map.undercover_classifier(obstacle_cluster, userdata.yaml.objects) print self.classified_regions print '#####################################' base = utils.manipulation.get_base_origin().point self.classified_regions.sort(key=lambda x: euclidean_distance(Point(*(utils.map.index_to_coordinates(*x[0].get_avg()))+(0.0,)), base)) # x[0].get_number_of_cells()) if self.next_cluster >= len(self.classified_regions): rospy.loginfo("searched all cluster") return 'noRegionLeft' current_region = self.classified_regions[self.next_cluster][0] rospy.logdebug("current region: " + str(self.next_cluster) + "\n" + str(current_region) + "\nclassified as " + str(self.classified_regions[self.next_cluster][1])) self.next_cluster += 1 region_centroid = Point(*(utils.map.index_to_coordinates(*current_region.get_avg()))+(-0.065,)) dist_to_region = mathemagie.euclidean_distance(Point(0, 0, 0), region_centroid) #if userdata.yaml.task_type == Task.TASK_5: # fixture_position = mathemagie.add_point(userdata.yaml.puzzle_fixture.position, Point(0.115, 0.165, 0)) # dist_to_fixture = mathemagie.euclidean_distance(fixture_position, region_centroid) # if dist_to_fixture < 0.35: # rospy.logdebug("Region classified as puzzle fixture, skipping") # return 'mapScanned' # If the arm cannot move ignore distant regions # TODO find the best max distance if not userdata.enable_movement: rospy.logdebug('Distance of the current region to the arm: %s' % str(dist_to_region)) if dist_to_region > 1.1: rospy.logwarn('Current region is out of reach. Ignoring it.') return 'mapScanned' angle = 1.2 distance = 0.6 + current_region.get_number_of_cells()*0.008 poses = make_scan_pose(region_centroid, distance, angle, n=16) if not userdata.enable_movement: poses = utils.manipulation.filter_close_poses(poses) poses = utils.map.filter_invalid_scan_poses2(region_centroid.x, region_centroid.y, poses) if userdata.sec_try: current_pose = utils.manipulation.get_eef_position().pose.position current_pose.z = 0 region_to_eef = subtract_point(region_centroid, current_pose) poses.sort(key=lambda pose: abs(get_angle(region_to_eef, subtract_point(Point(pose.pose.position.x, pose.pose.position.y, 0), region_centroid)) - pi/2)) visualize_poses(poses) if userdata.enable_movement: # move = utils.manipulation.move_arm_and_base_to plan = utils.manipulation.plan_arm_and_base_to else: # move = utils.manipulation.move_to plan = utils.manipulation.plan_arm_to utils.manipulation.blow_up_objects(do_not_blow_up_list=("map")) for pose in poses: # utils.manipulation.set_planning_time_arm(2) if utils.manipulation.move_with_plan_to(plan(pose)): # utils.manipulation.set_planning_time_arm(5) userdata.focused_point = region_centroid rospy.logdebug('Wait for clock') time.sleep(0.5) rospy.sleep(2.5) return 'newImage' utils.manipulation.blow_down_objects() return 'mapScanned'