Source Code for Module ee_cart_imped_control.control_switcher

 1  #!/usr/bin/env python 
 2  import roslib 
 3  roslib.load_manifest('ee_cart_imped_control') 
 4  import rospy 
 5  import pr2_controller_manager.pr2_controller_manager_interface 
 6   
 7 -class PR2CMClient: 
 8      ''' 
 9      This class switches between the ee_cart_imped controller and the  
10      standard arm_navigation suite of controllers on the PR2 arms. 
11   
12      When invoked, the methods stop one controller and start the other.   
13      They require that the controllers are already loaded (albeit stopped). 
14      The arm_navigation controllers can be loaded from that stack or by 
15      launching the  
16      pr2_tabletop_manipulation_launch/launch/pr2_tabletop_manipulation.launch  
17      file.  The ee_cart_imped controller can be launched using the 
18      ee_cart_imped_launch/launch/load_ee_cart_imped.launch file.  Note that 
19      using the ee_cart_imped.launch file will not work as that not only starts 
20      the ee_cart_imped controller, but also continuously inhibits the 
21      arm_navigation controllers. 
22      ''' 
23      
24      @staticmethod 
25 -    def load_ee_cart_imped(arm_name): 
26          ''' 
27          Stops the standard arm controllers and starts the ee_cart_imped 
28          controller. 
29          @type arm_name: string 
30          @param arm_name: the arm to control.  Must be 'right_arm' or 'left_arm' 
31          ''' 
32          rospy.logdebug('Starting ee_cart_imped controller on '+ arm_name) 
33          status = pr2_controller_manager.pr2_controller_manager_interface.stop_controller(arm_name[0]+'_arm_controller') 
34          status = pr2_controller_manager.pr2_controller_manager_interface.stop_controller\ 
35              (arm_name[0]+'_arm_cartesian_trajectory_controller') 
36          status = pr2_controller_manager.pr2_controller_manager_interface.stop_controller\ 
37              (arm_name[0]+'_arm_cartesian_pose_controller') 
38          status = pr2_controller_manager.pr2_controller_manager_interface.start_controller\ 
39              (arm_name[0]+'_arm_cart_imped_controller') 
40          rospy.logdebug('Controller started') 
41          return status 
42           
43      @staticmethod 
44 -    def load_cartesian(arm_name): 
45          ''' 
46          Stops the ee_cart_imped controller and the arm_navigation controllers 
47          and loads the standard r_arm_controller. 
48          @type arm_name: string 
49          @param arm_name: the arm to control.  Must be 'right_arm' or 'left_arm' 
50          ''' 
51          rospy.logdebug('starting cartesian controller on '+ arm_name) 
52          status = pr2_controller_manager.pr2_controller_manager_interface.stop_controller\ 
53              (arm_name[0]+'_arm_cart_imped_controller') 
54          status = pr2_controller_manager.pr2_controller_manager_interface.start_controller(arm_name[0]+'_arm_controller') 
55          status = pr2_controller_manager.pr2_controller_manager_interface.stop_controller\ 
56              (arm_name[0]+'_arm_cartesian_trajectory_controller') 
57          status = pr2_controller_manager.pr2_controller_manager_interface.stop_controller\ 
58              (arm_name[0]+'_arm_cartesian_pose_controller') 
59          rospy.logdebug('Controller started') 
60          return status 
61