sr_gui_change_controllers: change_controllers.py Source File

Go to the documentation of this file.
00001 # Copyright (c) 2011, Dirk Thomas, TU Darmstadt
00002 # All rights reserved.
00003 #
00004 # Redistribution and use in source and binary forms, with or without
00005 # modification, are permitted provided that the following conditions
00006 # are met:
00007 #
00008 #   * Redistributions of source code must retain the above copyright
00009 #     notice, this list of conditions and the following disclaimer.
00010 #   * Redistributions in binary form must reproduce the above
00011 #     copyright notice, this list of conditions and the following
00012 #     disclaimer in the documentation and/or other materials provided
00013 #     with the distribution.
00014 #   * Neither the name of the TU Darmstadt nor the names of its
00015 #     contributors may be used to endorse or promote products derived
00016 #     from this software without specific prior written permission.
00017 #
00018 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00019 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00020 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00021 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00022 # COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00023 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00024 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00025 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00026 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00027 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00028 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00029 # POSSIBILITY OF SUCH DAMAGE.
00030 
00031 
00032 import os, rospy, rospkg
00033 
00034 from time import sleep
00035 
00036 from qt_gui.plugin import Plugin
00037 from python_qt_binding import loadUi
00038 
00039 from QtCore import QEvent, QObject, Qt, QTimer, Slot
00040 from QtGui import QShortcut, QMessageBox, QWidget, QIcon
00041 from pr2_mechanism_msgs.srv import ListControllers, SwitchController, LoadController
00042 from sr_robot_msgs.srv import ChangeControlType
00043 from sr_robot_msgs.msg import ControlType
00044 
00045 class SrGuiChangeControllers(Plugin):
00046     """
00047     A rosgui plugin for loading the different controllers
00048     """
00049     ICON_DIR = os.path.join(rospkg.RosPack().get_path('sr_visualization_icons'), 'icons')
00050     CONTROLLER_ON_ICON = QIcon(os.path.join(ICON_DIR, 'green.png'))
00051     CONTROLLER_OFF_ICON = QIcon(os.path.join(ICON_DIR, 'red.png'))
00052 
00053     controllers = {"effort": ["sh_ffj0_effort_controller", "sh_ffj3_effort_controller", "sh_ffj4_effort_controller", "sh_mfj0_effort_controller", "sh_mfj3_effort_controller", "sh_mfj4_effort_controller", "sh_rfj0_effort_controller", "sh_rfj3_effort_controller", "sh_rfj4_effort_controller", "sh_lfj0_effort_controller", "sh_lfj3_effort_controller", "sh_lfj4_effort_controller", "sh_lfj5_effort_controller", "sh_thj1_effort_controller", "sh_thj2_effort_controller", "sh_thj3_effort_controller", "sh_thj4_effort_controller", "sh_thj5_effort_controller", "sh_wrj1_effort_controller", "sh_wrj2_effort_controller"],
00054                    "position": ["sh_ffj0_position_controller", "sh_ffj3_position_controller", "sh_ffj4_position_controller", "sh_mfj0_position_controller", "sh_mfj3_position_controller", "sh_mfj4_position_controller", "sh_rfj0_position_controller", "sh_rfj3_position_controller", "sh_rfj4_position_controller", "sh_lfj0_position_controller", "sh_lfj3_position_controller", "sh_lfj4_position_controller", "sh_lfj5_position_controller", "sh_thj1_position_controller", "sh_thj2_position_controller", "sh_thj3_position_controller", "sh_thj4_position_controller", "sh_thj5_position_controller", "sh_wrj1_position_controller", "sh_wrj2_position_controller"],
00055                    "mixed": ["sh_ffj0_mixed_position_velocity_controller", "sh_ffj3_mixed_position_velocity_controller", "sh_ffj4_mixed_position_velocity_controller", "sh_mfj0_mixed_position_velocity_controller", "sh_mfj3_mixed_position_velocity_controller", "sh_mfj4_mixed_position_velocity_controller", "sh_rfj0_mixed_position_velocity_controller", "sh_rfj3_mixed_position_velocity_controller", "sh_rfj4_mixed_position_velocity_controller", "sh_lfj0_mixed_position_velocity_controller", "sh_lfj3_mixed_position_velocity_controller", "sh_lfj4_mixed_position_velocity_controller", "sh_lfj5_mixed_position_velocity_controller", "sh_thj1_mixed_position_velocity_controller", "sh_thj2_mixed_position_velocity_controller", "sh_thj3_mixed_position_velocity_controller", "sh_thj4_mixed_position_velocity_controller", "sh_thj5_mixed_position_velocity_controller", "sh_wrj1_mixed_position_velocity_controller", "sh_wrj2_mixed_position_velocity_controller"],
00056                    "velocity": ["sh_ffj0_velocity_controller", "sh_ffj3_velocity_controller", "sh_ffj4_velocity_controller", "sh_mfj0_velocity_controller", "sh_mfj3_velocity_controller", "sh_mfj4_velocity_controller", "sh_rfj0_velocity_controller", "sh_rfj3_velocity_controller", "sh_rfj4_velocity_controller", "sh_lfj0_velocity_controller", "sh_lfj3_velocity_controller", "sh_lfj4_velocity_controller", "sh_lfj5_velocity_controller", "sh_thj1_velocity_controller", "sh_thj2_velocity_controller", "sh_thj3_velocity_controller", "sh_thj4_velocity_controller", "sh_thj5_velocity_controller", "sh_wrj1_velocity_controller", "sh_wrj2_velocity_controller"],
00057                    "stop": []}
00058 
00059     def __init__(self, context):
00060         super(SrGuiChangeControllers, self).__init__(context)
00061         self.setObjectName('SrGuiChangeControllers')
00062 
00063         self._publisher = None
00064         self._widget = QWidget()
00065 
00066         ui_file = os.path.join(rospkg.RosPack().get_path('sr_gui_change_controllers'), 'uis', 'SrChangeControllers.ui')
00067         loadUi(ui_file, self._widget)
00068         self._widget.setObjectName('SrChangeControllersUi')
00069         context.add_widget(self._widget)
00070         
00071         #Setting the initial state of the controller buttons
00072         self._widget.btn_mixed.setIcon(self.CONTROLLER_OFF_ICON)
00073         self._widget.btn_mixed.setChecked(False)
00074         self._widget.btn_effort.setIcon(self.CONTROLLER_OFF_ICON)
00075         self._widget.btn_effort.setChecked(False)
00076         self._widget.btn_position.setIcon(self.CONTROLLER_OFF_ICON)
00077         self._widget.btn_position.setChecked(False)
00078         self._widget.btn_velocity.setIcon(self.CONTROLLER_OFF_ICON)
00079         self._widget.btn_velocity.setChecked(False)
00080 
00081         #attaching the button press event to their actions
00082         self._widget.btn_stop.pressed.connect(self.on_stop_ctrl_clicked_)
00083         self._widget.btn_effort.pressed.connect(self.on_effort_ctrl_clicked_)
00084         self._widget.btn_position.pressed.connect(self.on_position_ctrl_clicked_)
00085         self._widget.btn_mixed.pressed.connect(self.on_mixed_ctrl_clicked_)
00086         self._widget.btn_velocity.pressed.connect(self.on_velocity_ctrl_clicked_)
00087                 
00088         #check the correct control box, depending on PWM_CONTROL env variable
00089         if os.environ.get('PWM_CONTROL') in [None, '0']:
00090             self._widget.radioButtonTorque.setChecked(True)
00091             self._widget.radioButtonPWM.setChecked(False)
00092         else:
00093             self._widget.radioButtonTorque.setChecked(False)
00094             self._widget.radioButtonPWM.setChecked(True)
00095         
00096         self._widget.radioButtonTorque.toggled.connect(self.on_control_mode_radio_button_toggled_)
00097         self._widget.radioButtonPWM.toggled.connect(self.on_control_mode_radio_button_toggled_)
00098 
00099     def on_control_mode_radio_button_toggled_(self, checked):
00100         """
00101         Switch between FORCE, PWM modes
00102         We only react to the currently ON radio button event
00103         """
00104         if checked:
00105             change_type_msg = ChangeControlType()
00106             if self._widget.radioButtonTorque.isChecked():
00107                 change_type_msg.control_type = ControlType.FORCE
00108                 rospy.loginfo("Change Control mode to FORCE")
00109             else:
00110                 change_type_msg.control_type = ControlType.PWM
00111                 rospy.loginfo("Change Control mode to PWM")
00112             self.change_force_ctrl_type(change_type_msg)
00113         
00114     def on_stop_ctrl_clicked_(self):
00115         """
00116         Stop controller
00117         """
00118         self._widget.btn_stop.setEnabled(False)
00119         self._widget.btn_mixed.setIcon(self.CONTROLLER_OFF_ICON)
00120         self._widget.btn_mixed.setChecked(False)
00121         self._widget.btn_effort.setIcon(self.CONTROLLER_OFF_ICON)
00122         self._widget.btn_effort.setChecked(False)
00123         self._widget.btn_position.setIcon(self.CONTROLLER_OFF_ICON)
00124         self._widget.btn_position.setChecked(False)
00125         self._widget.btn_velocity.setIcon(self.CONTROLLER_OFF_ICON)
00126         self._widget.btn_velocity.setChecked(False)
00127         self.change_ctrl( "stop" )
00128         self._widget.btn_stop.setEnabled(True)
00129 
00130     def on_effort_ctrl_clicked_(self):
00131         """
00132         Effort controller selected
00133         """
00134         self._widget.btn_effort.setEnabled(False)
00135         if not self._widget.btn_effort.isChecked():
00136             self._widget.btn_effort.setIcon(self.CONTROLLER_ON_ICON)
00137             self._widget.btn_effort.setChecked(True)
00138             self._widget.btn_position.setIcon(self.CONTROLLER_OFF_ICON)
00139             self._widget.btn_position.setChecked(False)
00140             self._widget.btn_mixed.setIcon(self.CONTROLLER_OFF_ICON)
00141             self._widget.btn_mixed.setChecked(False)
00142             self._widget.btn_velocity.setIcon(self.CONTROLLER_OFF_ICON)
00143             self._widget.btn_velocity.setChecked(False)
00144             rospy.loginfo("Effort checked: " + str(self._widget.btn_effort.isChecked()))
00145             self.change_ctrl( "effort" )
00146         else:
00147             self._widget.btn_effort.setIcon(self.CONTROLLER_OFF_ICON)
00148             self._widget.btn_effort.setChecked(False)
00149             rospy.loginfo("Effort checked: " + str(self._widget.btn_effort.isChecked()))
00150             self.change_ctrl( "stop" )
00151         self._widget.btn_effort.setEnabled(True)
00152 
00153     def on_position_ctrl_clicked_(self):
00154         """
00155         Position controller selected
00156         """
00157         self._widget.btn_position.setEnabled(False)
00158         if not self._widget.btn_position.isChecked():
00159             self._widget.btn_position.setIcon(self.CONTROLLER_ON_ICON)
00160             self._widget.btn_position.setChecked(True)
00161             self._widget.btn_effort.setIcon(self.CONTROLLER_OFF_ICON)
00162             self._widget.btn_effort.setChecked(False)
00163             self._widget.btn_mixed.setIcon(self.CONTROLLER_OFF_ICON)
00164             self._widget.btn_mixed.setChecked(False)
00165             self._widget.btn_velocity.setIcon(self.CONTROLLER_OFF_ICON)
00166             self._widget.btn_velocity.setChecked(False)
00167             rospy.loginfo("Position checked: " + str(self._widget.btn_position.isChecked()))
00168             self.change_ctrl( "position" )
00169         else:
00170             self._widget.btn_position.setIcon(self.CONTROLLER_OFF_ICON)
00171             self._widget.btn_position.setChecked(False)
00172             rospy.loginfo("Position checked: " + str(self._widget.btn_position.isChecked()))
00173             self.change_ctrl( "stop" )
00174         self._widget.btn_position.setEnabled(True)
00175 
00176     def on_mixed_ctrl_clicked_(self):
00177         """
00178         Mixed controller selected
00179         """
00180         self._widget.btn_mixed.setEnabled(False)
00181         if not self._widget.btn_mixed.isChecked():
00182             self._widget.btn_mixed.setIcon(self.CONTROLLER_ON_ICON)
00183             self._widget.btn_mixed.setChecked(True)
00184             self._widget.btn_effort.setIcon(self.CONTROLLER_OFF_ICON)
00185             self._widget.btn_effort.setChecked(False)
00186             self._widget.btn_position.setIcon(self.CONTROLLER_OFF_ICON)
00187             self._widget.btn_position.setChecked(False)
00188             self._widget.btn_velocity.setIcon(self.CONTROLLER_OFF_ICON)
00189             self._widget.btn_velocity.setChecked(False)
00190             rospy.loginfo("Mixed checked: " + str(self._widget.btn_mixed.isChecked()))
00191             self.change_ctrl( "mixed" )
00192         else:
00193             self._widget.btn_mixed.setIcon(self.CONTROLLER_OFF_ICON)
00194             self._widget.btn_mixed.setChecked(False)
00195             rospy.loginfo("Mixed checked: " + str(self._widget.btn_mixed.isChecked()))
00196             self.change_ctrl( "stop" )
00197         self._widget.btn_mixed.setEnabled(True)
00198             
00199             
00200 
00201     def on_velocity_ctrl_clicked_(self):
00202         """
00203         Velocity controller selected
00204         """
00205         self._widget.btn_velocity.setEnabled(False)
00206         if not self._widget.btn_velocity.isChecked():
00207             self._widget.btn_velocity.setIcon(self.CONTROLLER_ON_ICON)
00208             self._widget.btn_velocity.setChecked(True)
00209             self._widget.btn_effort.setIcon(self.CONTROLLER_OFF_ICON)
00210             self._widget.btn_effort.setChecked(False)
00211             self._widget.btn_position.setIcon(self.CONTROLLER_OFF_ICON)
00212             self._widget.btn_position.setChecked(False)
00213             self._widget.btn_mixed.setIcon(self.CONTROLLER_OFF_ICON)
00214             self._widget.btn_mixed.setChecked(False)
00215             rospy.loginfo("Velocity checked: " + str(self._widget.btn_velocity.isChecked()))
00216             self.change_ctrl( "velocity" )            
00217         else:
00218             self._widget.btn_velocity.setIcon(self.CONTROLLER_OFF_ICON)
00219             self._widget.btn_velocity.setChecked(False)
00220             rospy.loginfo("Velocity checked: " + str(self._widget.btn_velocity.isChecked()))
00221             self.change_ctrl( "stop" )
00222         self._widget.btn_velocity.setEnabled(True)
00223             
00224 
00225 
00226     def change_ctrl(self, controller):
00227         """
00228         Switch the current controller
00229         """
00230         success = True
00231         list_controllers = rospy.ServiceProxy('pr2_controller_manager/list_controllers', ListControllers)
00232         try:
00233             resp1 = list_controllers()
00234         except rospy.ServiceException:
00235             success = False
00236 
00237         if success:
00238             current_controllers = []
00239             all_loaded_controllers = resp1.controllers
00240             for state,tmp_contrl in zip(resp1.state,resp1.controllers):
00241                 if state == "running":
00242                     current_controllers.append(tmp_contrl)
00243 
00244             controllers_to_start = self.controllers[controller]
00245 
00246             load_controllers = rospy.ServiceProxy('pr2_controller_manager/load_controller', LoadController)
00247             for load_control in controllers_to_start:
00248                 if load_control not in all_loaded_controllers:
00249                     try:
00250                         resp1 = load_controllers(load_control)
00251                     except rospy.ServiceException:
00252                         success = False
00253                     if not resp1.ok:
00254                         success = False
00255 
00256             switch_controllers = rospy.ServiceProxy('pr2_controller_manager/switch_controller', SwitchController)
00257             try:
00258                 resp1 = switch_controllers(controllers_to_start, current_controllers, SwitchController._request_class.BEST_EFFORT)
00259             except rospy.ServiceException:
00260                 success = False
00261 
00262             if not resp1.ok:
00263                 success = False
00264 
00265         if not success:
00266             rospy.logwarn("Failed to change some of the controllers. This is normal if this is not a 5 finger hand.")
00267 
00268     def change_force_ctrl_type(self, chng_type_msg):
00269         """
00270         Calls the service (realtime_loop/change_control_type) that allows to tell the driver (sr_robot_lib) which type of force control has to be sent to the motor:
00271             - torque demand (sr_robot_msgs::ControlType::FORCE)
00272             - PWM (sr_robot_msgs::ControlType::PWM)
00273         it will deactivate the Effort, Position, Mixed and Velocity buttons for 3 secs to allow hardware controllers to be updated 
00274         """
00275         success = True
00276         change_control_type = rospy.ServiceProxy('realtime_loop/change_control_type', ChangeControlType)
00277         try:
00278             resp1 = change_control_type(chng_type_msg)
00279             if resp1.result.control_type != chng_type_msg.control_type:
00280                 success = False
00281         except rospy.ServiceException:
00282             success = False
00283 
00284         # Disable buttons for 3 secs until motors change their parameters
00285         self._widget.btn_effort.setEnabled(False)
00286         self._widget.btn_position.setEnabled(False)
00287         self._widget.btn_mixed.setEnabled(False)
00288         self._widget.btn_velocity.setEnabled(False)
00289         sleep(3)
00290         self._widget.btn_effort.setEnabled(True)
00291         self._widget.btn_position.setEnabled(True)
00292         self._widget.btn_mixed.setEnabled(True)
00293         self._widget.btn_velocity.setEnabled(True)
00294 
00295         if not success:
00296             QMessageBox.warning(self._widget, "Warning", "Failed to change the control type.")
00297 
00298     def _unregisterPublisher(self):
00299         if self._publisher is not None:
00300             self._publisher.unregister()
00301             self._publisher = None
00302 
00303     def shutdown_plugin(self):
00304         self._unregisterPublisher()
00305 
00306     def save_settings(self, global_settings, perspective_settings):
00307         pass
00308 
00309     def restore_settings(self, global_settings, perspective_settings):
00310         pass