00001 #! /usr/bin/env python 00002 00003 import roslib; roslib.load_manifest('tidyup_tools') 00004 import rospy 00005 import sys, traceback 00006 from tidyup_msgs import msg 00007 from tidyup_msgs import srv 00008 from pr2_python.planning_scene_interface import * 00009 from geometry_msgs.msg import Point, Pose, PoseStamped 00010 00011 if __name__ == '__main__': 00012 try: 00013 rospy.init_node('test_putdown_module', anonymous=True) 00014 rospy.loginfo('setting up planning scene...') 00015 psi = get_planning_scene_interface() 00016 psi.reset() 00017 # attach object to gripper 00018 object_name = 'bowl_1' 00019 # #object = psi.collision_object(object_name) 00020 # pose = Pose() 00021 # pose.position.x = 0.1 00022 # pose.position.y = 0.05 00023 # pose.position.z = 0.0 00024 # pose.orientation.x = 0.707 00025 # pose.orientation.y = -0.106 00026 # pose.orientation.z = -0.690 00027 # pose.orientation.w = 0.105 00028 # #object.poses[0] = pose 00029 # #object.header.frame_id = 'l_gripper_r_finger_tip_link' 00030 # #psi.add_object(object) 00031 # #if not psi.attach_object_to_gripper('left_arm', object_name): 00032 # # rospy.logerr('attach object failed.') 00033 # 00034 # # set robot pose 00035 # pose = Pose() 00036 # #table1_loc1_room1 /map 0.120862 0.698891 0.0510018 0 0 0.251132 0.967953 00037 # #table1_loc1_room1 /map 15.84 18.4088 0.0531112 0 0 0.999299 0.037442 00038 ## pose.position.x = 15.84 00039 ## pose.position.y = 18.40 00040 ## pose.position.z = 0.0510018 00041 ## pose.orientation.x = 0 00042 ## pose.orientation.y = 0 00043 ## pose.orientation.z = 1 00044 ## pose.orientation.w = 0 00045 # #table2_loc4_room2 1348583312 347842222 /map 20.0308 17.9577 0.0500652 0 0 -0.723473 0.690353 00046 # pose.position.x = 20.0308 00047 # pose.position.y = 17.9577 00048 # pose.position.z = 0.0510018 00049 # pose.orientation.x = 0 00050 # pose.orientation.y = 0 00051 # pose.orientation.z = -0.707 00052 # pose.orientation.w = 0.707 00053 # robot_state = psi.get_robot_state() 00054 # robot_state.multi_dof_joint_state.poses[0] = pose 00055 # converted = list(robot_state.joint_state.position) 00056 # converted[0] = pose.position.x 00057 # converted[1] = pose.position.y; 00058 # converted[2] = pose.position.z; 00059 # converted[3] = pose.orientation.x; 00060 # converted[4] = pose.orientation.y; 00061 # converted[5] = pose.orientation.z; 00062 # converted[6] = pose.orientation.w; 00063 # robot_state.joint_state.position = converted 00064 # rospy.loginfo('setting robot state...') 00065 # #psi.set_robot_state(robot_state) 00066 # rospy.loginfo(psi.get_robot_state()) 00067 # 00068 putdwon_service = rospy.ServiceProxy('/tidyup/request_putdown_pose', srv.GetPutdownPose) 00069 putdwon_service.wait_for_service() 00070 rospy.loginfo('connected to service.') 00071 request = srv.GetPutdownPoseRequest() 00072 request.arm = 'right_arm' 00073 request.static_object = 'table1_loc1' 00074 request.putdown_object = object_name 00075 response = putdwon_service.call(request) 00076 #psi.reset() 00077 rospy.loginfo('done.') 00078 except: 00079 traceback.print_exc(file=sys.stdout)
