def grasp_target_action_result_cb(userdata, status, result):
# here parse the result and compose a ObjectPoseList of what we found and return it in the output key object_found
obj = ObjectPoseList()
obj.header = result.detectionResult.table.pose.header # just copying some header
num_object = 0
i = 0
try:
for i in range(0, 10): # there wont be more objects, come on!
if result.detectionResult.models[i].model_list[0].model_id == userdata.object_id_from_name or userdata.object_id_from_name <= 0: # I think we only get one returned anyways
op = ObjectPose()
obj.object_list.append(op)
if userdata.object_id_from_name > 0:
obj.object_list[num_object].name = userdata.object_to_search_for
obj.object_list[num_object].pose = result.detectionResult.models[i].model_list[0].pose.pose
num_object += 1
break
else:
rospy.loginfo("Going to translate ID: " + str(result.detectionResult.models[i].model_list[0].model_id))
rospy.wait_for_service('object_translator_dataBase')
obj_trans_db = rospy.ServiceProxy('object_translator_dataBase', ObjectTranslatorDataBase)
obj_info = obj_trans_db(result.detectionResult.models[i].model_list[0].model_id)
#rospy.loginfo("Got info from db: " + str(obj_info))
if obj_info.objname == '':
obj.object_list[num_object].name = 'unknown'
else:
obj.object_list[num_object].name = obj_info.objname
obj.object_list[num_object].pose = result.detectionResult.models[i].model_list[0].pose.pose
num_object += 1
except Exception as e:
rospy.loginfo("Exception when looping over result.detectionResult.models[" + str(i) + "]")
rospy.loginfo("result.detectionResult.models[i] looks like:\n" + str(result.detectionResult.models[i]))
if str(e) != "list index out of range":
rospy.loginfo("Got exception: " + str(e))
pass # this exception should be that we are looping too far...
userdata.object_found = obj
rospy.loginfo("userdata.object_found generated as: " + str(obj))
if status == GoalStatus.SUCCEEDED and num_object > 0:
return succeeded
else:
global OUTPUT
OUTPUT = GRASP_TARGET_ACTION_FAILED
return aborted
def grasp_target_result_cb(userdata, status, result):
#rospy.loginfo("Result of the cb: result: " + str(result) )
# result is:
# int32 status
# copy of the tabletop detection result
# tabletop_object_detector/TabletopDetectionResult detectionResult
# int32 NO_CLOUD_RECEIVED=1
# int32 NO_TABLE=2
# int32 OTHER_ERROR=3
# int32 SUCCESS=4
# tabletop_object_detector/Table table
# geometry_msgs/PoseStamped pose
# std_msgs/Header header
# uint32 seq
# time stamp
# string frame_id
# geometry_msgs/Pose pose
# geometry_msgs/Point position
# float64 x
# float64 y
# float64 z
# geometry_msgs/Quaternion orientation
# float64 x
# float64 y
# float64 z
# float64 w
# float32 x_min
# float32 x_max
# float32 y_min
# float32 y_max
# arm_navigation_msgs/Shape convex_hull
# byte SPHERE=0
# byte BOX=1
# byte CYLINDER=2
# byte MESH=3
# byte type
# float64[] dimensions
# int32[] triangles
# geometry_msgs/Point[] vertices
# float64 x
# float64 y
# float64 z
# sensor_msgs/PointCloud[] clusters
# std_msgs/Header header
# uint32 seq
# time stamp
# string frame_id
# geometry_msgs/Point32[] points
# float32 x
# float32 y
# float32 z
# sensor_msgs/ChannelFloat32[] channels
# string name
# float32[] values
# household_objects_database_msgs/DatabaseModelPoseList[] models
# household_objects_database_msgs/DatabaseModelPose[] model_list
# int32 model_id
# geometry_msgs/PoseStamped pose
# std_msgs/Header header
# uint32 seq
# time stamp
# string frame_id
# geometry_msgs/Pose pose
# geometry_msgs/Point position
# float64 x
# float64 y
# float64 z
# geometry_msgs/Quaternion orientation
# float64 x
# float64 y
# float64 z
# float64 w
# float32 confidence
# string detector_name
# int32[] cluster_model_indices
# int32 result
# here parse the result and compose a ObjectPoseList of what we found and return it in the output key object_found
obj = ObjectPoseList()
# Header header
# ObjectPose[] object_list
# time originalTimeStamp
# time requestTimeStamp
obj.header = result.detectionResult.table.pose.header # just copying some header
####obj.object_list = [None] * 10 # ugly way to allocate space in python
# obj.object_list[0] = ObjectPose() # only putting the "interesting result" for now
# string name
# geometry_msgs/Pose pose
# geometry_msgs/Point32 pose2D
# int16[] convex_hull_x
# int16[] convex_hull_y
# float32 mean_quality
# int16 used_points
# NameTypeValue[] properties
# geometry_msgs/Pose[] pose_uncertainty_list
#obj.object_list[0].name = userdata.object_to_search_for
#rospy.loginfo("result.detectionResult.models:\n" + str(result.detectionResult.models))
num_object = 0
i=0
try:
for i in range(0, 10): # there wont be more objects, come on!
if result.detectionResult.models[i].model_list[0].model_id == userdata.object_id_from_name or userdata.object_id_from_name <= 0: # I think we only get one returned anyways
op = ObjectPose()
obj.object_list.append(op)
if userdata.object_id_from_name > 0:
obj.object_list[num_object].name = userdata.object_to_search_for
obj.object_list[num_object].pose = result.detectionResult.models[i].model_list[0].pose.pose
num_object += 1
break
else:
rospy.loginfo("Going to translate ID: " + str(result.detectionResult.models[i].model_list[0].model_id))
rospy.wait_for_service('object_translator_dataBase')
obj_trans_db = rospy.ServiceProxy('object_translator_dataBase', ObjectTranslatorDataBase)
obj_info = obj_trans_db(result.detectionResult.models[i].model_list[0].model_id)
#rospy.loginfo("Got info from db: " + str(obj_info))
if obj_info.objname == '':
obj.object_list[num_object].name = 'unknown'
else:
obj.object_list[num_object].name = obj_info.objname
obj.object_list[num_object].pose = result.detectionResult.models[i].model_list[0].pose.pose
num_object += 1
except Exception as e:
# rospy.loginfo("Exception when looping over result.detectionResult.models[" + str(i) + "]")
# rospy.loginfo("result.detectionResult.models[i] looks like:\n" + str(result.detectionResult.models[i]))
if str(e) != "list index out of range":
rospy.loginfo("Got exception: " + str(e))
pass # this exception should be that we are looping too far...
# for item, value in result.detectionResult.models.items(): # AttributeError: 'list' object has no attribute 'items' ITS A F*****G LIST??? WTF
# rospy.loginfo("item: " + item + "\n value: " + value)
# for i in range(len(result.detectionResult.models)): # TypeError: object of type 'DatabaseModelPoseList' has no len()
# for j in range(len(result.detectionResult.models[i])):
# if result.detectionResult.models[i].model_list[j].model_id == userdata.object_id_from_name:
# obj.object_list[0].pose = result.detectionResult.models[i].model_list[j].pose.pose
# CANT DO THIS BECAUSE PYTHON KEEPS SAYING THIS IS NOT ITERABLE /CRY
# for model_list in result.detectionResult.models: # TypeError: 'DatabaseModelPoseList' object is not iterable # maybe this is a UserDict?
# for model in model_list:
# if model.model_id == userdata.object_id_from_name:
# obj.object_list[0].pose = model.pose.pose
userdata.object_found = obj
rospy.loginfo("userdata.object_found generated as: " + str(obj) )
if status == GoalStatus.SUCCEEDED and num_object > 0:
return succeeded
else:
err = "Grasp target action failed. "
err += ("Status: %s" % str(status)) if status != GoalStatus.SUCCEEDED else ("num_object: %s" % str(num_object) )
set_grasp_error(err)
return aborted
