rqt_bhand.py
Go to the documentation of this file.
00001 # Copyright (c) 2014, Robotnik Automation
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 from __future__ import division
00032 import os
00033 import rospkg
00034 import threading
00035 
00036 import rospy
00037 from python_qt_binding import loadUi
00038 from python_qt_binding.QtCore import Qt, QTimer, Slot, QBasicTimer, SIGNAL
00039 from python_qt_binding.QtGui import QKeySequence, QShortcut, QWidget, QPixmap, QMessageBox, QStandardItemModel,QStandardItem
00040 from rqt_gui_py.plugin import Plugin
00041 
00042 import time
00043 import math
00044 
00045 from bhand_controller.msg import State, TactileArray, Service
00046 from bhand_controller.srv import Actions, SetControlMode
00047 from sensor_msgs.msg import JointState
00048 from rospy.exceptions import ROSException
00049 
00050 #angles
00051 max_base_spread = 3.14
00052 max_finger_spread = 2.44
00053 
00054 MAX_VELOCITY = 0.1 # rad/s
00055 BHAND_VELOCITY_COMMANDS_FREQ = 50 # Frequency of vel publications in ms
00056 
00057 class BHandGUI(Plugin):
00058         
00059         def __init__(self, context):
00060                 super(BHandGUI, self).__init__(context)
00061                 self.setObjectName('BHandGUI')
00062                 
00063                 self.read_ros_params()
00064                 self._publisher = None
00065 
00066                 self._widget = QWidget()
00067                 
00068                 # variable to store the sensor data when receives it
00069                 self._bhand_data = State()
00070                 self._joint_data = JointState()
00071                 self._tact_data = TactileArray()
00072                         
00073                 rp = rospkg.RosPack()
00074                 
00075                 # Flag to enable sending commands
00076                 self.enable_commands = False
00077                 
00078                 
00079                 #Variable inits
00080                 # DESIRED POSITION
00081                 self.base_spread = 0.0
00082                 self.finger1_spread = 0.0
00083                 self.finger2_spread = 0.0
00084                 self.finger3_spread = 0.0
00085                 # DESIRED VELOCITY
00086                 self.base_spread_vel = 0.0
00087                 self.finger1_spread_vel = 0.0
00088                 self.finger2_spread_vel = 0.0
00089                 self.finger3_spread_vel = 0.0
00090                 self.max_vel = MAX_VELOCITY
00091                 self.vel_factor = self.max_vel/100.0 # For the slider
00092                 
00093                 self.red_string = "background-color: rgb(255,0,0)"
00094                 self.orange_string = "background-color: rgb(255,128,0)"
00095                 self.yellow_string = "background-color: rgb(255,255,0)"
00096                 self.green_string = "background-color: rgb(128,255,0)"
00097                 
00098                 self.palm_factor = max_base_spread/99
00099                 self.finger_factor = max_finger_spread/99
00100                 
00101                 self.state_string = " "
00102                 
00103                 
00104                 # UI
00105                 ui_file = os.path.join(rp.get_path('rqt_bhand'), 'resource', 'BHandGUI.ui')
00106                 loadUi(ui_file, self._widget)
00107                 self._widget.setObjectName('BHandGUI')
00108                 
00109                 pixmap_red_file = os.path.join(rp.get_path('rqt_bhand'), 'resource', 'red.png')
00110                 pixmap_green_file = os.path.join(rp.get_path('rqt_bhand'), 'resource', 'green.png')
00111                 self._pixmap_red = QPixmap(pixmap_red_file)
00112                 self._pixmap_green = QPixmap(pixmap_green_file)
00113                 self._widget.qlabel_state_connection.setPixmap(self._pixmap_red) # Shows agvs_controller comm state
00114                 self._widget.qlabel_jointstate_connection.setPixmap(self._pixmap_red) # Shows agvs_controller comm state
00115                 
00116                                 
00117                 if context.serial_number() > 1:
00118                         self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))           
00119                 
00120                 # Adds this widget to the context
00121                 context.add_widget(self._widget)
00122 
00123                 # Try to connect to the topic
00124                 self._topic = '/%s/state'%self.bhand_node_name 
00125                 self._joint_states_topic = '/joint_states'
00126                 self._tact_topic = '/%s/tact_array'%self.bhand_node_name 
00127                 self._command_topic = '/%s/command'%self.bhand_node_name 
00128                 self._set_mode_service_name = '/%s/set_control_mode'%self.bhand_node_name
00129                 self._actions_service_name = '/%s/actions'%self.bhand_node_name
00130                  
00131                 # Saves the desired value
00132                 self.desired_ref = JointState()
00133                 
00134                 #self.joint_names = ['j11_joint', 'j32_joint', 'j12_joint', 'j22_joint', 'j23_joint', 'j13_joint', 'j33_joint']
00135                 #self.joint_names =  ['bh_j12_joint', 'bh_j13_joint', 'bh_j22_joint', 'bh_j23_joint', 'bh_j32_joint', 'bh_j31_joint', 'bh_j11_joint', 'bh_j21_joint']
00136                 
00137                 #self.joint_ids = ['F1', 'F1_TIP', 'F2', 'F2_TIP', 'F3', 'F3_TIP', 'SPREAD_1', 'SPREAD_2']
00138                 # Intermediate structure to save the read positions, vels and efforts
00139                 # Ej.: {'j1': [ 0, 0, 0 ]}
00140                 self.joint_state_pointer = {} 
00141                 for i in range(len(self.joint_ids)):
00142                         self.joint_state_pointer[self.joint_ids[i]] = {'joint': self.joint_names[i], 'values': [0, 0, 0]}
00143                         # Initializes joints data for receiving the read values
00144                         self._joint_data.name.append(self.joint_names[i])
00145                         self._joint_data.position.append(0.0)
00146                         self._joint_data.velocity.append(0.0)
00147                         self._joint_data.effort.append(0.0)
00148                 
00149                 # SUBSCRIPTIONS
00150                 try:
00151                         self._subscriber = rospy.Subscriber(self._topic, State, self._receive_state_data)
00152                 except ValueError, e:
00153                         rospy.logerr('BHandGUI: Error connecting topic (%s)'%e)
00154                         
00155                 try:
00156                         self._joint_subscriber = rospy.Subscriber(self._joint_states_topic, JointState, self._receive_joints_data)
00157                 except ValueError, e:
00158                         rospy.logerr('BHandGUI: Error connecting topic (%s)'%e)
00159                         
00160                 try:
00161                         self._tact_subscriber = rospy.Subscriber(self._tact_topic, TactileArray, self._receive_tact_data)
00162                 except ValueError, e:
00163                         rospy.logerr('BHandGUI: Error connecting topic (%s)'%e)
00164                 
00165                 # PUBLICATIONS
00166                 try:
00167                         self._publisher_command = rospy.Publisher(self._command_topic, JointState)
00168                 except ROSException, e:
00169                         rospy.logerr('BHandGUI: Error creating publisher for topic %s (%s)'%(self._command_topic, e))
00170                 
00171                 # SERVICES
00172                 try:
00173                         self._service_bhand_actions = rospy.ServiceProxy(self._actions_service_name, Actions)
00174                 except ValueError, e:
00175                         rospy.logerr('BHandGUI: Error connecting service (%s)'%e)
00176                         
00177                 try:
00178                         self._service_set_mode = rospy.ServiceProxy(self._set_mode_service_name, SetControlMode)
00179                 except ValueError, e:
00180                         rospy.logerr('BHandGUI: Error connecting service (%s)'%e)
00181                 
00182                 
00183                         
00184                 self.fixed_fingers = 0
00185                 
00186                 # HANDLERS
00187                 # Adds handlers to 'press button' event
00188                 self._widget.pushButtonStart.clicked.connect(self.start_button_pressed)
00189                 self._widget.pushButton_stop.clicked.connect(self.stop)
00190                 self._widget.pushButton_close.clicked.connect(self.close_button_pressed)
00191                 self._widget.pushButton_open.clicked.connect(self.open_button_pressed)
00192                 self._widget.pushButton_mode1.clicked.connect(self.mode1_button_pressed)
00193                 self._widget.pushButton_mode2.clicked.connect(self.mode2_button_pressed)
00194                 self._widget.horizontalSlider.valueChanged.connect(self.slider_1_changed)
00195                 self._widget.horizontalSlider_2.valueChanged.connect(self.slider_2_changed)
00196                 self._widget.horizontalSlider_3.valueChanged.connect(self.slider_3_changed)
00197                 self._widget.horizontalSlider_4.valueChanged.connect(self.slider_4_changed)
00198                 self._widget.horizontalSlider_v_spread.valueChanged.connect(self.slider_v_spread_changed)
00199                 self._widget.horizontalSlider_v_f1.valueChanged.connect(self.slider_v_f1_changed)
00200                 self._widget.horizontalSlider_v_f2.valueChanged.connect(self.slider_v_f2_changed)
00201                 self._widget.horizontalSlider_v_f3.valueChanged.connect(self.slider_v_f3_changed)
00202                 
00203                 self.connect(self._widget.checkBox_enable_control_vel, SIGNAL("stateChanged(int)"),  self.enable_command_vel_changed)
00204                 self.connect(self._widget.checkBox_enable_control_pos, SIGNAL("stateChanged(int)"),  self.enable_command_pos_changed)
00205                 self._widget.checkBox_enable_control_vel.setChecked(False)
00206                 
00207                 #self._widget.radioButtonPosition.clicked.connect(self.radio_button_position_clicked)
00208                 #self._widget.radioButtonVelocity.clicked.connect(self.radio_button_velocity_clicked)
00209                 self.connect(self._widget.radioButtonPosition, SIGNAL("clicked(bool)"),  self.radio_button_position_clicked)
00210                 self.connect(self._widget.radioButtonVelocity, SIGNAL("clicked(bool)"),  self.radio_button_velocity_clicked)
00211                 
00212                 
00213                 self._widget.checkBox.stateChanged.connect(self.fingers_check_box_pressed)
00214                 
00215                 
00216                 self._init_timers()
00217         
00218         
00219         def read_ros_params(self):
00220                 '''
00221                         Read ROS params from server
00222                 '''
00223                 _name = rospy.get_name()
00224                 
00225                 self.bhand_node_name = rospy.get_param('%s/bhand_node_name'%_name, 'bhand_node')
00226                 # Reads the configuration of the joints ids
00227                 self.joint_ids  = rospy.get_param('%s/joint_ids'%(self.bhand_node_name), ['F1', 'F1_TIP', 'F2', 'F2_TIP', 'F3', 'F3_TIP', 'SPREAD_1', 'SPREAD_2'])
00228                 self.joint_names  = rospy.get_param('%s/joint_names'%(self.bhand_node_name), ['bh_j12_joint', 'bh_j13_joint', 'bh_j22_joint', 'bh_j23_joint', 'bh_j32_joint', 'bh_j31_joint', 'bh_j11_joint', 'bh_j21_joint'])
00229                 
00230                 rospy.loginfo('%s::read_ros_params: bhand_node_name = %s'%(_name, self.bhand_node_name))
00231                 rospy.loginfo('%s::read_ros_params: joint_ids = %s'%(_name, self.joint_ids))
00232                 rospy.loginfo('%s::read_ros_params: joint_names = %s'%(_name, self.joint_names))
00233                 
00234                 
00235                 
00236         # inits the timers used to control the connection, ...
00237         def _init_timers(self):
00238                 self._topic_connected = False
00239                 self._topic_joint_states_connected = False
00240                 
00241                 self._topic_timer = time.time()
00242                 self._topic_joint_states_timer = time.time()
00243                 
00244                 
00245                 
00246                 
00247                 self._topic_timeout_connection = 5 # seconds
00248                 # Creates a basic timer and starts it
00249                 self._timer = QBasicTimer()
00250                 self._timer.start(1, self)
00251                 # Creates a timer to send command refs periodically
00252                 self._timer_commands = QTimer()
00253                 self.connect(self._timer_commands, SIGNAL("timeout()"),  self.timeout_command_timer)
00254                 
00255         def start_button_pressed(self):
00256                 '''
00257                         Handles the press button event to call initialization service
00258                 '''
00259                 self.send_bhand_action(Service.INIT_HAND)
00260                 '''
00261                         rospy.wait_for_service(self._actions_service_name)
00262                         try:
00263                                 action = rospy.ServiceProxy(self._actions_service_name, Actions)
00264                                 resp1 = action(Service.INIT_HAND)
00265                                 return resp1
00266                         except rospy.ServiceException, e:
00267                                 rospy.logerr("Service call failed: %s"%e)
00268                 '''
00269         
00270         def close_button_pressed(self):
00271                 '''
00272                         Handles the press button event to call close hand service
00273                 '''
00274                 #self._widget.checkBox_enable_control_pos.setChecked(False)
00275                 self._widget.radioButtonPosition.setChecked(True)
00276                 self.send_bhand_action(Service.CLOSE_GRASP)
00277         
00278         
00279         def open_button_pressed(self):
00280                 '''
00281                         Handles the press button event to call open hand service
00282                 '''
00283                 self.send_bhand_action(Service.OPEN_GRASP)
00284         
00285         
00286         def mode1_button_pressed(self):
00287                 '''
00288                         Handles the press button event to call set mode 1  service
00289                 '''
00290                 self.send_bhand_action(Service.SET_GRASP_1)
00291         
00292         
00293         def mode2_button_pressed(self):
00294                 '''
00295                         Handles the press button event to call set mode 2 service
00296                 '''
00297                 self.send_bhand_action(Service.SET_GRASP_2)
00298                 
00299                 
00300         
00301         def slider_1_changed(self):
00302                 self.base_spread = self._widget.horizontalSlider.value() * self.palm_factor
00303                 if self.enable_commands and self._bhand_data.control_mode == 'POSITION':
00304                         self.send_position_command()
00305                 
00306                 
00307                 
00308         def slider_2_changed(self):
00309                 self.finger3_spread = self._widget.horizontalSlider_2.value() * self.finger_factor
00310                 if self.fixed_fingers == 1:
00311                         self.finger1_spread = self.finger2_spread = self.finger3_spread
00312                         self._widget.horizontalSlider_3.setSliderPosition(self._widget.horizontalSlider_2.value())
00313                         self._widget.horizontalSlider_4.setSliderPosition(self._widget.horizontalSlider_2.value())
00314                 if self.enable_commands and self._bhand_data.control_mode == 'POSITION':
00315                         self.send_position_command()
00316                 
00317         def slider_3_changed(self):
00318                 self.finger1_spread = self._widget.horizontalSlider_3.value() * self.finger_factor
00319                 if self.fixed_fingers == 1:
00320                         self.finger3_spread = self.finger2_spread = self.finger1_spread
00321                         self._widget.horizontalSlider_2.setSliderPosition(self._widget.horizontalSlider_3.value())
00322                         self._widget.horizontalSlider_4.setSliderPosition(self._widget.horizontalSlider_3.value())
00323                 if self.enable_commands and self._bhand_data.control_mode == 'POSITION':
00324                         self.send_position_command()
00325                 
00326         def slider_4_changed(self):
00327                 self.finger2_spread = self._widget.horizontalSlider_4.value() * self.finger_factor
00328                 if self.fixed_fingers == 1:
00329                         self.finger1_spread = self.finger2_spread = self.finger2_spread
00330                         self._widget.horizontalSlider_2.setSliderPosition(self._widget.horizontalSlider_4.value())
00331                         self._widget.horizontalSlider_3.setSliderPosition(self._widget.horizontalSlider_4.value())
00332                 if self.enable_commands and self._bhand_data.control_mode == 'POSITION':
00333                         self.send_position_command()
00334         
00335         def slider_v_spread_changed(self):
00336                 '''
00337                         Handler for slider v_spread
00338                 '''
00339                 self.base_spread_vel = self._widget.horizontalSlider_v_spread.value() * self.vel_factor
00340                 
00341         
00342         def slider_v_f1_changed(self):
00343                 '''
00344                         Handler for slider v_f1
00345                 '''
00346                 self.finger1_spread_vel = self._widget.horizontalSlider_v_f1.value() * self.vel_factor
00347                 if self._widget.checkBox_moveall_velocity.isChecked():
00348                         self._widget.horizontalSlider_v_f2.setSliderPosition(self._widget.horizontalSlider_v_f1.value())
00349                         self._widget.horizontalSlider_v_f3.setSliderPosition(self._widget.horizontalSlider_v_f1.value())
00350         
00351         
00352         def slider_v_f2_changed(self):
00353                 '''
00354                         Handler for slider v_f2
00355                 '''
00356                 self.finger2_spread_vel = self._widget.horizontalSlider_v_f2.value() * self.vel_factor
00357                 if self._widget.checkBox_moveall_velocity.isChecked():
00358                         self._widget.horizontalSlider_v_f1.setSliderPosition(self._widget.horizontalSlider_v_f2.value())
00359                         self._widget.horizontalSlider_v_f3.setSliderPosition(self._widget.horizontalSlider_v_f2.value())
00360                 
00361         
00362         def slider_v_f3_changed(self):
00363                 '''
00364                         Handler for slider v_f3
00365                 '''
00366                 self.finger3_spread_vel = self._widget.horizontalSlider_v_f3.value() * self.vel_factor
00367                 if self._widget.checkBox_moveall_velocity.isChecked():
00368                         self._widget.horizontalSlider_v_f2.setSliderPosition(self._widget.horizontalSlider_v_f3.value())
00369                         self._widget.horizontalSlider_v_f1.setSliderPosition(self._widget.horizontalSlider_v_f3.value())
00370         
00371 
00372                 
00373         def send_position_command(self):
00374                 '''
00375                         Sends a command in position mode
00376                 '''
00377                 
00378                 self.desired_ref.name = [self.joint_state_pointer['SPREAD_1']['joint'], self.joint_state_pointer['F3']['joint'], self.joint_state_pointer['F1']['joint'], self.joint_state_pointer['F2']['joint']]
00379                 self.desired_ref.position = [self.base_spread, self.finger3_spread, self.finger1_spread, self.finger2_spread]
00380                 self.desired_ref.velocity = [0.1, 0.1, 0.1, 0.1]
00381                 self.desired_ref.effort = [0.0, 0.0, 0.0, 0.0]
00382                 self._publisher_command.publish(self.desired_ref)
00383                 
00384                 
00385         def     fingers_check_box_pressed(self, state):
00386                 '''
00387                         Handler call when clicking checkbox to control all fingers
00388                 '''
00389                 if state == 0:
00390                         self.fixed_fingers = 0
00391                 elif state == 2:
00392                         self.fixed_fingers = 1
00393                 
00394                         
00395                 
00396         @Slot(str)
00397         # Handles the change of topic's name
00398         def _on_topic_changed(self):
00399                 # reads the topic's name from the 'QLineEdit' 'topic_line_edit' (in the .ui file)
00400                 #self._topic = self._widget.topic_line_edit.text()
00401                 self._subscriber.unregister()
00402                 try:
00403                         self._subscriber = rospy.Subscriber(self._topic, State, self._receive_state_data)
00404                 except ValueError, e:
00405                         rospy.logerr('BHandGUI: Error connecting topic (%s)'%e)
00406                 
00407         # Handles the messages from the agvs controller
00408         def _receive_state_data(self, msg):
00409                 self._bhand_data = msg
00410                 self._topic_timer = time.time()
00411                 
00412                 if not self._topic_connected:
00413                         rospy.loginfo('BHandGUI: connection stablished with %s'%self._topic)
00414                         self._topic_connected = True
00415         
00416         
00417         def _receive_joints_data(self, msg):
00418                 '''
00419                         Handler for Joint States
00420                 '''
00421                 self._joints_data = msg
00422                 
00423                 self._topic_joint_states_timer = time.time()
00424                 
00425                 for i in range(len(msg.name)):
00426                         for j in self.joint_state_pointer:
00427                                 if self.joint_state_pointer[j]['joint'] == msg.name[i]:
00428                                         self.joint_state_pointer[j]['values'] = [msg.position[i], msg.velocity[i], msg.effort[i]]
00429                         
00430                 
00431                 if not self._topic_joint_states_connected:
00432                         rospy.loginfo('Bhand: connection stablished with %s'%self._joint_states_topic)
00433                         self._topic_joint_states_connected = True
00434                 
00435                 
00436                                 
00437                 
00438         def _receive_tact_data(self, msg):
00439                 self._tact_data = msg
00440         
00441                         
00442                         
00443                                 
00444         # Handles the messages from the dspic controller
00445         def _receive_dspic_data(self, msg):
00446                 self._dspic_data = msg
00447                 self._topic_dspic_timer = time.time()
00448                 
00449                 if not self._topic_dspic_connected:
00450                         rospy.loginfo('BHandGUI: connection stablished with %s'%self._topic_dspic)
00451                         self._topic_dspic_connected = True
00452         
00453                         
00454         # Handles the press event of the button dspic reset odom
00455         def press_reset_dspic_odom(self):       
00456                 
00457                 ret = QMessageBox.question(self._widget, "Dspic Reset Odometry", 'Are you sure of resetting the odometry?', QMessageBox.Ok, QMessageBox.Cancel)
00458                 
00459                 if ret == QMessageBox.Ok:
00460                         
00461                         # Call the service recalibrate dspic
00462                         try:
00463                                 ret = self._service_dspic_reset_odom(0.0, 0.0, 0.0, 0.0)                                
00464                         except ValueError, e:
00465                                 rospy.logerr('BHandGUI::press_reset_dspic_odom: (%s)'%e)
00466                         except rospy.ServiceException, e:
00467                                 rospy.logerr('BHandGUI::press_reset_dspic_odom: (%s)'%e)
00468                                 QMessageBox.warning(self._widget, "Warning", "Servicio no disponible")
00469         
00470         
00471         def enable_command_vel_changed(self, value):
00472                 '''
00473                         Handles command enabled event from checkbox
00474                 '''
00475                  
00476                 if value != 0:
00477                         self._widget.checkBox_enable_control_pos.setChecked(True)
00478                         self.enable_commands = True
00479                         #if self._bhand_data.control_mode == 'VELOCITY':
00480                         #       self._timer_commands.start(BHAND_VELOCITY_COMMANDS_FREQ)
00481                                 
00482                                  
00483                 else:
00484                         self._widget.checkBox_enable_control_pos.setChecked(False)
00485                         self.enable_commands = False
00486                         #if self._bhand_data.control_mode == 'VELOCITY':
00487                         #       self._timer_commands.stop()
00488                         
00489                         
00490         
00491         def enable_command_pos_changed(self, value):
00492                 '''
00493                         Handles command enabled event from checkbox
00494                 '''
00495                 
00496                 if value != 0:
00497                         self._widget.checkBox_enable_control_vel.setChecked(True)
00498                         self.enable_commands = True
00499                 else:
00500                         self._widget.checkBox_enable_control_vel.setChecked(False)
00501                         self.enable_commands = False
00502         
00503         
00504         def radio_button_position_clicked(self, value):
00505                 '''
00506                         Handles the click of this radio button
00507                 '''
00508                 self.set_control_mode('POSITION')
00509                 self._timer_commands.stop()
00510                 self.stop()
00511                 
00512         
00513         def radio_button_velocity_clicked(self, value):
00514                 '''
00515                         Handles the click of this radio button
00516                 '''
00517                 self.set_control_mode('VELOCITY')
00518                 self._timer_commands.start(BHAND_VELOCITY_COMMANDS_FREQ)
00519         
00520 
00521         def set_control_mode(self, mode):       
00522                 '''
00523                         Calls the service to set the control mode of the hand
00524                         @param mode: Desired Bhand mode of operation ('POSITION', 'VELOCITY')
00525                         @type mode: string
00526                 '''                     
00527                 try:
00528                         ret = self._service_set_mode(mode)                              
00529                 except ValueError, e:
00530                         rospy.logerr('BHandGUI::set_control_mode: (%s)'%e)
00531                 except rospy.ServiceException, e:
00532                         rospy.logerr('BHandGUI::set_control_mode: (%s)'%e)
00533                         QMessageBox.warning(self._widget, "Warning", "Servicio no disponible")
00534         
00535         
00536         def send_bhand_action(self, action):    
00537                 '''
00538                         Calls the service to set the control mode of the hand
00539                         @param action: Action number (defined in msgs/Service.msg)
00540                         @type action: int
00541                 '''                     
00542                 try:
00543                         ret = self._service_bhand_actions(action)                               
00544                 except ValueError, e:
00545                         rospy.logerr('BHandGUI::send_bhand_action: (%s)'%e)
00546                 except rospy.ServiceException, e:
00547                         rospy.logerr('BHandGUI::send_bhand_action: (%s)'%e)
00548                         QMessageBox.warning(self._widget, "Warning", "Servicio no disponible")
00549                                                 
00550         
00551         def send_velocity_command(self):
00552                 '''
00553                         Sends a command to the bhand
00554                 '''
00555                 self.desired_ref.name = [self.joint_state_pointer['SPREAD_1']['joint'], self.joint_state_pointer['F3']['joint'], self.joint_state_pointer['F1']['joint'], self.joint_state_pointer['F2']['joint']]
00556                 self.desired_ref.position = [0, 0, 0, 0]
00557                 self.desired_ref.velocity = [self.base_spread_vel, self.finger3_spread_vel, self.finger1_spread_vel, self.finger2_spread_vel]
00558                 self.desired_ref.effort = [0.0, 0.0, 0.0, 0.0]
00559                 self._publisher_command.publish(self.desired_ref)
00560                 
00561 
00562         def timeout_command_timer(self):
00563                 '''
00564                         Handles every timeout triggered by the Qtimer for sending commands      
00565                 '''
00566                 if self.enable_commands and self._bhand_data.control_mode == 'VELOCITY':
00567                         self.send_velocity_command()
00568         
00569         
00570         def stop(self):
00571                 '''
00572                         Stops the movement of the joints
00573                         It sets the velocity to 0
00574                 '''
00575                 self._widget.horizontalSlider_v_spread.setValue(0)
00576                 self._widget.horizontalSlider_v_f1.setValue(0)
00577                 self._widget.horizontalSlider_v_f2.setValue(0)
00578                 self._widget.horizontalSlider_v_f3.setValue(0)
00579         
00580         
00581         def shutdown_plugin(self):
00582                 '''
00583                         Shutdowns connections
00584                 '''
00585                 self._timer_commands.stop()
00586                 self._timer.stop()
00587                 self._subscriber.unregister()
00588                 self._joint_subscriber.unregister()
00589                 self._tact_subscriber.unregister()
00590                 self._publisher_command.unregister()
00591                 self._service_bhand_actions.close()
00592                 self._service_set_mode.close()
00593                 
00594         
00595         # Method executed periodically
00596         # Updates the graphical qt components
00597         def timerEvent(self, e):
00598                 
00599                 #state = self._bhand_data.state
00600                 
00601                 #Initialized?
00602                 if self._bhand_data.hand_initialized:
00603                         self._widget.checkBox_hand_initialized.setChecked(1)
00604                 else:
00605                         self._widget.checkBox_hand_initialized.setChecked(0)
00606                 
00607                 #State          
00608                 if self._bhand_data.state == 100:
00609                         self.state_string = "INIT_STATE"
00610                 elif self._bhand_data.state == 200:
00611                         self.state_string = "STANDBY_STATE"
00612                 elif self._bhand_data.state == 300:
00613                         self.state_string = "READY_STATE"
00614                 elif self._bhand_data.state == 400:
00615                         self.state_string = "EMERGENCY_STATE"
00616                 elif self._bhand_data.state == 500:
00617                         self.state_string = "FAILURE_STATE"
00618                 elif self._bhand_data.state == 600:
00619                         self.state_string = "SHUTDOWN_STATE"
00620                         
00621                 if self.state_string != " ":
00622                         self._widget.lineEdit_state.setText(self.state_string)
00623                 
00624                 self._widget.lineEdit_mode.setText(self._bhand_data.control_mode)
00625                         
00626                 #Frequencies
00627                 self._widget.lineEdit_motor_pos_hz.setText(str(self._bhand_data.desired_freq))
00628                 self._widget.lineEdit_motor_pos_hz_2.setText(str(round(self._bhand_data.real_freq,1)))
00629                 
00630                 #Temperatures
00631                 self._widget.lineEdit.setText(str(self._bhand_data.temp_spread[0]))
00632                 self._widget.lineEdit_3.setText(str(self._bhand_data.temp_spread[1]))
00633                 self._widget.lineEdit_2.setText(str(self._bhand_data.temp_f1[0]))
00634                 self._widget.lineEdit_4.setText(str(self._bhand_data.temp_f1[1]))
00635                 self._widget.lineEdit_5.setText(str(self._bhand_data.temp_f2[0]))
00636                 self._widget.lineEdit_6.setText(str(self._bhand_data.temp_f2[1]))
00637                 self._widget.lineEdit_7.setText(str(self._bhand_data.temp_f3[0]))
00638                 self._widget.lineEdit_8.setText(str(self._bhand_data.temp_f3[1]))
00639                 
00640                 # Desired Control Positions
00641                 self._widget.lineEdit_spread_des_pos.setText(str(round(self.base_spread,3)))
00642                 self._widget.lineEdit_f1_des_pos.setText(str(round(self.finger1_spread,3)))
00643                 self._widget.lineEdit_f2_des_pos.setText(str(round(self.finger2_spread,3)))
00644                 self._widget.lineEdit_f3_des_pos.setText(str(round(self.finger3_spread,3)))
00645                 # Read Control Positions
00646                 self._widget.lineEdit_spread_read_pos.setText(str(round(self.joint_state_pointer['SPREAD_1']['values'][0],3)))
00647                 self._widget.lineEdit_f1_read_pos.setText(str(round(self.joint_state_pointer['F1']['values'][0],3)))
00648                 self._widget.lineEdit_f2_read_pos.setText(str(round(self.joint_state_pointer['F2']['values'][0],3)))
00649                 self._widget.lineEdit_f3_read_pos.setText(str(round(self.joint_state_pointer['F3']['values'][0],3)))
00650                 self._widget.lineEdit_spread_read_pos_2.setText(str(round(self.joint_state_pointer['SPREAD_1']['values'][0],3)))
00651                 self._widget.lineEdit_f1_read_pos_2.setText(str(round(self.joint_state_pointer['F1']['values'][0],3)))
00652                 self._widget.lineEdit_f2_read_pos_2.setText(str(round(self.joint_state_pointer['F2']['values'][0],3)))
00653                 self._widget.lineEdit_f3_read_pos_2.setText(str(round(self.joint_state_pointer['F3']['values'][0],3)))
00654                 # Desired velocities
00655                 self._widget.lineEdit_spread_des_vel.setText(str(round(self.base_spread_vel,3)))
00656                 self._widget.lineEdit_f1_des_vel.setText(str(round(self.finger1_spread_vel,3)))
00657                 self._widget.lineEdit_f2_des_vel.setText(str(round(self.finger2_spread_vel,3)))
00658                 self._widget.lineEdit_f3_des_vel.setText(str(round(self.finger3_spread_vel,3)))
00659                 
00660                 #Effort
00661                 if self._bhand_data.state == 300:
00662                         self._widget.lineEdit_fingertip1.setText(str(round(self.joint_state_pointer['F1_TIP']['values'][2],4)))
00663                         self._widget.lineEdit_fingertip2.setText(str(round(self.joint_state_pointer['F2_TIP']['values'][2],4)))
00664                         self._widget.lineEdit_fingertip3.setText(str(round(self.joint_state_pointer['F3_TIP']['values'][2],4)))
00665                 
00666                 #Tactile sensors
00667                 
00668                 #self.red = 255
00669                 #self.green = 255
00670                 #self.blue = 0
00671                 #color_string = "background-color: rgb(" + str(self.red) + "," + str(self.green) + "," + str(self.blue) + ")"
00672                 
00673                 if self._bhand_data.state == 300:
00674                         
00675                         #Finger 1
00676                         for i in range(0,8):
00677                                 for j in range(0,3):
00678                                         lcd_string = "lcdNumber" + str(i*3 + j)
00679                                         value = round(self._tact_data.finger1[(7-i)*3 + j ], 1)
00680                                         getattr(self._widget,lcd_string).display(value)
00681                                         if 0.0 <= value and value < 4.0:
00682                                                 color_string = self.green_string
00683                                         elif 4.0 <= value and value < 8.0:
00684                                                 color_string = self.yellow_string
00685                                         elif 8.0 <= value and value < 12.0:
00686                                                 color_string = self.orange_string
00687                                         elif 12.0 <= value and value <= 16.0:
00688                                                 color_string = self.red_string
00689                                         getattr(self._widget,lcd_string).setStyleSheet(color_string)
00690                 
00691                         #Finger 2
00692                         for i in range(0,8):
00693                                 for j in range(0,3):
00694                                         lcd_string = "lcdNumber" + str(i*3 + j) + "_3"
00695                                         value = round(self._tact_data.finger2[(7-i)*3 + j ], 1)
00696                                         getattr(self._widget,lcd_string).display(value)
00697                                         if 0.0 <= value and value < 4.0:
00698                                                 color_string = self.green_string
00699                                         elif 4.0 <= value and value < 8.0:
00700                                                 color_string = self.yellow_string
00701                                         elif 8.0 <= value and value < 12.0:
00702                                                 color_string = self.orange_string
00703                                         elif 12.0 <= value and value <= 16.0:
00704                                                 color_string = self.red_string
00705                                         getattr(self._widget,lcd_string).setStyleSheet(color_string)
00706                                 
00707                         #Finger 3
00708                         for i in range(0,8):
00709                                 for j in range(0,3):
00710                                         lcd_string = "lcdNumber" + str(i*3 + j) + "_4"
00711                                         value = round(self._tact_data.finger3[(7-i)*3 + j], 1)
00712                                         getattr(self._widget,lcd_string).display(value)
00713                                         if 0.0 <= value and value < 4.0:
00714                                                 color_string = self.green_string
00715                                         elif 4.0 <= value and value < 8.0:
00716                                                 color_string = self.yellow_string
00717                                         elif 8.0 <= value and value < 12.0:
00718                                                 color_string = self.orange_string
00719                                         elif 12.0 <= value and value <= 16.0:
00720                                                 color_string = self.red_string
00721                                         getattr(self._widget,lcd_string).setStyleSheet(color_string)
00722                                 
00723                         #Palm
00724                         for i in range(0,24):
00725                                 lcd_string = "lcdNumber" + str(i) + "_6"
00726                                 value = round(self._tact_data.palm[i], 1)
00727                                 getattr(self._widget,lcd_string).display(value)
00728                                 if 0.0 <= value and value < 2.5:
00729                                         color_string = self.green_string
00730                                 elif 2.5 <= value and value < 5.0:
00731                                         color_string = self.yellow_string
00732                                 elif 5.0 <= value and value < 7.5:
00733                                         color_string = self.orange_string
00734                                 elif 7.5 <= value and value <= 16.0:
00735                                         color_string = self.red_string
00736                                 getattr(self._widget,lcd_string).setStyleSheet(color_string)
00737                 
00738                 
00739                 # Checks the ROS connection
00740                 t = time.time()
00741                 if self._topic_connected and (t - self._topic_timer >= self._topic_timeout_connection):
00742                         self._topic_connected = False
00743                         rospy.logerr('BHandGUI: error in communication with %s'%self._topic)
00744                         self._widget.qlabel_state_connection.setPixmap(self._pixmap_red)
00745                         
00746                 if self._topic_joint_states_connected and (t - self._topic_joint_states_timer >= self._topic_timeout_connection):
00747                         self._topic_joint_states_connected = False
00748                         rospy.logerr('BHandGUI: error in communication with %s'%self._joint_states_topic)
00749                         self._widget.qlabel_jointstate_connection.setPixmap(self._pixmap_red)
00750                 
00751                 if self._topic_connected:
00752                         self._widget.qlabel_state_connection.setPixmap(self._pixmap_green)
00753                         
00754                 if self._topic_joint_states_connected:
00755                         self._widget.qlabel_jointstate_connection.setPixmap(self._pixmap_green)


rqt_bhand
Author(s): Román Navarro , Jorge Ariño
autogenerated on Sun Oct 5 2014 22:24:42