target_obj_pose.orientation.w = 1 workspace_info = getWorkspaceOnMap() parent_obj_geometry = SRSSpatialInfo() parent_obj_geometry.pose.position.x = workspace_info.objectsInfo[0].pose.position.x parent_obj_geometry.pose.position.y = workspace_info.objectsInfo[0].pose.position.y parent_obj_geometry.pose.position.z = workspace_info.objectsInfo[0].pose.position.z parent_obj_geometry.pose.orientation.x = workspace_info.objectsInfo[0].pose.orientation.x parent_obj_geometry.pose.orientation.y = workspace_info.objectsInfo[0].pose.orientation.y parent_obj_geometry.pose.orientation.z = workspace_info.objectsInfo[0].pose.orientation.z parent_obj_geometry.pose.orientation.w = workspace_info.objectsInfo[0].pose.orientation.w parent_obj_geometry.l = workspace_info.objectsInfo[0].l parent_obj_geometry.w = workspace_info.objectsInfo[0].w parent_obj_geometry.h = workspace_info.objectsInfo[0].h #parent_obj_geometry.pose.position.x = -3.3 #parent_obj_geometry.pose.position.y = 0.5 furniture_geometry_list = list() furniture_geometry_list = workspace_info.objectsInfo #rospy.loginfo(workspace_info.objectsInfo[6]) print "Requesting reachability and grasp base pose." result = symbol_grounding_grasp_base_pose_experimental_client(target_obj_pose, parent_obj_geometry, furniture_geometry_list) print result #print math.atan(0.26 / 0.7)
index_of_the_target_workspace = all_workspaces_on_map.objects.index(userdata.target_workspace_name) #get the pose of the workspace from knowledge service target_object_pose = all_workspaces_on_map.objectsInfo[index_of_the_target_workspace].pose #get the houseHoldID of the workspace object_id = all_workspaces_on_map.houseHoldId[index_of_the_target_workspace] #rospy.loginfo ("target name: %s", userdata.target_workspace_name) #rospy.loginfo ("target pose: %s", target_object_pose) #rospy.loginfo ("target id: %s", object_id) except rospy.ServiceException, e: print "Service call failed: %s"%e return 'failed' parent_obj_geometry = SRSSpatialInfo() parent_obj_geometry.pose = target_object_pose parent_obj_geometry.l = all_workspaces_on_map.objectsInfo[index_of_the_target_workspace].l parent_obj_geometry.w = all_workspaces_on_map.objectsInfo[index_of_the_target_workspace].w parent_obj_geometry.h = all_workspaces_on_map.objectsInfo[index_of_the_target_workspace].h rospy.wait_for_service('symbol_grounding_grasp_base_pose_experimental') symbol_grounding_grasp_base_pose_experimental = rospy.ServiceProxy('symbol_grounding_grasp_base_pose_experimental', SymbolGroundingGraspBasePoseExperimental) try: result = symbol_grounding_grasp_base_pose_experimental(object_pose_map.pose, parent_obj_geometry, all_workspaces_on_map.objectsInfo) userdata.base_pose = [result.grasp_base_pose.x, result.grasp_base_pose.y, result.grasp_base_pose.theta] return 'retry' except rospy.ServiceException, e: print "Service call failed: %s" %e return 'failed'
object_id = all_workspaces_on_map.houseHoldId[ index_of_the_target_workspace] #rospy.loginfo ("target name: %s", userdata.target_workspace_name) #rospy.loginfo ("target pose: %s", target_object_pose) #rospy.loginfo ("target id: %s", object_id) except rospy.ServiceException, e: print "Service call failed: %s" % e return 'failed' parent_obj_geometry = SRSSpatialInfo() parent_obj_geometry.pose = target_object_pose parent_obj_geometry.l = all_workspaces_on_map.objectsInfo[ index_of_the_target_workspace].l parent_obj_geometry.w = all_workspaces_on_map.objectsInfo[ index_of_the_target_workspace].w parent_obj_geometry.h = all_workspaces_on_map.objectsInfo[ index_of_the_target_workspace].h rospy.wait_for_service('symbol_grounding_grasp_base_pose_experimental') symbol_grounding_grasp_base_pose_experimental = rospy.ServiceProxy( 'symbol_grounding_grasp_base_pose_experimental', SymbolGroundingGraspBasePoseExperimental) try: result = symbol_grounding_grasp_base_pose_experimental( object_pose_map.pose, parent_obj_geometry, all_workspaces_on_map.objectsInfo) userdata.base_pose = [ result.grasp_base_pose.x, result.grasp_base_pose.y, result.grasp_base_pose.theta ]
parent_obj_geometry.pose.position.x = workspace_info.objectsInfo[ 2].pose.position.x parent_obj_geometry.pose.position.y = workspace_info.objectsInfo[ 2].pose.position.y parent_obj_geometry.pose.position.z = workspace_info.objectsInfo[ 2].pose.position.z parent_obj_geometry.pose.orientation.x = workspace_info.objectsInfo[ 2].pose.orientation.x parent_obj_geometry.pose.orientation.y = workspace_info.objectsInfo[ 2].pose.orientation.y parent_obj_geometry.pose.orientation.z = workspace_info.objectsInfo[ 2].pose.orientation.z parent_obj_geometry.pose.orientation.w = workspace_info.objectsInfo[ 2].pose.orientation.w parent_obj_geometry.l = workspace_info.objectsInfo[2].l parent_obj_geometry.w = workspace_info.objectsInfo[2].w parent_obj_geometry.h = workspace_info.objectsInfo[2].h rospy.loginfo(parent_obj_geometry.pose) #parent_obj_geometry.pose.position.x = 8.6 #parent_obj_geometry.pose.position.y = 2.5 #parent_obj_geometry.pose.orientation.x = 0 #parent_obj_geometry.pose.orientation.y = 0 #parent_obj_geometry.pose.orientation.z = -0.999783754349 #parent_obj_geometry.pose.orientation.w = 0.0207948293537 #parent_obj_geometry.l = 1.5 #parent_obj_geometry.w = 0.4 furniture_geometry_list = list() furniture_geometry_list = workspace_info.objectsInfo
parent_obj_geometry.pose.orientation.w = workspace_info.objectsInfo[0].pose.orientation.w parent_obj_geometry.l = workspace_info.objectsInfo[0].l parent_obj_geometry.w = workspace_info.objectsInfo[0].w parent_obj_geometry.h = workspace_info.objectsInfo[0].h ''' parent_obj_geometry.pose.position.x = -3.3 parent_obj_geometry.pose.position.y = 0.5 parent_obj_geometry.pose.position.z = 1.0 parent_obj_geometry.pose.orientation.x = 0 parent_obj_geometry.pose.orientation.y = 0 parent_obj_geometry.pose.orientation.z = 0 parent_obj_geometry.pose.orientation.w = 1.0 parent_obj_geometry.l = 2.0 parent_obj_geometry.w = 1.0 parent_obj_geometry.h = 0.85 furniture_geometry_list = list() #furniture_geometry_list = workspace_info.objectsInfo #rospy.loginfo(workspace_info.objectsInfo[6]) print "Requesting reachability and grasp base pose." result = symbol_grounding_grasp_base_pose_experimental_client(target_obj_pose, parent_obj_geometry, furniture_geometry_list) print result #print math.atan(0.26 / 0.7)