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_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")
