cob_gazebo: cob_gazebo: gazebo_services.py Source File

00001 #!/usr/bin/env python
00002 import roslib; roslib.load_manifest('cob_gazebo')
00003 
00004 import rospy
00005 import tf
00006 import actionlib
00007 
00008 from control_msgs.msg import FollowJointTrajectoryAction
00009 from move_base_msgs.msg import MoveBaseAction
00010 
00011 # care-o-bot includes
00012 from cob_srvs.srv import *
00013 
00014 class gazebo_services():
00015 
00016         def __init__(self):
00017                 # base
00018                 self.base_client = actionlib.SimpleActionClient('/move_base', MoveBaseAction)
00019                 #self.base_init_srv = rospy.Service('/base_controller/init', Trigger, self.base_init_cb)
00020                 self.base_stop_srv = rospy.Service('/base_controller/stop', Trigger, self.base_stop_cb)
00021                 #self.base_recover_srv = rospy.Service('/base_controller/recover', Trigger, self.base_recover_cb)
00022                 #self.base_set_operation_mode_srv = rospy.Service('/base_controller/set_operation_mode', SetOperationMode, self.base_set_operation_mode_cb)
00023 
00024                 #torso
00025                 self.torso_client = actionlib.SimpleActionClient('/torso_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
00026                 self.torso_init_srv = rospy.Service('/torso_controller/init', Trigger, self.torso_init_cb)
00027                 self.torso_stop_srv = rospy.Service('/torso_controller/stop', Trigger, self.torso_stop_cb)
00028                 self.torso_recover_srv = rospy.Service('/torso_controller/recover', Trigger, self.torso_recover_cb)
00029                 self.torso_set_operation_mode_srv = rospy.Service('/torso_controller/set_operation_mode', SetOperationMode, self.torso_set_operation_mode_cb)
00030 
00031                 #tray
00032                 self.tray_client = actionlib.SimpleActionClient('/tray_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
00033                 self.tray_init_srv = rospy.Service('/tray_controller/init', Trigger, self.tray_init_cb)
00034                 self.tray_stop_srv = rospy.Service('/tray_controller/stop', Trigger, self.tray_stop_cb)
00035                 self.tray_recover_srv = rospy.Service('/tray_controller/recover', Trigger, self.tray_recover_cb)
00036                 self.tray_set_operation_mode_srv = rospy.Service('/tray_controller/set_operation_mode', SetOperationMode, self.tray_set_operation_mode_cb)
00037 
00038                 #arm
00039                 self.arm_client = actionlib.SimpleActionClient('/arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
00040                 self.arm_init_srv = rospy.Service('/arm_controller/init', Trigger, self.arm_init_cb)
00041                 self.arm_stop_srv = rospy.Service('/arm_controller/stop', Trigger, self.arm_stop_cb)
00042                 self.arm_recover_srv = rospy.Service('/arm_controller/recover', Trigger, self.arm_recover_cb)
00043                 self.arm_set_operation_mode_srv = rospy.Service('/arm_controller/set_operation_mode', SetOperationMode, self.arm_set_operation_mode_cb)
00044                 self.arm_set_joint_stiffness_srv = rospy.Service('/arm_controller/set_joint_stiffness', SetJointStiffness, self.arm_set_joint_stiffness_cb)
00045 
00046                 #arm left
00047                 self.arm_left_client = actionlib.SimpleActionClient('/arm_left_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
00048                 self.arm_left_init_srv = rospy.Service('/arm_left_controller/init', Trigger, self.arm_left_init_cb)
00049                 self.arm_left_stop_srv = rospy.Service('/arm_left_controller/stop', Trigger, self.arm_left_stop_cb)
00050                 self.arm_left_recover_srv = rospy.Service('/arm_left_controller/recover', Trigger, self.arm_left_recover_cb)
00051                 self.arm_left_set_operation_mode_srv = rospy.Service('/arm_left_controller/set_operation_mode', SetOperationMode, self.arm_left_set_operation_mode_cb)
00052                 
00053                 #arm right
00054                 self.arm_right_client = actionlib.SimpleActionClient('/arm_right_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
00055                 self.arm_right_init_srv = rospy.Service('/arm_right_controller/init', Trigger, self.arm_right_init_cb)
00056                 self.arm_right_stop_srv = rospy.Service('/arm_right_controller/stop', Trigger, self.arm_right_stop_cb)
00057                 self.arm_right_recover_srv = rospy.Service('/arm_right_controller/recover', Trigger, self.arm_right_recover_cb)
00058                 self.arm_right_set_operation_mode_srv = rospy.Service('/arm_right_controller/set_operation_mode', SetOperationMode, self.arm_right_set_operation_mode_cb)
00059 
00060                 #sdh
00061                 self.sdh_client = actionlib.SimpleActionClient('/sdh_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
00062                 self.sdh_init_srv = rospy.Service('/sdh_controller/init', Trigger, self.sdh_init_cb)
00063                 self.sdh_stop_srv = rospy.Service('/sdh_controller/stop', Trigger, self.sdh_stop_cb)
00064                 self.sdh_recover_srv = rospy.Service('/sdh_controller/recover', Trigger, self.sdh_recover_cb)
00065                 self.sdh_set_operation_mode_srv = rospy.Service('/sdh_controller/set_operation_mode', SetOperationMode, self.sdh_set_operation_mode_cb)
00066                 
00067                 #sdh left
00068                 self.sdh_left_client = actionlib.SimpleActionClient('/sdh_left_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
00069                 self.sdh_left_init_srv = rospy.Service('/sdh_left_controller/init', Trigger, self.sdh_left_init_cb)
00070                 self.sdh_left_stop_srv = rospy.Service('/sdh_left_controller/stop', Trigger, self.sdh_left_stop_cb)
00071                 self.sdh_left_recover_srv = rospy.Service('/sdh_left_controller/recover', Trigger, self.sdh_left_recover_cb)
00072                 self.sdh_left_set_operation_mode_srv = rospy.Service('/sdh_left_controller/set_operation_mode', SetOperationMode, self.sdh_left_set_operation_mode_cb)
00073                 
00074                 #sdh right
00075                 self.sdh_right_client = actionlib.SimpleActionClient('/sdh_right_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
00076                 self.sdh_right_init_srv = rospy.Service('/sdh_right_controller/init', Trigger, self.sdh_right_init_cb)
00077                 self.sdh_right_stop_srv = rospy.Service('/sdh_right_controller/stop', Trigger, self.sdh_right_stop_cb)
00078                 self.sdh_right_recover_srv = rospy.Service('/sdh_right_controller/recover', Trigger, self.sdh_right_recover_cb)
00079                 self.sdh_right_set_operation_mode_srv = rospy.Service('/sdh_right_controller/set_operation_mode', SetOperationMode, self.sdh_right_set_operation_mode_cb)
00080                 
00081                 #head
00082                 self.head_client = actionlib.SimpleActionClient('/head_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
00083                 self.head_init_srv = rospy.Service('/head_controller/init', Trigger, self.head_init_cb)
00084                 self.head_stop_srv = rospy.Service('/head_controller/stop', Trigger, self.head_stop_cb)
00085                 self.head_recover_srv = rospy.Service('/head_controller/recover', Trigger, self.head_recover_cb)
00086                 self.head_set_operation_mode_srv = rospy.Service('/head_controller/set_operation_mode', SetOperationMode, self.head_set_operation_mode_cb)
00087 
00088         # base
00089         def base_init_cb(self, req):
00090                 resp = TriggerResponse()
00091                 resp.success.data = True
00092                 return resp
00093         
00094         def base_stop_cb(self, req):
00095                 self.base_client.cancel_all_goals()
00096                 resp = TriggerResponse()
00097                 resp.success.data = True
00098                 return resp
00099 
00100         def base_recover_cb(self, req):
00101                 resp = TriggerResponse()
00102                 resp.success.data = True
00103                 return resp
00104         
00105         def base_set_operation_mode_cb(self, req):
00106                 resp = SetOperationModeResponse()
00107                 resp.success.data = True
00108                 return resp
00109 
00110         # torso
00111         def torso_init_cb(self, req):
00112                 resp = TriggerResponse()
00113                 resp.success.data = True
00114                 return resp
00115         
00116         def torso_stop_cb(self, req):
00117                 self.torso_client.cancel_all_goals()
00118                 resp = TriggerResponse()
00119                 resp.success.data = True
00120                 return resp
00121 
00122         def torso_recover_cb(self, req):
00123                 resp = TriggerResponse()
00124                 resp.success.data = True
00125                 return resp
00126         
00127         def torso_set_operation_mode_cb(self, req):
00128                 resp = SetOperationModeResponse()
00129                 resp.success.data = True
00130                 return resp
00131 
00132         #tray
00133         def tray_init_cb(self, req):
00134                 resp = TriggerResponse()
00135                 resp.success.data = True
00136                 return resp
00137         
00138         def tray_stop_cb(self, req):
00139                 self.tray_client.cancel_all_goals()
00140                 resp = TriggerResponse()
00141                 resp.success.data = True
00142                 return resp
00143 
00144         def tray_recover_cb(self, req):
00145                 resp = TriggerResponse()
00146                 resp.success.data = True
00147                 return resp
00148         
00149         def tray_set_operation_mode_cb(self, req):
00150                 resp = SetOperationModeResponse()
00151                 resp.success.data = True
00152                 return resp
00153 
00154         # arm
00155         def arm_init_cb(self, req):
00156                 resp = TriggerResponse()
00157                 resp.success.data = True
00158                 return resp
00159         
00160         def arm_stop_cb(self, req):
00161                 self.arm_client.cancel_all_goals()
00162                 resp = TriggerResponse()
00163                 resp.success.data = True
00164                 return resp
00165 
00166         def arm_recover_cb(self, req):
00167                 resp = TriggerResponse()
00168                 resp.success.data = True
00169                 return resp
00170         
00171         def arm_set_operation_mode_cb(self, req):
00172                 resp = SetOperationModeResponse()
00173                 resp.success.data = True
00174                 return resp
00175 
00176         def arm_set_joint_stiffness_cb(self, req):
00177                 resp = SetJointStiffnessResponse()
00178                 resp.success.data = True
00179                 return resp
00180                 
00181         # arm left
00182         def arm_left_init_cb(self, req):
00183                 resp = TriggerResponse()
00184                 resp.success.data = True
00185                 return resp
00186         
00187         def arm_left_stop_cb(self, req):
00188                 self.arm_left_client.cancel_all_goals()
00189                 resp = TriggerResponse()
00190                 resp.success.data = True
00191                 return resp
00192 
00193         def arm_left_recover_cb(self, req):
00194                 resp = TriggerResponse()
00195                 resp.success.data = True
00196                 return resp
00197         
00198         def arm_left_set_operation_mode_cb(self, req):
00199                 resp = SetOperationModeResponse()
00200                 resp.success.data = True
00201                 return resp
00202                 
00203         # arm right
00204         def arm_right_init_cb(self, req):
00205                 resp = TriggerResponse()
00206                 resp.success.data = True
00207                 return resp
00208         
00209         def arm_right_stop_cb(self, req):
00210                 self.arm_right_client.cancel_all_goals()
00211                 resp = TriggerResponse()
00212                 resp.success.data = True
00213                 return resp
00214 
00215         def arm_right_recover_cb(self, req):
00216                 resp = TriggerResponse()
00217                 resp.success.data = True
00218                 return resp
00219         
00220         def arm_right_set_operation_mode_cb(self, req):
00221                 resp = SetOperationModeResponse()
00222                 resp.success.data = True
00223                 return resp
00224 
00225         # sdh
00226         def sdh_init_cb(self, req):
00227                 resp = TriggerResponse()
00228                 resp.success.data = True
00229                 return resp
00230         
00231         def sdh_stop_cb(self, req):
00232                 self.sdh_client.cancel_all_goals()
00233                 resp = TriggerResponse()
00234                 resp.success.data = True
00235                 return resp
00236 
00237         def sdh_recover_cb(self, req):
00238                 resp = TriggerResponse()
00239                 resp.success.data = True
00240                 return resp
00241         
00242         def sdh_set_operation_mode_cb(self, req):
00243                 resp = SetOperationModeResponse()
00244                 resp.success.data = True
00245                 return resp
00246                 
00247         # sdh left
00248         def sdh_left_init_cb(self, req):
00249                 resp = TriggerResponse()
00250                 resp.success.data = True
00251                 return resp
00252         
00253         def sdh_left_stop_cb(self, req):
00254                 self.sdh_left_client.cancel_all_goals()
00255                 resp = TriggerResponse()
00256                 resp.success.data = True
00257                 return resp
00258 
00259         def sdh_left_recover_cb(self, req):
00260                 resp = TriggerResponse()
00261                 resp.success.data = True
00262                 return resp
00263         
00264         def sdh_left_set_operation_mode_cb(self, req):
00265                 resp = SetOperationModeResponse()
00266                 resp.success.data = True
00267                 return resp
00268                 
00269         # sdh right
00270         def sdh_right_init_cb(self, req):
00271                 resp = TriggerResponse()
00272                 resp.success.data = True
00273                 return resp
00274         
00275         def sdh_right_stop_cb(self, req):
00276                 self.sdh_right_client.cancel_all_goals()
00277                 resp = TriggerResponse()
00278                 resp.success.data = True
00279                 return resp
00280 
00281         def sdh_right_recover_cb(self, req):
00282                 resp = TriggerResponse()
00283                 resp.success.data = True
00284                 return resp
00285         
00286         def sdh_right_set_operation_mode_cb(self, req):
00287                 resp = SetOperationModeResponse()
00288                 resp.success.data = True
00289                 return resp
00290 
00291         # head
00292         def head_init_cb(self, req):
00293                 resp = TriggerResponse()
00294                 resp.success.data = True
00295                 return resp
00296         
00297         def head_stop_cb(self, req):
00298                 self.head_client.cancel_all_goals()
00299                 resp = TriggerResponse()
00300                 resp.success.data = True
00301                 return resp
00302 
00303         def head_recover_cb(self, req):
00304                 resp = TriggerResponse()
00305                 resp.success.data = True
00306                 return resp
00307         
00308         def head_set_operation_mode_cb(self, req):
00309                 resp = SetOperationModeResponse()
00310                 resp.success.data = True
00311                 return resp
00312 
00313 if __name__ == "__main__":
00314    rospy.init_node('gazebo_services')
00315    gazebo_services()
00316    rospy.loginfo("gazebo_services running")
00317    rospy.spin()
00318