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
00033 import roslib
00034 roslib.load_manifest('sr_gui_change_controllers')
00035 import rospy
00036 
00037 from qt_gui.plugin import Plugin
00038 from python_qt_binding import loadUi
00039 
00040 from QtCore import QEvent, QObject, Qt, QTimer, Slot
00041 from QtGui import QShortcut, QMessageBox, QWidget, QIcon
00042 from pr2_mechanism_msgs.srv import ListControllers, SwitchController, LoadController
00043 from sr_robot_msgs.srv import ChangeControlType
00044 from sr_robot_msgs.msg import ControlType
00045 
00046 class SrGuiChangeControllers(Plugin):
00047     
00048     CONTROLLER_ON_ICON_PATH = '../../images/icons/green.png'
00049     CONTROLLER_OFF_ICON_PATH = '../../images/icons/red.png'
00050     CONTROLLER_ON_ICON = QIcon(os.path.join(os.path.dirname(os.path.realpath(__file__)), CONTROLLER_ON_ICON_PATH))
00051     CONTROLLER_OFF_ICON = QIcon(os.path.join(os.path.dirname(os.path.realpath(__file__)), CONTROLLER_OFF_ICON_PATH))
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(os.path.dirname(os.path.realpath(__file__)), '../../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         #We only react to the currently ON radio button event
00101         if checked:
00102             change_type_msg = ChangeControlType()
00103             if self._widget.radioButtonTorque.isChecked():
00104                 change_type_msg.control_type = ControlType.FORCE
00105                 rospy.loginfo("Change Control mode to FORCE")
00106             else:
00107                 change_type_msg.control_type = ControlType.PWM
00108                 rospy.loginfo("Change Control mode to PWM")
00109             self.change_force_ctrl_type(change_type_msg)
00110         
00111     def on_stop_ctrl_clicked_(self):
00112         self._widget.btn_stop.setEnabled(False)
00113         self._widget.btn_mixed.setIcon(self.CONTROLLER_OFF_ICON)
00114         self._widget.btn_mixed.setChecked(False)
00115         self._widget.btn_effort.setIcon(self.CONTROLLER_OFF_ICON)
00116         self._widget.btn_effort.setChecked(False)
00117         self._widget.btn_position.setIcon(self.CONTROLLER_OFF_ICON)
00118         self._widget.btn_position.setChecked(False)
00119         self._widget.btn_velocity.setIcon(self.CONTROLLER_OFF_ICON)
00120         self._widget.btn_velocity.setChecked(False)
00121         self.change_ctrl( "stop" )
00122         self._widget.btn_stop.setEnabled(True)
00123 
00124     def on_effort_ctrl_clicked_(self):
00125         self._widget.btn_effort.setEnabled(False)
00126         if not self._widget.btn_effort.isChecked():
00127             self._widget.btn_effort.setIcon(self.CONTROLLER_ON_ICON)
00128             self._widget.btn_effort.setChecked(True)
00129             self._widget.btn_position.setIcon(self.CONTROLLER_OFF_ICON)
00130             self._widget.btn_position.setChecked(False)
00131             self._widget.btn_mixed.setIcon(self.CONTROLLER_OFF_ICON)
00132             self._widget.btn_mixed.setChecked(False)
00133             self._widget.btn_velocity.setIcon(self.CONTROLLER_OFF_ICON)
00134             self._widget.btn_velocity.setChecked(False)
00135             rospy.loginfo("Effort checked: " + str(self._widget.btn_effort.isChecked()))
00136             self.change_ctrl( "effort" )
00137         else:
00138             self._widget.btn_effort.setIcon(self.CONTROLLER_OFF_ICON)
00139             self._widget.btn_effort.setChecked(False)
00140             rospy.loginfo("Effort checked: " + str(self._widget.btn_effort.isChecked()))
00141             self.change_ctrl( "stop" )
00142         self._widget.btn_effort.setEnabled(True)
00143 
00144     def on_position_ctrl_clicked_(self):
00145         self._widget.btn_position.setEnabled(False)
00146         if not self._widget.btn_position.isChecked():
00147             self._widget.btn_position.setIcon(self.CONTROLLER_ON_ICON)
00148             self._widget.btn_position.setChecked(True)
00149             self._widget.btn_effort.setIcon(self.CONTROLLER_OFF_ICON)
00150             self._widget.btn_effort.setChecked(False)
00151             self._widget.btn_mixed.setIcon(self.CONTROLLER_OFF_ICON)
00152             self._widget.btn_mixed.setChecked(False)
00153             self._widget.btn_velocity.setIcon(self.CONTROLLER_OFF_ICON)
00154             self._widget.btn_velocity.setChecked(False)
00155             rospy.loginfo("Position checked: " + str(self._widget.btn_position.isChecked()))
00156             self.change_ctrl( "position" )
00157         else:
00158             self._widget.btn_position.setIcon(self.CONTROLLER_OFF_ICON)
00159             self._widget.btn_position.setChecked(False)
00160             rospy.loginfo("Position checked: " + str(self._widget.btn_position.isChecked()))
00161             self.change_ctrl( "stop" )
00162         self._widget.btn_position.setEnabled(True)
00163 
00164     def on_mixed_ctrl_clicked_(self):
00165         self._widget.btn_mixed.setEnabled(False)
00166         if not self._widget.btn_mixed.isChecked():
00167             self._widget.btn_mixed.setIcon(self.CONTROLLER_ON_ICON)
00168             self._widget.btn_mixed.setChecked(True)
00169             self._widget.btn_effort.setIcon(self.CONTROLLER_OFF_ICON)
00170             self._widget.btn_effort.setChecked(False)
00171             self._widget.btn_position.setIcon(self.CONTROLLER_OFF_ICON)
00172             self._widget.btn_position.setChecked(False)
00173             self._widget.btn_velocity.setIcon(self.CONTROLLER_OFF_ICON)
00174             self._widget.btn_velocity.setChecked(False)
00175             rospy.loginfo("Mixed checked: " + str(self._widget.btn_mixed.isChecked()))
00176             self.change_ctrl( "mixed" )
00177         else:
00178             self._widget.btn_mixed.setIcon(self.CONTROLLER_OFF_ICON)
00179             self._widget.btn_mixed.setChecked(False)
00180             rospy.loginfo("Mixed checked: " + str(self._widget.btn_mixed.isChecked()))
00181             self.change_ctrl( "stop" )
00182         self._widget.btn_mixed.setEnabled(True)
00183             
00184             
00185 
00186     def on_velocity_ctrl_clicked_(self):
00187         self._widget.btn_velocity.setEnabled(False)
00188         if not self._widget.btn_velocity.isChecked():
00189             self._widget.btn_velocity.setIcon(self.CONTROLLER_ON_ICON)
00190             self._widget.btn_velocity.setChecked(True)
00191             self._widget.btn_effort.setIcon(self.CONTROLLER_OFF_ICON)
00192             self._widget.btn_effort.setChecked(False)
00193             self._widget.btn_position.setIcon(self.CONTROLLER_OFF_ICON)
00194             self._widget.btn_position.setChecked(False)
00195             self._widget.btn_mixed.setIcon(self.CONTROLLER_OFF_ICON)
00196             self._widget.btn_mixed.setChecked(False)
00197             rospy.loginfo("Velocity checked: " + str(self._widget.btn_velocity.isChecked()))
00198             self.change_ctrl( "velocity" )            
00199         else:
00200             self._widget.btn_velocity.setIcon(self.CONTROLLER_OFF_ICON)
00201             self._widget.btn_velocity.setChecked(False)
00202             rospy.loginfo("Velocity checked: " + str(self._widget.btn_velocity.isChecked()))
00203             self.change_ctrl( "stop" )
00204         self._widget.btn_velocity.setEnabled(True)
00205             
00206 
00207 
00208     def change_ctrl(self, controller):
00209         success = True
00210         list_controllers = rospy.ServiceProxy('pr2_controller_manager/list_controllers', ListControllers)
00211         try:
00212             resp1 = list_controllers()
00213         except rospy.ServiceException:
00214             success = False
00215 
00216         if success:
00217             current_controllers = []
00218             all_loaded_controllers = resp1.controllers
00219             for state,tmp_contrl in zip(resp1.state,resp1.controllers):
00220                 if state == "running":
00221                     current_controllers.append(tmp_contrl)
00222 
00223             controllers_to_start = self.controllers[controller]
00224 
00225             load_controllers = rospy.ServiceProxy('/pr2_controller_manager/load_controller', LoadController)
00226             for load_control in controllers_to_start:
00227                 if load_control not in all_loaded_controllers:
00228                     try:
00229                         resp1 = load_controllers(load_control)
00230                     except rospy.ServiceException:
00231                         success = False
00232                     if not resp1.ok:
00233                         success = False
00234 
00235             switch_controllers = rospy.ServiceProxy('pr2_controller_manager/switch_controller', SwitchController)
00236             try:
00237                 resp1 = switch_controllers(controllers_to_start, current_controllers, SwitchController._request_class.BEST_EFFORT)
00238             except rospy.ServiceException:
00239                 success = False
00240 
00241             if not resp1.ok:
00242                 success = False
00243 
00244         if not success:
00245             rospy.logwarn("Failed to change some of the controllers. This could be normal if this is not a 5 finger hand.")
00246 
00247     def change_force_ctrl_type(self, chng_type_msg):
00248         '''
00249         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:
00250             - torque demand (sr_robot_msgs::ControlType::FORCE)
00251             - PWM (sr_robot_msgs::ControlType::PWM)
00252         '''
00253         success = True
00254         change_control_type = rospy.ServiceProxy('/realtime_loop/change_control_type', ChangeControlType)
00255         try:
00256             resp1 = change_control_type(chng_type_msg)
00257             if resp1.result.control_type != chng_type_msg.control_type:
00258                 success = False
00259         except rospy.ServiceException:
00260             success = False
00261 
00262         if not success:
00263             QMessageBox.warning(self._widget, "Warning", "Failed to change the control type.")
00264 
00265     def _unregisterPublisher(self):
00266         if self._publisher is not None:
00267             self._publisher.unregister()
00268             self._publisher = None
00269 
00270     def shutdown_plugin(self):
00271         self._unregisterPublisher()
00272 
00273     def save_settings(self, global_settings, perspective_settings):
00274         pass
00275 
00276     def restore_settings(self, global_settings, perspective_settings):
00277         pass
00278