cob_3d_mapping_gazebo: RecordDiffRange.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/python
00002 import sys
00003 import roslib
00004 roslib.load_manifest('cob_3d_mapping_gazebo')
00005 roslib.load_manifest('cob_gazebo_worlds')
00006 roslib.load_manifest('cob_script_server')
00007 
00008 import rospy
00009 import os
00010 
00011 from gazebo.srv import *
00012 from simple_script_server import script
00013 
00014 class MyScript(script):
00015         def Run(self):
00016                 self.sss.move("torso",[[-0.2,0,-0.2]])
00017                 self.sss.move("head","front")
00018                 self.sss.move("tray","down")
00019                 # Move training table on top of gripper
00020                 srv_set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
00021                 req_set = SetModelStateRequest()
00022                 req_set.model_state.model_name = "cob3"
00023                 req_set.model_state.pose.position.x = -2.5
00024                 req_set.model_state.pose.position.y = 0
00025                 req_set.model_state.pose.position.z = 0
00026                 req_set.model_state.pose.orientation.w = 0
00027                 req_set.model_state.pose.orientation.x = 0
00028                 req_set.model_state.pose.orientation.y = 0
00029                 req_set.model_state.pose.orientation.z = 0
00030                 req_set.model_state.reference_frame = "map"     
00031                 res_set = srv_set_model_state(req_set)
00032                 self.sss.wait_for_input()
00033 
00034                 req_set.model_state.pose.position.x = -3
00035                 res_set = srv_set_model_state(req_set)
00036                 self.sss.wait_for_input()
00037                 
00038                 self.sss.move("torso",[[-0.1,0,-0.2]])
00039                 req_set.model_state.pose.position.x = -3.5
00040                 res_set = srv_set_model_state(req_set)
00041                 self.sss.wait_for_input()
00042                 
00043                 req_set.model_state.pose.position.x = -4
00044                 res_set = srv_set_model_state(req_set)
00045                 self.sss.wait_for_input()
00046 
00047                 
00048 if __name__ == "__main__":
00049         SCRIPT = MyScript()
00050         SCRIPT.Start()
00051         #if len(sys.argv) < 2:
00052         #       print '[Train_LoadObject.py] Please specify the name of the urdf model to be loaded'
00053         #       sys.exit()
00054         
00055 
00056                 
00057 
00058 
00059