def execute(self, userdata):
b_type = "ObjectPoseList" in str(type(userdata.pose_object))
obj = userdata.pose_object.object_list[0] if b_type else userdata.pose_object
if obj.name != None:
userdata.object_to_search_for = obj.name
return succeeded
else:
set_grasp_error("GraspStateMachine: obj.name is NONE!! cant pass the name throught for grasping!")
return aborted
def arm_result_cb(userdata, result_status, result):
if result_status != GoalStatus.SUCCEEDED: # SUCCEEDED = 3 http://www.ros.org/doc/api/move_arm_msgs/html/msg/MoveArmActionResult.html
set_grasp_error("MoveArmActionResult result wasn't succeeded, was: " + str(result_status) +
"\nmessage: \n" + str(result))
# if result.error_code.val == 1: # SUCCESS
# rospy.loginfo("But we got SUCCESS as the result, so the planner will replan by itself.")
# return succeeded
return aborted
else:
return succeeded
def loc_response_cb(userdata, response):
if response.exists:
if response.databaseID != 0:
userdata.object_id_from_name = response.databaseID
rospy.loginfo("Got databaseID " + str(response.databaseID) + " for object " + userdata.object_to_search_for)
return succeeded
else:
set_grasp_error("Object " + userdata.object_to_search_for + " not in coord_translator!")
else:
set_grasp_error("We got no response from coord_translator, object " + userdata.object_to_search_for + " not in coord_translator")
return aborted
def grasp_arm_result_cb(userdata, status, result):
if status == GoalStatus.SUCCEEDED:
#rospy.loginfo("Succeeded: result of right_hand_controller: " + str(result.error_code) )
return succeeded
else:
rospy.loginfo("Other than succeded: result of right_hand_controller ( GOAL_TOLERANCE_VIOLATED=-5 ): " + str(result.error_code))
if result.error_code != result.GOAL_TOLERANCE_VIOLATED or result.error_code != result.PATH_TOLERANCE_VIOLATED:
rospy.loginfo("Continuing even with this error as it's not really aborted...")
return succeeded
else:
set_grasp_error("Hand_to_normal_position aborted: GOAL_TOLERANCE_VIOLATED or PATH_TOLERANCE_VIOLATED")
return aborted
def generic_action_result_cb(userdata, status, result):
""" Generic action result callback that set grasp error if the status is not succeeded. """
set_grasp_error(str(self._current_label) + " failed", status)
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
def grasp_arm_result_cb(userdata, status, result):
if status == GoalStatus.SUCCEEDED:
return succeeded
set_grasp_error("Close hand for referee failed")
