def __classify(self, taskdata): rospy.loginfo('Executing state ClassifyObject') taskdata.sec_try = False rospy.loginfo('Using simple perception.') get_gripper = perception.get_gripper_perception() if get_gripper is None: rospy.logwarn("get_gripper is None!!!?!?") get_gripper = [] perceived_objects = get_valid_objects(get_gripper) rospy.logdebug('Found ' + str(len(get_gripper)) + ' objects, ' + str(len(perceived_objects)) + ' are valid.') if not perceived_objects: if not taskdata.sec_try_done: taskdata.sec_try = True return 'noObject' collision_objects = [] matched_objects = [] found_object_names = [] for obj in taskdata.objects_found: found_object_names.append(obj.mpe_object.id) for obj in perceived_objects: # classify object matched_obj = classify_object(obj) rospy.logdebug('Matched: \n' + str(obj) + '\n----------------------------------\nwith: ' + str(matched_obj)) if matched_obj.c_type == EurocObject.OBSTACLE: if matched_obj.c_cuboid_success: collision_objects.append(matched_obj.object) continue if matched_obj.c_type == EurocObject.UNKNOWN or matched_obj.c_type == EurocObject.TABLE: if matched_obj.c_cuboid_success: collision_objects.append(matched_obj.object) # TODO something better than ignoring the problem rospy.logwarn('Unknown object perceived or table. Ignoring it for now.') continue if matched_obj.c_type == EurocObject.OBJECT: # check if the object was already placed rospy.loginfo('Matched object ' + str(matched_obj.object.id) + '.') if matched_obj.object.id in found_object_names: rospy.loginfo('Object %s was already seen.' % matched_obj.object.id) else: euroc_object_to_odom_combined(matched_obj) matched_objects.append(matched_obj) # publish_collision_objects(collision_objects) taskdata.classified_objects = matched_objects # check if it was an object if matched_objects: return 'objectsClassified' else: if not taskdata.sec_try_done: taskdata.sec_try = True return 'noObject'
def _handle_euroc_to_combined(self, req): print(req) print(req.toConvert) resp = EurocObjectToOdomCombinedResponse() utils.euroc_object_to_odom_combined(req.toConvert) resp.converted = req.toConvert return resp
def _handle_pose_estimate_object(self, req): rospy.loginfo('Executing state PoseEstimateObject') taskdata = req.taskdata taskdata.focused_object = taskdata.object_to_focus taskdata.sec_try = False taskdata.fitted_object = None # Get the ID of the classified object from the YAML file ids = utils.get_yaml_objects_nrs(taskdata.yaml, taskdata.focused_object.object.id) # pose_estimated = perception.get_gripper_perception(pose_estimation=True, object_ids=ids)[0] # OLD pose_estimated_objects = perception.get_gripper_perception(pose_estimation=True, cuboid=False, object_ids=ids) rospy.logdebug("Returned pose_estimated_objects:") rospy.logdebug(str(pose_estimated_objects)) if pose_estimated_objects is None: rospy.logwarn('Couldn\'t get gripper perception.') if not taskdata.sec_try_done: taskdata.sec_try = True return TaskDataServiceResponse(taskdata = taskdata, result = "fail") if not pose_estimated_objects: rospy.logdebug('No object found for pose estimation.') if not taskdata.sec_try_done: taskdata.sec_try = True return TaskDataServiceResponse(taskdata = taskdata, result = "fail") # Convert the poses to odom_combined map(lambda obj: utils.euroc_object_to_odom_combined(obj), pose_estimated_objects) # TODO Investigate the result for the object that's closest to the original object we were interested in # TODO find proper threshold corresponding_object_idx = utils.get_nearest_object_idx(taskdata.focused_object, pose_estimated_objects, 0.1) # # TODO if more than one object were recognized, classify again and take the closest # if len(pose_estimated_objects) == 1: # corresponding_object_idx = 0 # else: # corresponding_object_idx = None # for idx in range(0, len(pose_estimated_objects)): # if pose_estimated_objects[idx].mpe_success: # corresponding_object_idx = idx # TODO Call the perception again if corresponding_object_idx is None: rospy.logdebug('Couldn\'t find the desired object on the next lookup again') if not taskdata.sec_try_done: taskdata.sec_try = True return TaskDataServiceResponse(taskdata = taskdata, result = "fail") pose_estimated = pose_estimated_objects[corresponding_object_idx] rospy.logdebug('Use Result from Object :%s' % str(pose_estimated)) if pose_estimated is None or not pose_estimated.mpe_success: rospy.logwarn('Poseestimation failed.') if not taskdata.sec_try_done: taskdata.sec_try = True return TaskDataServiceResponse(taskdata = taskdata, result = "fail") taskdata.fitted_object = pose_estimated rospy.loginfo('Publishing objects into planning scene.') utils.publish_collision_objects([pose_estimated.mpe_object]) return TaskDataServiceResponse(taskdata = taskdata, result = "success")
def _handle_pose_estimate_object(self, req): rospy.loginfo('Executing state PoseEstimateObject') taskdata = req.taskdata taskdata.focused_object = taskdata.object_to_focus taskdata.sec_try = False taskdata.fitted_object = None # Get the ID of the classified object from the YAML file ids = utils.get_yaml_objects_nrs(taskdata.yaml, taskdata.focused_object.object.id) # pose_estimated = perception.get_gripper_perception(pose_estimation=True, object_ids=ids)[0] # OLD pose_estimated_objects = perception.get_gripper_perception( pose_estimation=True, cuboid=False, object_ids=ids) rospy.logdebug("Returned pose_estimated_objects:") rospy.logdebug(str(pose_estimated_objects)) if pose_estimated_objects is None: rospy.logwarn('Couldn\'t get gripper perception.') if not taskdata.sec_try_done: taskdata.sec_try = True return TaskDataServiceResponse(taskdata=taskdata, result="fail") if not pose_estimated_objects: rospy.logdebug('No object found for pose estimation.') if not taskdata.sec_try_done: taskdata.sec_try = True return TaskDataServiceResponse(taskdata=taskdata, result="fail") # Convert the poses to odom_combined map(lambda obj: utils.euroc_object_to_odom_combined(obj), pose_estimated_objects) # TODO Investigate the result for the object that's closest to the original object we were interested in # TODO find proper threshold corresponding_object_idx = utils.get_nearest_object_idx( taskdata.focused_object, pose_estimated_objects, 0.1) # # TODO if more than one object were recognized, classify again and take the closest # if len(pose_estimated_objects) == 1: # corresponding_object_idx = 0 # else: # corresponding_object_idx = None # for idx in range(0, len(pose_estimated_objects)): # if pose_estimated_objects[idx].mpe_success: # corresponding_object_idx = idx # TODO Call the perception again if corresponding_object_idx is None: rospy.logdebug( 'Couldn\'t find the desired object on the next lookup again') if not taskdata.sec_try_done: taskdata.sec_try = True return TaskDataServiceResponse(taskdata=taskdata, result="fail") pose_estimated = pose_estimated_objects[corresponding_object_idx] rospy.logdebug('Use Result from Object :%s' % str(pose_estimated)) if pose_estimated is None or not pose_estimated.mpe_success: rospy.logwarn('Poseestimation failed.') if not taskdata.sec_try_done: taskdata.sec_try = True return TaskDataServiceResponse(taskdata=taskdata, result="fail") taskdata.fitted_object = pose_estimated rospy.loginfo('Publishing objects into planning scene.') utils.publish_collision_objects([pose_estimated.mpe_object]) return TaskDataServiceResponse(taskdata=taskdata, result="success")