00001
00002
00003 import time
00004
00005 import roslib
00006 roslib.load_manifest('cob_script_server')
00007 import rospy
00008
00009 import simple_script_server
00010 import script_utils
00011 import sys
00012 sys.path.insert(0,'/home/cob/svn/cob/Tests/GO/')
00013 from ScriptUtils import *
00014 from ScriptParameter import *
00015 del sys.path[0]
00016
00017 class GraspFromCoolerAndDeliver(ssscript):
00018
00019 def Initialize(self, servers):
00020 self.su = script_utils.script_utils()
00021 self.util = ScriptUtils(servers)
00022 self.sss.Init("tray")
00023 self.sss.Init("torso")
00024 self.sss.Init("arm")
00025 self.sss.Init("sdh")
00026
00027 self.sss.Move("tray","down",False)
00028 self.sss.Move("sdh","home",False)
00029 self.sss.Move("torso","back",False)
00030 self.su.MoveLED("arm","folded")
00031 self.sss.SetLight("green")
00032
00033
00034 def GraspFromCooler(self):
00035 rospy.loginfo("Grasping water from cooler...")
00036
00037
00038 handle01 = self.sss.Move("arm","pregrasp",False)
00039 self.su.MoveLED("sdh","coolerbuttonup")
00040 handle01.wait()
00041
00042
00043 self.su.MoveLED("arm","coolerbutton")
00044 self.sss.Move("sdh","coolerbuttondown")
00045 self.sss.sleep(3)
00046 self.sss.Move("sdh","coolerbuttonup")
00047
00048
00049 handle01 = self.sss.Move("arm","coolerpregrasp",False)
00050 self.sss.sleep(0.5)
00051 self.sss.Move("sdh","cylclosed")
00052 handle01.wait()
00053 self.sss.Move("sdh","coolercupopen")
00054 self.su.MoveLED("arm","coolergrasp")
00055 self.sss.Move("sdh","coolercupclosed")
00056
00057
00058 self.su.MoveLED("arm","coolerpostgrasp")
00059 handle01 = self.sss.Move("arm","coolerpostgrasp-to-tablet",False)
00060 self.sss.sleep(2)
00061 self.sss.Move("torso","front",False)
00062 self.sss.Move("tray","up")
00063 handle01.wait()
00064 self.su.MoveLED("arm","tablet")
00065 self.sss.Move("sdh","cuprelease")
00066
00067
00068 handle01 = self.sss.Move("arm","tablet-to-folded",False)
00069 self.sss.sleep(2)
00070 self.sss.Move("sdh","home")
00071 handle01.wait()
00072 return 0
00073
00074 def DriveToCooler(self):
00075 rospy.loginfo("Driving to water cooler ...")
00076
00077
00078 position_param = "script_server/base/watercooler_mm_deg"
00079 if not rospy.has_param(position_param):
00080 rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",position_param)
00081 return -2
00082 else:
00083 position = rospy.get_param(position_param)
00084 handle01 = self.sss.Move("torso","back",False)
00085 self.util.movePlatformWait(*position)
00086 handle01.wait()
00087 return 0
00088
00089 def DeliverDrink(self):
00090 rospy.loginfo("Delivering drink ...")
00091
00092
00093 position_param = "script_server/base/table_mm_deg"
00094 if not rospy.has_param(position_param):
00095 rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",position_param)
00096 return -2
00097 else:
00098 position = rospy.get_param(position_param)
00099 handle01 = self.sss.Move("torso","front",False)
00100 self.util.movePlatformWait(*position)
00101 handle01.wait()
00102
00103
00104 handle01 = self.sss.Move("torso","left",False)
00105 self.su.SpeakRandom("offer")
00106 handle01.wait()
00107 self.sss.Move("torso","right")
00108 self.sss.Move("torso","front")
00109
00110
00111 taken_param = self.sss.wait_for_input()
00112 if taken_param == "\n":
00113 handle01 = self.sss.Move("torso","bow",False)
00114 self.su.SpeakRandom("taken")
00115 handle01.wait()
00116 self.sss.sleep(0.5)
00117
00118
00119 position_param = "script_server/base/watercooler_mm_deg"
00120 if not rospy.has_param(position_param):
00121 rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",position_param)
00122 return -2
00123 else:
00124 position = rospy.get_param(position_param)
00125 self.util.movePlatformWait(*position)
00126 if taken_param == "\n":
00127 self.sss.Move("tray","down",False)
00128 return 0
00129
00130 def run(self):
00131 self.Initialize()
00132 self.DriveToCooler()
00133 self.GraspFromCooler()
00134 self.DeliverDrink()
00135 self.sss.Move("sdh","coolercupopen")
00136 self.su.MoveLED("arm","coolergrasp")
00137 self.sss.wait_for_input()
00138 self.sss.Move("sdh","coolercupclosed")
00139 self.su.MoveLED("arm","grasp-to-tablet")
00140 self.su.MoveLED("arm","tablet-to-folded")
00141
00142 if __name__ == "__main__":
00143 servers = Servers()
00144 SCRIPT = GraspFromCoolerAndDeliver(servers)
00145 SCRIPT.Start('grasp_deliver')
00146
00147 SCRIPT.run()