def in_target_zone(euroc_object, yaml): if (yaml.task_type == Task.TASK_5): for puzzle_part in yaml.relative_puzzle_part_target_poses: centroid = deepcopy(euroc_object.c_centroid) centroid.z = 0 target_position = mathemagie.add_point( yaml.puzzle_fixture.position, puzzle_part.pose.position) fake_target_zone = TargetZone fake_target_zone.name = puzzle_part.name + "_target" fake_target_zone.expected_object = puzzle_part.name fake_target_zone.target_position = target_position fake_target_zone.max_distance = 0.05 # dafuq do i know? dist = mathemagie.euclidean_distance(centroid, target_position) if dist < fake_target_zone.max_distance: return fake_target_zone else: for target_zone in yaml.target_zones: centroid = deepcopy(euroc_object.c_centroid) centroid.z = 0 dist = mathemagie.euclidean_distance(centroid, target_zone.target_position) if dist < target_zone.max_distance: return target_zone return None
def _handle_focus_object(self, req): # Read the objects_to_focus if this is the first iteration taskdata = req.taskdata taskdata.objects_to_focus = taskdata.classified_objects if self._objects_to_focus is None: self._objects_to_focus = taskdata.objects_to_focus print("Objects to focus1: ") print(self._objects_to_focus) if not taskdata.focused_point is None: object_to_focus = None min_dist = 100 for obj in self._objects_to_focus: dist = mathemagie.euclidean_distance( obj.c_centroid, taskdata.focused_point) if dist < min_dist: min_dist = dist object_to_focus = obj self._objects_to_focus = [object_to_focus] # Remember the fitted objects elif not taskdata.fitted_object is None: self._fitted_objects.append(taskdata.fitted_object) if not utils.map is None: position = taskdata.fitted_object.mpe_object.primitive_poses[ 0].position utils.map.mark_region_as_object_under_point( position.x, position.y) rospy.logdebug(str(utils.map)) co = utils.map.to_collision_object() utils.manipulation.get_planning_scene().add_object(co) # If there are no more objects to focus if not self._objects_to_focus: self._objects_to_focus = None print("Objects to focus is None 2") taskdata.fitted_objects = self._fitted_objects self._fitted_objects = [] return TaskDataServiceResponse(taskdata=taskdata, result="success") # Set the next object to focus next_object = self._objects_to_focus.pop() print("Objects to focus2") print(self._objects_to_focus) taskdata.object_to_focus = next_object print("nect_object:") print(next_object) if utils.is_handle(next_object.object.id, taskdata.yaml): return TaskDataServiceResponse(taskdata=taskdata, result="focusHandle") else: return TaskDataServiceResponse(taskdata=taskdata, result="focusObject")
def _handle_focus_object(self, req): # Read the objects_to_focus if this is the first iteration taskdata = req.taskdata taskdata.objects_to_focus = taskdata.classified_objects if self._objects_to_focus is None: self._objects_to_focus = taskdata.objects_to_focus print("Objects to focus1: ") print(self._objects_to_focus) if not taskdata.focused_point is None: object_to_focus = None min_dist = 100 for obj in self._objects_to_focus: dist = mathemagie.euclidean_distance(obj.c_centroid, taskdata.focused_point) if dist < min_dist: min_dist = dist object_to_focus = obj self._objects_to_focus = [object_to_focus] # Remember the fitted objects elif not taskdata.fitted_object is None: self._fitted_objects.append(taskdata.fitted_object) if not utils.map is None: position = taskdata.fitted_object.mpe_object.primitive_poses[0].position utils.map.mark_region_as_object_under_point(position.x, position.y) rospy.logdebug(str(utils.map)) co = utils.map.to_collision_object() utils.manipulation.get_planning_scene().add_object(co) # If there are no more objects to focus if not self._objects_to_focus: self._objects_to_focus = None print("Objects to focus is None 2") taskdata.fitted_objects = self._fitted_objects self._fitted_objects = [] return TaskDataServiceResponse(taskdata = taskdata, result = "success") # Set the next object to focus next_object = self._objects_to_focus.pop() print("Objects to focus2") print(self._objects_to_focus) taskdata.object_to_focus = next_object print("nect_object:") print(next_object) if utils.is_handle(next_object.object.id, taskdata.yaml): return TaskDataServiceResponse(taskdata = taskdata, result = "focusHandle") else: return TaskDataServiceResponse(taskdata = taskdata, result = "focusObject")
def in_target_zone(euroc_object, yaml): if (yaml.task_type == Task.TASK_5): for puzzle_part in yaml.relative_puzzle_part_target_poses: centroid = deepcopy(euroc_object.c_centroid) centroid.z = 0 target_position = mathemagie.add_point(yaml.puzzle_fixture.position, puzzle_part.pose.position) fake_target_zone = TargetZone fake_target_zone.name = puzzle_part.name + "_target" fake_target_zone.expected_object = puzzle_part.name fake_target_zone.target_position = target_position fake_target_zone.max_distance = 0.05 # dafuq do i know? dist = mathemagie.euclidean_distance(centroid, target_position) if dist < fake_target_zone.max_distance: return fake_target_zone else: for target_zone in yaml.target_zones: centroid = deepcopy(euroc_object.c_centroid) centroid.z = 0 dist = mathemagie.euclidean_distance(centroid, target_zone.target_position) if dist < target_zone.max_distance: return target_zone return None
def execute(self, userdata): # Read the objects_to_focus if this is the first iteration if self._objects_to_focus is None: self._objects_to_focus = userdata.objects_to_focus if not userdata.focused_point is None: object_to_focus = None min_dist = 100 for obj in self._objects_to_focus: dist = mathemagie.euclidean_distance(obj.c_centroid, userdata.focused_point) if dist < min_dist: min_dist = dist object_to_focus = obj self._objects_to_focus = [object_to_focus] # Remember the fitted objects elif not userdata.fitted_object is None: self._fitted_objects.append(userdata.fitted_object) if not utils.map is None: position = userdata.fitted_object.mpe_object.primitive_poses[0].position utils.map.mark_region_as_object_under_point(position.x, position.y) rospy.logdebug(str(utils.map)) co = utils.map.to_collision_object() utils.manipulation.get_planning_scene().add_object(co) # If there are no more objects to focus if not self._objects_to_focus: self._objects_to_focus = None userdata.fitted_objects = self._fitted_objects self._fitted_objects = [] return 'success' # Set the next object to focus next_object = self._objects_to_focus.pop() userdata.object_to_focus = next_object if utils.is_handle(next_object.object.id, userdata.yaml): return 'focusHandle' else: return 'focusObject'
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 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'