target_obj_pose = Pose()

	target_obj_pose.position.x = 0.64
	target_obj_pose.position.y = 0.9
	target_obj_pose.position.z = 0.8
	target_obj_pose.orientation.x = 0
	target_obj_pose.orientation.y = 0
	target_obj_pose.orientation.z = 0
	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
示例#2
0
            #get the index of the target workspace e.g. table0    
            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'
            #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,
示例#4
0
def getWorkspaceOnMap():
    print 'test get all workspace (furnitures basically here) from map'
    try:
        requestNewTask = rospy.ServiceProxy('get_workspace_on_map',
                                            GetWorkspaceOnMap)
        res = requestNewTask('ipa-kitchen-map', True)
        return res
    except rospy.ServiceException, e:
        print "Service call failed: %s" % e


if __name__ == "__main__":

    workspace_info = getWorkspaceOnMap()

    parent_obj_geometry = SRSSpatialInfo()

    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
示例#5
0

	target_obj_pose = Pose()

	target_obj_pose.position.x = 0.64
	target_obj_pose.position.y = 0.9
	target_obj_pose.position.z = 0.8
	target_obj_pose.orientation.x = 0
	target_obj_pose.orientation.y = 0
	target_obj_pose.orientation.z = 0
	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