Пример #1
0
class Joint(object):
    def __init__(self, name, sensor_bias, control_bias, gearing_bias, controller):
        self.name             = name
        self.sensor_bias      = sensor_bias
        self.control_bias     = control_bias
        self.controller       = controller
        self.gearing_bias     = gearing_bias

        print "Initial data for joint ",self.name, " biases=(",self.sensor_bias,", ",self.control_bias,")"
        self.max_sensor_bias  =  3.5
        self.min_sensor_bias  = -3.5
        self.range_sensor_bias = self.max_sensor_bias - self.min_sensor_bias
        self.max_control_bias =  0.3
        self.min_control_bias = -0.3
        self.max_gearing_bias =  1.3
        self.min_gearing_bias =  0.7
        self.range_gearing_bias = self.max_gearing_bias - self.min_gearing_bias

        self.range_control_bias = self.max_control_bias - self.min_control_bias
        self.sensor_bias_spinbox = QDoubleSpinBox()
        self.sensor_bias_spinbox.setDecimals(4);
        self.sensor_bias_spinbox.setRange(self.min_sensor_bias, self.max_sensor_bias)
        self.sensor_bias_spinbox.setValue(self.sensor_bias)
        self.sensor_bias_spinbox.setSingleStep(self.range_sensor_bias/500)
        self.sensor_bias_spinbox.valueChanged.connect(self.on_biasChanged)

        self.control_bias_spinbox = QDoubleSpinBox()
        self.control_bias_spinbox.setDecimals(4);
        self.control_bias_spinbox.setRange(self.min_control_bias, self.max_control_bias)
        self.control_bias_spinbox.setSingleStep(self.range_control_bias/50)
        self.control_bias_spinbox.setValue(self.control_bias)
        self.control_bias_spinbox.valueChanged.connect(self.on_biasChanged)

        self.gearing_bias_spinbox = QDoubleSpinBox()
        self.gearing_bias_spinbox.setDecimals(4);
        self.gearing_bias_spinbox.setRange(self.min_gearing_bias, self.max_gearing_bias)
        self.gearing_bias_spinbox.setSingleStep(self.range_gearing_bias/50)
        self.gearing_bias_spinbox.setValue(self.gearing_bias)
        self.gearing_bias_spinbox.valueChanged.connect(self.on_biasChanged)

    def on_biasChanged(self, value):

        # grab both if either change
        self.sensor_bias  = self.sensor_bias_spinbox.value()
        self.control_bias = self.control_bias_spinbox.value()
        self.gearing_bias = self.gearing_bias_spinbox.value()

        joint_state = JointState()
        joint_state.name = [self.name];
        joint_state.position = [self.sensor_bias];
        joint_state.effort   = [self.control_bias];
        joint_state.velocity   = [self.gearing_bias];

        print "Set biases:\n ",joint_state
        self.controller.parent.parent.bias_pub.publish(joint_state)

    def saveData(self, saveFilePtr):
        saveFilePtr.write(","+self.name+"/"+str(self.sensor_bias)+"/"+str(self.control_bias)+"/"+str(self.gearing_bias))
class FootstepParamControlWidget(QObject):

    def __init__(self, context):
        #super(FootstepParamControlDialog, self).__init__(context)
        #self.setObjectName('FootstepParamControlDialog')
        super(FootstepParamControlWidget, self).__init__()
        self.name = 'FootstepParamControlWidget'
        
        #self._widget = QWidget()
        self._widget = context
        vbox = QVBoxLayout()

        ### STEP_COST_ESTIMATOR ########
        self.step_cost_vbox = QVBoxLayout()
   
        self.step_cost_groupbox = QGroupBox( "Step Cost Estimator" )
        self.step_cost_groupbox.setCheckable( True )
        self.step_cost_groupbox.setChecked(False)

        self.step_cost_combobox = QComboBox()

        self.step_cost_combobox.addItem( "Euclidean" )
        self.step_cost_combobox.addItem( "GPR" )       
        self.step_cost_combobox.addItem( "Map" ) 
        self.step_cost_combobox.addItem( "Boundary" )
        self.step_cost_combobox.addItem( "Dynamics" )
        
        self.step_cost_vbox.addWidget( self.step_cost_combobox )
       
        self.step_cost_groupbox.setLayout( self.step_cost_vbox )
 
        vbox.addWidget( self.step_cost_groupbox )

# #HARD       ### FOOTSTEP_SET ########
#        # parameters for discret footstep planning mode
#        vigir_atlas_control_msgs/VigirBehaviorStepData[] footstep_set            # set of footsteps (displacement vectors (in meter / rad))
#        float32[] footstep_cost                                         # cost for each footstep given in footstep set
#
# #HARD       ### LOAD_GPR_STEP_COST ########
#        # map step cost file
#        std_msgs/String map_step_cost_file
#
# #HARD       ### LOAD_MAP_STEP_COST ########
#        # destination of gpr file
#        std_msgs/String gpr_step_cost_file

        ### COLLISION_CHECK_TYPE ########
        self.collision_check_groupbox = QGroupBox( "Collision Check Type" )
        self.collision_check_groupbox.setCheckable( True )
        self.collision_check_groupbox.setChecked( False)

        self.collision_check_vbox = QVBoxLayout()
   
        self.collision_check_feet_checkbox = QCheckBox( "Feet" )      
        self.collision_check_vbox.addWidget( self.collision_check_feet_checkbox  )
        
        self.collision_check_ub_checkbox = QCheckBox( "Upper Body" )
        self.collision_check_vbox.addWidget( self.collision_check_ub_checkbox  )
        
        self.collision_check_fc_checkbox = QCheckBox( "Foot Contact Support" ) 
        self.collision_check_vbox.addWidget( self.collision_check_fc_checkbox  )

        self.collision_check_groupbox.setLayout( self.collision_check_vbox )
        
        vbox.addWidget( self.collision_check_groupbox )

        ### FOOT_SIZE ########
        self.foot_size_vbox = QVBoxLayout()
        
        self.foot_size_groupbox = QGroupBox( "Foot Size" )
        self.foot_size_groupbox.setCheckable( True )
        self.foot_size_groupbox.setChecked( False )

        self.foot_size_hbox = QHBoxLayout()

        self.foot_size_label = QLabel("Foot Size")
        self.foot_size_hbox.addWidget( self.foot_size_label )

        self.foot_size_x = QDoubleSpinBox()
        self.foot_size_x.setSingleStep(.01)
        self.foot_size_hbox.addWidget(self.foot_size_x)

        self.foot_size_y = QDoubleSpinBox()
        self.foot_size_y.setSingleStep(.01)
        self.foot_size_hbox.addWidget(self.foot_size_y)

        self.foot_size_z = QDoubleSpinBox()
        self.foot_size_z.setSingleStep(.01)
        self.foot_size_hbox.addWidget(self.foot_size_z)

        self.foot_size_vbox.addLayout( self.foot_size_hbox )
        
        self.foot_shift_hbox = QHBoxLayout()

        self.foot_shift_label = QLabel("Foot Origin Shift")
        self.foot_shift_hbox.addWidget( self.foot_shift_label )

        self.foot_shift_x = QDoubleSpinBox()
        self.foot_shift_x.setRange(-1.0, 1.0)
        self.foot_shift_x.setSingleStep(.01)
        self.foot_shift_hbox.addWidget(self.foot_shift_x)

        self.foot_shift_y = QDoubleSpinBox()
        self.foot_shift_y.setRange(-1.0, 1.0)
        self.foot_shift_y.setSingleStep(.01)
        self.foot_shift_hbox.addWidget(self.foot_shift_y)

        self.foot_shift_z = QDoubleSpinBox()
        self.foot_shift_z.setRange(-1.0, 1.0)
        self.foot_shift_z.setSingleStep(.01)
        self.foot_shift_hbox.addWidget(self.foot_shift_z)

        self.foot_size_vbox.addLayout( self.foot_shift_hbox )
    
        self.foot_size_groupbox.setLayout( self.foot_size_vbox )
        vbox.addWidget( self.foot_size_groupbox )

        ### UPPER_BODY_SIZE ########
            
        self.upper_vbox = QVBoxLayout()
   
        self.upper_groupbox = QGroupBox( "Upper Body Size" )
        self.upper_groupbox.setCheckable( True )
        self.upper_groupbox.setChecked( False )
        
        self.upper_size_hbox = QHBoxLayout()

        self.upper_size_label = QLabel("Upper Body Size")
        self.upper_size_hbox.addWidget( self.upper_size_label )

        self.upper_size_x = QDoubleSpinBox()
        self.upper_size_x.setSingleStep(.01)
        self.upper_size_hbox.addWidget(self.upper_size_x)

        self.upper_size_y = QDoubleSpinBox()
        self.upper_size_y.setSingleStep(.01)
        self.upper_size_hbox.addWidget(self.upper_size_y)

        self.upper_size_z = QDoubleSpinBox()
        self.upper_size_z.setSingleStep(.01)
        self.upper_size_hbox.addWidget(self.upper_size_z)

        self.upper_vbox.addLayout( self.upper_size_hbox )
        
        self.upper_origin_hbox = QHBoxLayout()

        self.upper_origin_label = QLabel("Upper Body Origin")
        self.upper_origin_hbox.addWidget( self.upper_origin_label )

        self.upper_origin_x = QDoubleSpinBox()
        self.upper_origin_x.setRange(-1.0, 1.0)
        self.upper_origin_x.setSingleStep(.01)
        self.upper_origin_hbox.addWidget(self.upper_origin_x)

        self.upper_origin_y = QDoubleSpinBox()
        self.upper_origin_y.setRange(-1.0, 1.0)
        self.upper_origin_y.setSingleStep(.01)
        self.upper_origin_hbox.addWidget(self.upper_origin_y)

        self.upper_origin_z = QDoubleSpinBox()
        self.upper_origin_z.setRange(-1.0, 1.0)
        self.upper_origin_z.setSingleStep(.01)
        self.upper_origin_hbox.addWidget(self.upper_origin_z)

        self.upper_vbox.addLayout( self.upper_origin_hbox )
       
        self.upper_groupbox.setLayout( self.upper_vbox ) 
        vbox.addWidget( self.upper_groupbox )
    
        ### TERRAIN_MODEL ########
        self.terrain_model_groupbox = QGroupBox( "Terrain Model" )
        self.terrain_model_groupbox.setCheckable( True )
        self.terrain_model_groupbox.setChecked( False )

        self.terrain_model_vbox = QVBoxLayout()
        self.terrain_model_checkbox = QCheckBox( "Use Terrain Model" )      
        self.terrain_model_vbox.addWidget( self.terrain_model_checkbox )
        
        self.terrain_min_ssx_hbox = QHBoxLayout()
        
        self.terrain_min_ssx_label = QLabel("Min Sampling Steps x")
        self.terrain_min_ssx_hbox.addWidget( self.terrain_min_ssx_label )

        self.terrain_min_ssx_val = QDoubleSpinBox()
        self.terrain_min_ssx_val.setSingleStep(1)
        self.terrain_min_ssx_val.setRange(0,100)
        self.terrain_min_ssx_hbox.addWidget( self.terrain_min_ssx_val )

        self.terrain_model_vbox.addLayout( self.terrain_min_ssx_hbox )
    
        self.terrain_min_ssy_hbox = QHBoxLayout()
        
        self.terrain_min_ssy_label = QLabel("Min Sampling Steps y")
        self.terrain_min_ssy_hbox.addWidget( self.terrain_min_ssy_label )

        self.terrain_min_ssy_val = QDoubleSpinBox()
        self.terrain_min_ssy_val.setSingleStep(1)
        self.terrain_min_ssy_val.setRange(0,100)
        self.terrain_min_ssy_hbox.addWidget( self.terrain_min_ssy_val )

        self.terrain_model_vbox.addLayout( self.terrain_min_ssy_hbox )
        
        self.terrain_max_ssx_hbox = QHBoxLayout()
        
        self.terrain_max_ssx_label = QLabel("Max Sampling Steps x")
        self.terrain_max_ssx_hbox.addWidget( self.terrain_max_ssx_label )

        self.terrain_max_ssx_val = QDoubleSpinBox()
        self.terrain_max_ssx_val.setSingleStep(1)
        self.terrain_max_ssx_val.setRange(0,100)
        self.terrain_max_ssx_hbox.addWidget( self.terrain_max_ssx_val )

        self.terrain_model_vbox.addLayout( self.terrain_max_ssx_hbox )
    
        self.terrain_max_ssy_hbox = QHBoxLayout()
        
        self.terrain_max_ssy_label = QLabel("Max Sampling Steps y")
        self.terrain_max_ssy_hbox.addWidget( self.terrain_max_ssy_label )

        self.terrain_max_ssy_val = QDoubleSpinBox()
        self.terrain_max_ssy_val.setSingleStep(1)
        self.terrain_max_ssy_val.setRange(0,100)
        self.terrain_max_ssy_hbox.addWidget( self.terrain_max_ssy_val )

        self.terrain_model_vbox.addLayout( self.terrain_max_ssy_hbox )

        self.terrain_max_iz_hbox = QHBoxLayout()
        
        self.terrain_max_iz_label = QLabel("Max Intrusion z")
        self.terrain_max_iz_hbox.addWidget( self.terrain_max_iz_label )

        self.terrain_max_iz_val = QDoubleSpinBox()
        self.terrain_max_iz_val.setDecimals(4)
        self.terrain_max_iz_val.setSingleStep(.0001)
        self.terrain_max_iz_val.setRange(0,.25)
        self.terrain_max_iz_hbox.addWidget( self.terrain_max_iz_val )

        self.terrain_model_vbox.addLayout( self.terrain_max_iz_hbox )

        self.terrain_max_gc_hbox = QHBoxLayout()
        
        self.terrain_max_gc_label = QLabel("Max Ground Clearance")
        self.terrain_max_gc_hbox.addWidget( self.terrain_max_gc_label )

        self.terrain_max_gc_val = QDoubleSpinBox()
        self.terrain_max_gc_val.setDecimals(4)
        self.terrain_max_gc_val.setSingleStep(.0001)
        self.terrain_max_gc_val.setRange(0,.25)
        self.terrain_max_gc_hbox.addWidget( self.terrain_max_gc_val )

        self.terrain_model_vbox.addLayout( self.terrain_max_gc_hbox )

        self.terrain_ms_hbox = QHBoxLayout()
        
        self.terrain_ms_label = QLabel("Minimal Support")
        self.terrain_ms_hbox.addWidget( self.terrain_ms_label )

        self.terrain_ms_val = QDoubleSpinBox()
        self.terrain_ms_val.setDecimals(2)
        self.terrain_ms_val.setSingleStep(.01)
        self.terrain_ms_val.setRange(0,1)
        self.terrain_ms_hbox.addWidget( self.terrain_ms_val )

        self.terrain_model_vbox.addLayout( self.terrain_ms_hbox )

        self.terrain_model_groupbox.setLayout( self.terrain_model_vbox )
        
        vbox.addWidget( self.terrain_model_groupbox ) 

        ### STANDARD_STEP_PARAMS ########

        self.std_step_vbox = QVBoxLayout()

        self.std_step_groupbox = QGroupBox( "Standard Step Parameters" )
        self.std_step_groupbox.setCheckable( True )
        self.std_step_groupbox.setChecked( False )

        self.foot_sep_hbox = QHBoxLayout()
        
        self.foot_sep_label = QLabel("Foot Separation")
        self.foot_sep_hbox.addWidget( self.foot_sep_label )

        self.foot_sep_val = QDoubleSpinBox()
        self.foot_sep_val.setSingleStep(.01)
        self.foot_sep_hbox.addWidget(self.foot_sep_val)

        self.std_step_vbox.addLayout( self.foot_sep_hbox )

        self.std_step_step_duration_hbox = QHBoxLayout()
        
        self.std_step_step_duration_label = QLabel("Step Duration")
        self.std_step_step_duration_hbox.addWidget( self.std_step_step_duration_label )

        self.std_step_step_duration_val = QDoubleSpinBox()
        self.std_step_step_duration_val.setSingleStep(.01)
        self.std_step_step_duration_hbox.addWidget( self.std_step_step_duration_val )

        self.std_step_vbox.addLayout( self.std_step_step_duration_hbox )

        self.std_step_sway_duration_hbox = QHBoxLayout()
        
        self.std_step_sway_duration_label = QLabel("Sway Duration")
        self.std_step_sway_duration_hbox.addWidget( self.std_step_sway_duration_label )

        self.std_step_sway_duration_val = QDoubleSpinBox()
        self.std_step_sway_duration_val.setSingleStep(.01)
        self.std_step_sway_duration_hbox.addWidget( self.std_step_sway_duration_val )

        self.std_step_vbox.addLayout( self.std_step_sway_duration_hbox )

        self.std_step_swing_height_hbox = QHBoxLayout()
        
        self.std_step_swing_height_label = QLabel("Swing Height")
        self.std_step_swing_height_hbox.addWidget( self.std_step_swing_height_label )

        self.std_step_swing_height_val = QDoubleSpinBox()
        self.std_step_swing_height_val.setSingleStep(.01)
        self.std_step_swing_height_hbox.addWidget( self.std_step_swing_height_val )

        self.std_step_vbox.addLayout( self.std_step_swing_height_hbox )

        self.std_step_lift_height_hbox = QHBoxLayout()
        
        self.std_step_lift_height_label = QLabel("Lift Height")
        self.std_step_lift_height_hbox.addWidget( self.std_step_lift_height_label )

        self.std_step_lift_height_val = QDoubleSpinBox()
        self.std_step_lift_height_val.setSingleStep(.01)
        self.std_step_lift_height_hbox.addWidget( self.std_step_lift_height_val )

        self.std_step_vbox.addLayout( self.std_step_lift_height_hbox )
       
        self.std_step_groupbox.setLayout( self.std_step_vbox ) 
        vbox.addWidget( self.std_step_groupbox )

        button_hbox = QHBoxLayout()
        
        button_get = QPushButton("Get Current Values")
        button_hbox.addWidget( button_get )

        button_submit = QPushButton("Send Values")
        button_hbox.addWidget( button_submit)

        vbox.addLayout( button_hbox )

        vbox.addStretch(1)

        self._widget.setLayout(vbox)

        #context.add_widget(self._widget)

        # publishers and subscribers

        self.param_pub = rospy.Publisher('/flor/footstep_planner/set_params', FootstepPlannerParams, queue_size=10)
        button_submit.pressed.connect(self.sendParams)

        self.param_sub = self.stateSubscriber   = rospy.Subscriber('/ros_footstep_planner/params', FootstepPlannerParams, self.getParamCallbackFcn)
        button_get.pressed.connect(self.getParams)


    def shutdown_plugin(self):
        print "Shutdown ..." 
        self.param_pub.unregister()
        print "Done!"
    
    def getParams( self ):
        msg = FootstepPlannerParams()

        msg.change_mask = 0
        
        self.param_pub.publish( msg ) 

    def sendParams( self ):
        msg = FootstepPlannerParams()

        msg.change_mask = 0
        
        if self.step_cost_groupbox.isChecked():
            msg.change_mask = msg.change_mask | 0x001

        if self.collision_check_groupbox.isChecked():
            msg.change_mask = msg.change_mask | 0x010

        if self.foot_size_groupbox.isChecked():
            msg.change_mask = msg.change_mask | 0x020

        if self.upper_groupbox.isChecked():
            msg.change_mask = msg.change_mask | 0x040

        if self.std_step_groupbox.isChecked():
            msg.change_mask = msg.change_mask | 0x080
        
        if self.terrain_model_groupbox.isChecked():
            msg.change_mask = msg.change_mask | 0x100

        ### STEP_COST_ESTIMATOR ########
        msg.step_cost_type = self.step_cost_combobox.currentIndex()

# #HARD       ### FOOTSTEP_SET ########
#        # parameters for discret footstep planning mode
#        vigir_atlas_control_msgs/VigirBehaviorStepData[] footstep_set            # set of footsteps (displacement vectors (in meter / rad))
#        float32[] footstep_cost                                         # cost for each footstep given in footstep set
#
# #HARD       ### LOAD_GPR_STEP_COST ########
#        # map step cost file
#        std_msgs/String map_step_cost_file
#
# #HARD       ### LOAD_MAP_STEP_COST ########
#        # destination of gpr file
#        std_msgs/String gpr_step_cost_file

        ### COLLISION_CHECK_TYPE ########
        msg.collision_check_type = 0

        if self.collision_check_feet_checkbox.isChecked():
            msg.collision_check_type = msg.collision_check_type | 0x01
        
        if self.collision_check_ub_checkbox.isChecked():
            msg.collision_check_type = msg.collision_check_type | 0x02

        if self.collision_check_fc_checkbox.isChecked():
            msg.collision_check_type = msg.collision_check_type | 0x04
        
        ### FOOT_SIZE ########
        msg.foot_size.x = self.foot_size_x.value()
        msg.foot_size.y = self.foot_size_y.value()
        msg.foot_size.z = self.foot_size_z.value()

        msg.foot_origin_shift.x = self.foot_shift_x.value() 
        msg.foot_origin_shift.y = self.foot_shift_y.value() 
        msg.foot_origin_shift.z = self.foot_shift_z.value() 

        ### UPPER_BODY_SIZE ########
        msg.upper_body_size.x = self.upper_size_x.value()
        msg.upper_body_size.y = self.upper_size_y.value()
        msg.upper_body_size.z = self.upper_size_z.value()
        
        msg.upper_body_origin_shift.x = self.upper_origin_x.value()
        msg.upper_body_origin_shift.y = self.upper_origin_y.value()
        msg.upper_body_origin_shift.z = self.upper_origin_z.value()
    
        ### TERRAIN_MODEL ########
        msg.use_terrain_model = self.terrain_model_checkbox.isChecked()
        msg.min_sampling_steps_x = self.terrain_min_ssx_val.value()
        msg.min_sampling_steps_y = self.terrain_min_ssy_val.value()
        msg.max_sampling_steps_x = self.terrain_max_ssx_val.value()
        msg.max_sampling_steps_y = self.terrain_max_ssy_val.value()
        msg.max_intrusion_z      = self.terrain_max_iz_val.value()
        msg.max_ground_clearance = self.terrain_max_gc_val.value()
        msg.minimal_support      = self.terrain_ms_val.value()

        ### STANDARD_STEP_PARAMS ########
        msg.foot_seperation = self.foot_sep_val.value() 
        msg.step_duration = self.std_step_step_duration_val.value() 
        msg.sway_duration = self.std_step_sway_duration_val.value() 
        msg.swing_height = self.std_step_swing_height_val.value()
        msg.lift_height =  self.std_step_lift_height_val.value() 

        print msg

        self.param_pub.publish( msg ) 


    def getParamCallbackFcn(self, msg):
     
        ### STEP_COST_ESTIMATOR ########
        if msg.change_mask & 0x001:
            self.step_cost_combobox.setCurrentIndex( msg.step_cost_type )

# #HARD       ### FOOTSTEP_SET ########
#        # parameters for discret footstep planning mode
#        vigir_atlas_control_msgs/VigirBehaviorStepData[] footstep_set            # set of footsteps (displacement vectors (in meter / rad))
#        float32[] footstep_cost                                         # cost for each footstep given in footstep set
#
# #HARD       ### LOAD_GPR_STEP_COST ########
#        # map step cost file
#        std_msgs/String map_step_cost_file
#
# #HARD       ### LOAD_MAP_STEP_COST ########
#        # destination of gpr file
#        std_msgs/String gpr_step_cost_file

        ### COLLISION_CHECK_TYPE ########
        if msg.change_mask & 0x010:
            self.collision_check_feet_checkbox.setChecked( msg.collision_check_type & 0x01 )
            self.collision_check_ub_checkbox.setChecked( msg.collision_check_type & 0x02 )
            self.collision_check_fc_checkbox.setChecked( msg.collision_check_type & 0x04 )

        ### FOOT_SIZE ########
        if msg.change_mask & 0x020:
            self.foot_size_x.setValue( msg.foot_size.x )
            self.foot_size_y.setValue( msg.foot_size.y )
            self.foot_size_z.setValue( msg.foot_size.z )

            self.foot_shift_x.setValue( msg.foot_origin_shift.x )
            self.foot_shift_y.setValue( msg.foot_origin_shift.y )
            self.foot_shift_z.setValue( msg.foot_origin_shift.z )
           
        ### UPPER_BODY_SIZE ########
        if msg.change_mask & 0x040:
            self.upper_size_x.setValue( msg.upper_body_size.x )
            self.upper_size_y.setValue( msg.upper_body_size.y )
            self.upper_size_z.setValue( msg.upper_body_size.z )
            
            self.upper_origin_x.setValue( msg.upper_body_origin_shift.x )
            self.upper_origin_y.setValue( msg.upper_body_origin_shift.y )
            self.upper_origin_z.setValue( msg.upper_body_origin_shift.z )

        ### STANDARD_STEP_PARAMS ########
        if msg.change_mask & 0x080:
            self.foot_sep_val.setValue( msg.foot_seperation ) 
            self.std_step_step_duration_val.setValue( msg.step_duration ) 
            self.std_step_sway_duration_val.setValue( msg.sway_duration ) 
            self.std_step_swing_height_val.setValue( msg.swing_height ) 
            self.std_step_lift_height_val.setValue( msg.lift_height ) 

        ### TERRAIN_MODEL ########
        if msg.change_mask & 0x100:
            self.terrain_model_checkbox.setChecked( msg.use_terrain_model )
            self.terrain_min_ssx_val.setValue(   msg.min_sampling_steps_x  )
            self.terrain_min_ssy_val.setValue(   msg.min_sampling_steps_y  )
            self.terrain_max_ssx_val.setValue(   msg.max_sampling_steps_x  )
            self.terrain_max_ssy_val.setValue(   msg.max_sampling_steps_y  )
            self.terrain_max_iz_val.setValue(  msg.max_intrusion_z     )  
            self.terrain_max_gc_val.setValue(  msg.max_ground_clearance )
            self.terrain_ms_val.setValue(    msg.minimal_support )
Пример #3
0
class SpeakEasyGUI(QMainWindow):
    '''
    One instance of this class builds the entire sound play UI.
    Instance variable that hold widgets of interest to controllers:
    
	    - C{speechInputFld}
	    - C{onceOrRepeatDict}
	    - C{voicesRadioButtonsDict}
	    - C{recorderDict}
	    - C{programButtonDict}
	    - C{soundButtonDict}

    '''

    # ---------------------- Durations --------------------
    
    # Number of seconds a program button must be pressed to
    # go into program mode:
    PROGRAM_BUTTON_HOLD_TIME = 3.0;

    # Amount of time the program button stays in alternate look
    # to indicate that it is changing to programming mode: 

    PROGRAM_BUTTON_LOOK_CHANGE_DURATION = 0.2; # seconds

    # ---------------------- Button and Font Sizes -------------------- 

    # Minimum height of buttons:
    BUTTON_MIN_HEIGHT = 30; # pixels
    # Pushbutton minimum font size:
    BUTTON_LABEL_FONT_SIZE = 16; # pixels
    
    # Radio button minimum font size:
    RADIO_BUTTON_LABEL_FONT_SIZE = 16; # pixels
    
    EDIT_FIELD_TEXT_SIZE = 18; # pixels
    
    NUM_OF_PROGRAM_BUTTON_COLUMNS = 4;
    NUM_OF_SOUND_BUTTON_COLUMNS   = 4;
    
    # ---------------------- Names for Voices ----------------------
    
    # Official names of voices as recognized by the
    # underlying text-to-speech engines:
    voices = {'VOICE_1': 'voice_kal_diphone',  # Festival voice
              'VOICE_2': 'David',              # Cepstral voices
              'VOICE_3': 'Amy',
              'VOICE_4': 'Shouty',
              'VOICE_5': 'Whispery',
              'VOICE_6': 'Lawrence',
              'VOICE_7': 'William'
              };
        
    # ---------------------- Names for Widgets ----------------------
    
    interactionWidgets = {
                          'PLAY_ONCE': 'Play once',
                          'PLAY_REPEATEDLY': 'Play repeatedly',
                          'PLAY_REPEATEDLY_PERIOD': 'Pause between plays',
                          'VOICE_1': 'Machine',
                          'VOICE_2': 'David',
                          'VOICE_3': 'Amy',
                          'VOICE_4': 'Shout',
                          'VOICE_5': 'Whisper',
                          'VOICE_6': 'Lawrence',
                          'VOICE_7': 'William',
                          'PLAY_TEXT': 'Play Text',
                          'STOP': 'Stop',
                          'STOP_ALL' : 'Stop All',
                          
                          'SPEECH_1' : 'Speech 1',
                          'SPEECH_2' : 'Speech 2',
                          'SPEECH_3' : 'Speech 3',
                          'SPEECH_4' : 'Speech 4',
                          'SPEECH_5' : 'Speech 5',
                          'SPEECH_6' : 'Speech 6',
                          'SPEECH_7' : 'Speech 7',
                          'SPEECH_8' : 'Speech 8',
                          'SPEECH_9' : 'Speech 9',
                          'SPEECH_10' : 'Speech 10',
                          'SPEECH_11' : 'Speech 11',
                          'SPEECH_12' : 'Speech 12',
                          'SPEECH_13' : 'Speech 13',
                          'SPEECH_14' : 'Speech 14',
                          'SPEECH_15' : 'Speech 15',
                          'SPEECH_16' : 'Speech 16',
                          'SPEECH_17' : 'Speech 17',
                          'SPEECH_18' : 'Speech 18',
                          'SPEECH_19' : 'Speech 19',
                          'SPEECH_20' : 'Speech 20',
                          'SPEECH_21' : 'Speech 21',
                          'SPEECH_22' : 'Speech 22',
                          'SPEECH_23' : 'Speech 23',
                          'SPEECH_24' : 'Speech 24',
                          
                          'SOUND_1' : 'Rooster',
                          'SOUND_2' : 'Drill',
                          'SOUND_3' : 'Bull call',
                          'SOUND_4' : 'Clown horn',
                          'SOUND_5' : 'Cash register',
                          'SOUND_6' : 'Glass breaking',
                          'SOUND_7' : 'Cell door',
                          'SOUND_8' : 'Cow',
                          'SOUND_9' : 'Birds',
                          'SOUND_10' : 'Big truck',
                          'SOUND_11' : 'Seagulls',
                          'SOUND_12' : 'Lift',
                          
                          'NEW_SPEECH_SET' : 'Save speech set',
                          'PICK_SPEECH_SET' : 'Pick different speech set',
                          
                          'PLAY_LOCALLY' : 'Play locally',
                          'PLAY_AT_ROBOT' : 'Play at robot',
                          'SPEECH_MODULATION' : 'Speech modulation',
                          'PASTE' : 'Paste', 
                          'CLEAR' : 'Clear', 
                          } 
        
    # ---------------------- Application Background styling --------------------     

    # Application background:
    veryLightBlue = QColor(230, 255,255);
    stylesheetAppBG = 'QDialog {background-color: %s}' % veryLightBlue.name();
    
    defaultStylesheet = stylesheetAppBG;
    defaultStylesheetName = "Default";
    
    # ---------------------- Edit field styling -----------------

    editFieldBGColor =  QColor(12,21,109);
    editFieldTextColor = QColor(244,244,246);

    inputFldStylesheet =\
        'TextPanel {background-color: ' + editFieldBGColor.name() +\
        '; color: ' + editFieldTextColor.name() +\
        '; font-size: ' + str(EDIT_FIELD_TEXT_SIZE) + 'pt' +\
        '}';
    
    # ---------------------- Pushbutton styling -------------------- 
    
    # Button color definitions:
    recorderButtonBGColor = QColor(176,220,245); # Light blue
    recorderButtonDisabledBGColor = QColor(187,200,208); # Gray-blue
    recorderButtonTextColor = QColor(0,0,0);     # Black
    programButtonBGColor = QColor(117,150,169);  # Middle blue
    programButtonTextColor = QColor(251,247,247);# Off-white
    soundButtonBGColor = QColor(110,134,211);      # Dark blue
    soundButtonTextColor = QColor(251,247,247);  # Off-white

    # Button stylesheets:    
    recorderButtonStylesheet =\
        'QPushButton {background-color: ' + recorderButtonBGColor.name() +\
        '; color: ' + recorderButtonTextColor.name() +\
        '; font-size: ' + str(BUTTON_LABEL_FONT_SIZE) + 'px' +\
        '}';
        
    recorderButtonDisabledStylesheet =\
        'QPushButton {background-color: ' + recorderButtonDisabledBGColor.name() +\
        '; color: ' + recorderButtonTextColor.name() +\
        '; font-size: ' + str(BUTTON_LABEL_FONT_SIZE) + 'px' +\
        '}';
        
    programButtonStylesheet =\
        'QPushButton {background-color: ' + programButtonBGColor.name() +\
        '; color: ' + programButtonTextColor.name() +\
        '; font-size: ' + str(BUTTON_LABEL_FONT_SIZE) + 'px' +\
        '}';

    # Stylesheet for when program button look is temporarily
    # changed to indicate transition to programming mode:                                                          
    programButtonModeTransitionStylesheet =\
        'QPushButton {background-color: ' + editFieldBGColor.name() +\
        '; color: ' + programButtonTextColor.name() +\
        '; font-size: ' + str(BUTTON_LABEL_FONT_SIZE) + 'px' +\
        '}';
                                                          
                                                          
    soundButtonStylesheet =\
        'QPushButton {background-color: ' + soundButtonBGColor.name() +\
        '; color: ' + soundButtonTextColor.name() +\
        '; font-size: ' + str(BUTTON_LABEL_FONT_SIZE) + 'px' +\
        '}';

    # ---------------------- Radiobutton and Play Repeat Delay Spinbox styling -----------------
    
    # Radiobutton color definitions:
    playOnceRepeatButtonBGColor = QColor(121,229,230); # Very light cyan
    voicesButtonBGColor = QColor(97,164,165); # Darker cyan

    
    playOnceRepeatButtonStylesheet =\
     'font-size: ' + str(RADIO_BUTTON_LABEL_FONT_SIZE) + 'px' +\
     '; color: ' + soundButtonTextColor.name() +\
     '; background-color: ' + voicesButtonBGColor.name();
     
    playRepeatSpinboxStylesheet =\
     'font-size: ' + str(RADIO_BUTTON_LABEL_FONT_SIZE) + 'px' +\
     '; color: ' + soundButtonTextColor.name() +\
     '; background-color: ' + voicesButtonBGColor.name();

#    voiceButtonStylesheet =\
#     'font-size: ' + str(RADIO_BUTTON_LABEL_FONT_SIZE) + 'px' +\
#     '; background-color: ' + voicesButtonBGColor.name();

    voiceButtonStylesheet =\
     'font-size: ' + str(RADIO_BUTTON_LABEL_FONT_SIZE) + 'px' +\
     '; color: ' + soundButtonTextColor.name() +\
     '; background-color: ' + voicesButtonBGColor.name();

    # ---------------------- Signals -----------------
    
    hideButtonSignal  	= pyqtSignal(QPushButton); # hide the given button
    showButtonSignal  	= pyqtSignal(QPushButton); # show the hidden button

    #----------------------------------
    # Initializer 
    #--------------
        
    def __init__(self, parent=None, mirrored=True, stand_alone=False, sound_effect_labels=None):
        
        super(SpeakEasyGUI, self).__init__(parent);
        
        self.stand_alone = stand_alone;
        self.sound_effect_labels = sound_effect_labels;
        if (sound_effect_labels is None):
            raise ValueError("Must pass in non-null array of sound effect button labels.");
        
        #self.setMaximumWidth(1360);
        #self.setMaximumHeight(760);
        
            # Vertical box to hold all top level widget groups
        appLayout = QVBoxLayout();
        appWidget = QDialog();      
        appWidget.setStyleSheet(SpeakEasyGUI.defaultStylesheet);
        # Activate the window resize handle in the lower right corner
        # of the app window:
        appWidget.setSizeGripEnabled(True);
        
        self.setCentralWidget(appWidget);

        self.addTitle(appLayout);        
        self.addTxtInputFld(appLayout);
        self.buildTapeRecorderButtons(appLayout);
        self.addOnceOrRepeat_And_VoiceRadioButtons(appLayout);
        
        self.buildHorizontalDivider(appLayout);
        self.buildProgramButtons(appLayout);
        self.buildSoundButtons(appLayout);
        self.buildButtonSetControls(appLayout);
        self.buildOptionsRadioButtons(appLayout);
        
        appWidget.setLayout(appLayout);
    
        #*******self.connectEvents();    
        self.show();

    #----------------------------------
    # programButtonIterator 
    #--------------

    def programButtonIterator(self, gridLayout=None):
        
        if gridLayout is None:
            gridLayout = self.programButtonGridLayout;
        
        programButtonArray = [];
        # Collect the buttons into a flat array:
        for row in range(gridLayout.rowCount()):
            for col in range(SpeakEasyGUI.NUM_OF_PROGRAM_BUTTON_COLUMNS):
                layoutItem = gridLayout.itemAtPosition(row, col);
                if layoutItem is not None:
                    programButtonArray.append(layoutItem.widget());
        return iter(programButtonArray);

    #----------------------------------
    # replaceProgramButtons
    #--------------

    def replaceProgramButtons(self, buttonProgramsArray):
        
        programButtonObjIt = self.programButtonIterator();
        # Remove the existing program buttons from the application's
        # layout, and mark them for deletion:
        try:
            while True:
                programButtonObj = programButtonObjIt.next();
                self.programButtonGridLayout.removeWidget(programButtonObj);
                programButtonObj.deleteLater();
        except StopIteration:
            pass
        
        # Make an array of button labels from the ButtonProgram
        # instances in buttonProgramsArray:
        buttonLabelArr = [];
        for buttonProgram in buttonProgramsArray:
            buttonLabelArr.append(buttonProgram.getLabel());
            
        # No more program buttons present. Make 
        # new ones, and add them to the grid layout:
        (newProgramButtonGridLayout, self.programButtonDict) =\
            SpeakEasyGUI.buildButtonGrid(buttonLabelArr,
                                         SpeakEasyGUI.NUM_OF_PROGRAM_BUTTON_COLUMNS);
        for buttonObj in self.programButtonDict.values():
            buttonObj.setStyleSheet(SpeakEasyGUI.programButtonStylesheet);
            buttonObj.setMinimumHeight(SpeakEasyGUI.BUTTON_MIN_HEIGHT);

        # Transfer the buttons from this new gridlayout object to the
        # original layout that is embedded in the application Dialog:
        newButtonsIt = self.programButtonIterator(gridLayout=newProgramButtonGridLayout);
        try:
            while True:
                self.programButtonGridLayout.addWidget(newButtonsIt.next());
        except StopIteration:
            pass;
    
    #----------------------------------
    # connectEvents
    #--------------
    
    def connectEvents(self):
        mousePressEvent.connect(self.textAreaMouseClick);
        
    def textAreaMouseClick(self, event):
        print('Text area clicked.')
    
    
    #----------------------------------
    # connectSignalsToWidgets 
    #--------------
    
#    def connectSignalsToWidgets(self):
#        self.commChannel = CommChannel();
#        self.commChannel.hideButtonSignal.connect(SpeakEasyGUI.hideButtonHandler);
#        self.commChannel.showButtonSignal.connect(SpeakEasyGUI.showButtonHandler);


    #----------------------------------
    # addTitle 
    #--------------

    def addTitle(self,layout):
        title = QLabel("<H1>SpeakEasy</H1>");
        hbox = QHBoxLayout();
        hbox.addStretch(1);
        hbox.addWidget(title);
        hbox.addStretch(1);
        
        layout.addLayout(hbox);
        
        
    #----------------------------------
    # addTxtInputFld 
    #--------------
        
    def addTxtInputFld(self, layout):
        '''
        Creates text input field label and text field
        in a horizontal box layout. Adds that hbox layout
        to the passed-in layout.
        
        Sets instance variables:
            1. C{self.speechInputFld}
         
        @param layout: Layout object to which the label/txt-field C{hbox} is to be added.
        @type  layout: QLayout
        '''
        speechControlsLayout = QHBoxLayout();
        speechControlsLayout.addStretch(1);
        speechInputFldLabel = QLabel("<b>What to say:</b>")
        speechControlsLayout.addWidget(speechInputFldLabel);
        
        self.speechInputFld = TextPanel(numLines=5);
        self.speechInputFld.setStyleSheet(SpeakEasyGUI.inputFldStylesheet);
        self.speechInputFld.setFontPointSize(SpeakEasyGUI.EDIT_FIELD_TEXT_SIZE);
        
        speechControlsLayout.addWidget(self.speechInputFld);
        
        layout.addLayout(speechControlsLayout);
        
        # Create and hide the dialog for adding Cepstral voice modulation 
        # markup to text in the text input field:
        self.speechControls = MarkupManagementUI(textPanel=self.speechInputFld, parent=self);

    #----------------------------------
    # addVoiceRadioButtons
    #--------------
            
    def addOnceOrRepeat_And_VoiceRadioButtons(self, layout):
        '''
        Creates radio buttons for selecting whether a
        sound is to play once, or repeatedly until stopped.
        Also adds radio buttons for selecting voices.
        Places all in a horizontal box layout. Adds 
        that hbox layout to the passed-in layout.
        
        Sets instance variables:
            1. C{self.onceOrRepeatDict}
            2. C{self.voicesRadioButtonsDict}
         
        @param layout: Layout object to which the label/txt-field C{hbox} is to be added.
        @type  layout: QLayout
         '''
        
        hbox = QHBoxLayout();

        (self.onceOrRepeatGroup, onceOrRepeatButtonLayout, self.onceOrRepeatDict) =\
            self.buildRadioButtons([SpeakEasyGUI.interactionWidgets['PLAY_ONCE'], 
                                    SpeakEasyGUI.interactionWidgets['PLAY_REPEATEDLY']
                                    ],
                                   Orientation.HORIZONTAL,
                                   Alignment.LEFT,
                                   activeButtons=[SpeakEasyGUI.interactionWidgets['PLAY_ONCE']],
                                   behavior=CheckboxGroupBehavior.RADIO_BUTTONS);

        self.replayPeriodSpinBox = QDoubleSpinBox(self);
        self.replayPeriodSpinBox.setRange(0.0, 99.9); # seconds
        self.replayPeriodSpinBox.setSingleStep(0.5);
        self.replayPeriodSpinBox.setDecimals(1);
        onceOrRepeatButtonLayout.addWidget(self.replayPeriodSpinBox);
        secondsLabel = QLabel("secs delay");
        onceOrRepeatButtonLayout.addWidget(secondsLabel);
        
        # Create an array of voice radio button labels:
        voiceRadioButtonLabels = [];
        for voiceKey in SpeakEasyGUI.voices.keys():
            voiceRadioButtonLabels.append(SpeakEasyGUI.interactionWidgets[voiceKey]);
            
        (self.voicesGroup, voicesButtonLayout, self.voicesRadioButtonsDict) =\
            self.buildRadioButtons(voiceRadioButtonLabels,
                                   Orientation.HORIZONTAL,
                                   Alignment.RIGHT,
                                   activeButtons=[SpeakEasyGUI.interactionWidgets['VOICE_1']],
                                   behavior=CheckboxGroupBehavior.RADIO_BUTTONS);
                                   
        # Style all the radio buttons:
        for playFreqButton in self.onceOrRepeatDict.values():
            playFreqButton.setStyleSheet(SpeakEasyGUI.playOnceRepeatButtonStylesheet); 
        for playFreqButton in self.voicesRadioButtonsDict.values():
            playFreqButton.setStyleSheet(SpeakEasyGUI.voiceButtonStylesheet);
        #...and the replay delay spinbox:
        self.replayPeriodSpinBox.setStyleSheet(SpeakEasyGUI.playRepeatSpinboxStylesheet);
        #****** replayPeriodSpinBox styling 
                                   
        hbox.addLayout(onceOrRepeatButtonLayout);
        hbox.addStretch(1);
        hbox.addLayout(voicesButtonLayout);
        layout.addLayout(hbox);
        
    #----------------------------------
    # buildTapeRecorderButtons 
    #--------------
        
    def buildTapeRecorderButtons(self, layout):
        '''
        Creates tape recorder buttons (Play Text, Stop,etc.).
        Places all in a row, though the layout is a 
        QGridLayout. Adds QGridLayout to the passed-in 
        layout.
        
        Sets instance variables:
            1. C{self.recorderDict}
         
        @param layout: Layout object to which the label/txt-field C{QGridlayout} is to be added.
        @type  layout: QLayout
        '''
        
        (buttonGridLayout, self.recorderButtonDict) =\
            SpeakEasyGUI.buildButtonGrid([SpeakEasyGUI.interactionWidgets['PLAY_TEXT'],
                                          SpeakEasyGUI.interactionWidgets['STOP'],
                                          #SpeakEasyGUI.interactionWidgets['STOP_ALL'] # Stop button already stops all.
                                          ],
                                         2); # Two columns
        for buttonObj in self.recorderButtonDict.values():
            buttonObj.setStyleSheet(SpeakEasyGUI.recorderButtonStylesheet);
            buttonObj.setMinimumHeight(SpeakEasyGUI.BUTTON_MIN_HEIGHT);
        layout.addLayout(buttonGridLayout);                                 
         
    #----------------------------------
    # buildProgramButtons
    #--------------
        
    def buildProgramButtons(self, layout):
        '''
        Creates grid of buttons for saving sounds.
        Adds the resulting QGridLayout to the passed-in 
        layout.
        
        Sets instance variables:
            1. C{self.programButtonDict}
         
        @param layout: Layout object to which the label/txt-field C{QGridlayout} is to be added.
        @type  layout: QLayout
        '''

        buttonLabelArr = [];
        for i in range(NUM_SPEECH_PROGRAM_BUTTONS):
            key = "SPEECH_" + str(i+1);
            buttonLabelArr.append(SpeakEasyGUI.interactionWidgets[key]);
                    
        (self.programButtonGridLayout, self.programButtonDict) =\
            SpeakEasyGUI.buildButtonGrid(buttonLabelArr,
                                         SpeakEasyGUI.NUM_OF_PROGRAM_BUTTON_COLUMNS);
        for buttonObj in self.programButtonDict.values():
            buttonObj.setStyleSheet(SpeakEasyGUI.programButtonStylesheet);
            buttonObj.setMinimumHeight(SpeakEasyGUI.BUTTON_MIN_HEIGHT);
            
        layout.addLayout(self.programButtonGridLayout);                                 
        
    #----------------------------------
    # buildSoundButtons
    #--------------
        
    def buildSoundButtons(self, layout):
        '''
        Creates grid of buttons for playing canned sounds.
        Adds the resulting QGridLayout to the passed-in 
        layout.
        
        Sets instance variables:
            1. C{self.soundButtonDict}
         
        @param layout: Layout object to which the label/txt-field C{QGridlayout} is to be added.
        @type  layout: QLayout
        '''
        
        for i in range(len(self.sound_effect_labels)):
            key = "SOUND_" + str(i);
            SpeakEasyGUI.interactionWidgets[key] = self.sound_effect_labels[i];
        
        (buttonGridLayout, self.soundButtonDict) =\
            SpeakEasyGUI.buildButtonGrid(self.sound_effect_labels, SpeakEasyGUI.NUM_OF_SOUND_BUTTON_COLUMNS);
        for buttonObj in self.soundButtonDict.values():
            buttonObj.setStyleSheet(SpeakEasyGUI.soundButtonStylesheet);
            buttonObj.setMinimumHeight(SpeakEasyGUI.BUTTON_MIN_HEIGHT);
                        
        layout.addLayout(buttonGridLayout);                                 

    #----------------------------------
    # buildButtonSetControls
    #--------------

    def buildButtonSetControls(self, layout):
        
        buttonLabelArray = [self.interactionWidgets['NEW_SPEECH_SET'], self.interactionWidgets['PICK_SPEECH_SET']]; 
        # Two columns of buttons:
        (buttonGridLayout, self.speechSetButtonDict) = SpeakEasyGUI.buildButtonGrid(buttonLabelArray, 2);
        for buttonObj in self.speechSetButtonDict.values():
            buttonObj.setStyleSheet(SpeakEasyGUI.programButtonStylesheet);
            buttonObj.setMinimumHeight(SpeakEasyGUI.BUTTON_MIN_HEIGHT);
        layout.addLayout(buttonGridLayout);
        
    #----------------------------------
    # buildOptionsRadioButtons 
    #--------------
        
    def buildOptionsRadioButtons(self, layout):
        hbox = QHBoxLayout();
        (self.playLocalityGroup, playLocalityButtonLayout, self.playLocalityRadioButtonsDict) =\
            self.buildRadioButtons([SpeakEasyGUI.interactionWidgets['PLAY_LOCALLY'],
                                    SpeakEasyGUI.interactionWidgets['PLAY_AT_ROBOT']
                                    ],
                                   Orientation.HORIZONTAL,
                                   Alignment.LEFT,
                                   activeButtons=[SpeakEasyGUI.interactionWidgets[DEFAULT_PLAY_LOCATION]],
                                   behavior=CheckboxGroupBehavior.RADIO_BUTTONS);
                                   #behavior=CheckboxGroupBehavior.CHECKBOXES);
                                   
        # Style all the radio buttons:
        for playLocalityButton in self.playLocalityRadioButtonsDict.values():
            playLocalityButton.setStyleSheet(SpeakEasyGUI.voiceButtonStylesheet); 
        hbox.addLayout(playLocalityButtonLayout);
        hbox.addStretch(1);
        self.buildConvenienceButtons(hbox);
        layout.addLayout(hbox);

    #----------------------------------
    # buildConvenienceButtons
    #--------------
        
    def buildConvenienceButtons(self, layout):
        '''
        Creates buttons meant for accessibility convenience.
        Example: Paste.
        Places all in a row, though the layout is a 
        QGridLayout. Adds QGridLayout to the passed-in 
        layout.
        
        Sets instance variables:
            1. C{self.convenienceButtonDict}
         
        @param layout: Layout object to which the label/txt-field C{QGridlayout} is to be added.
        @type  layout: QLayout
        '''
        
        (buttonGridLayout, self.convenienceButtonDict) =\
            SpeakEasyGUI.buildButtonGrid([SpeakEasyGUI.interactionWidgets['SPEECH_MODULATION'],
                                          SpeakEasyGUI.interactionWidgets['PASTE'],
                                          SpeakEasyGUI.interactionWidgets['CLEAR'],
                                          ],
                                         2); # Two columns
        for buttonObj in self.convenienceButtonDict.values():
            buttonObj.setStyleSheet(SpeakEasyGUI.recorderButtonStylesheet);
            buttonObj.setMinimumHeight(SpeakEasyGUI.BUTTON_MIN_HEIGHT);
        layout.addLayout(buttonGridLayout);                                 
        
    #----------------------------------
    # buildHorizontalDivider 
    #--------------
        
    def buildHorizontalDivider(self, layout):
        frame = QFrame();
        frame.setFrameShape(QFrame.HLine);
        #*******frame.setFrameStyle(QFrame.Shadow.Sunken.value());
        frame.setLineWidth(3);
        frame.setMidLineWidth(3);
        layout.addWidget(frame);
        
        
    #----------------------------------
    # buildRadioButtons
    #--------------
        
    # Note: Whenever a button is switched on or off it emits the 
    # PySide.QtGui.QAbstractButton.toggled() signal. Connect to this 
    # signal if you want to trigger an action each time the button changes state.         
    
    def buildRadioButtons(self, 
                          labelTextArray,
                          orientation,
                          alignment,
                          activeButtons=None, 
                          behavior=CheckboxGroupBehavior.RADIO_BUTTONS):
        '''
        @param labelTextArray: Names of buttons
        @type labelTextArray: [string]
        @param orientation: Whether to arrange the buttons vertically or horizontally.
        @type orientation: Orientation
        @param alignment: whether buttons should be aligned Left/Center/Right for horizontal,
                          Top/Center/Bottom for vertical:/c
        @type  alignment: Alignment
        @param activeButtons: Name of the buttons that is to be checked initially. Or None.
        @type activeButtons: [string]
        @param behavior: Indicates whether the button group is to behave like Radio Buttons, or like Checkboxes.
        @type behavior: CheckboxGroupBehavior
        @return 
            1. The button group that contains the related buttons. Caller: ensure that 
               this object does not go out of scope. 
            2. The button layout, which callers will need to add to
               their own layouts.
            3. and a dictionary mapping button names to button objects that
               were created within this method. This dict is needed by the
               controller.
        @rtype (QButtonGroup, QLayout, dict<string,QRadioButton>).
        '''
        
        # Button control management group. It has no visible
        # representation. It allows the radio button bahavior, for instance:
        buttonGroup = QButtonGroup();
        
        if behavior == CheckboxGroupBehavior.CHECKBOXES:
            buttonGroup.setExclusive(False);
        else:
            buttonGroup.setExclusive(True);
        
        if orientation == Orientation.VERTICAL:
            buttonCompLayout = QVBoxLayout();
            # Arrange for the alignment (Part 1):
            if (alignment == Alignment.CENTER) or\
               (alignment == Alignment.BOTTOM):
                buttonCompLayout.addStretch(1);
        else:
            buttonCompLayout = QHBoxLayout();
            # Arrange for the alignment (Part 1):
            if (alignment == Alignment.CENTER) or\
               (alignment == Alignment.RIGHT):
                buttonCompLayout.addStretch(1);
            
        # Build the buttons:
        buttonDict  = {};
        
        for label in labelTextArray:
            button = QRadioButton(label);
            buttonDict[label] = button;
            # Add button to the button management group:
            buttonGroup.addButton(button);
            # Add the button to the visual group:
            buttonCompLayout.addWidget(button);
            if label in activeButtons:
                button.setChecked(True);

        if orientation == Orientation.VERTICAL:
            buttonCompLayout = QVBoxLayout();
            # Arrange for the alignment (Part 2):
            if (alignment == Alignment.CENTER) or\
               (alignment == Alignment.TOP):
                buttonCompLayout.addStretch(1);
        else:
            # Arrange for the alignment (Part 2):
            if (alignment == Alignment.CENTER) or\
               (alignment == Alignment.LEFT):
                buttonCompLayout.addStretch(1);
                        
        return (buttonGroup, buttonCompLayout, buttonDict);
    
    #----------------------------------
    # buildButtonGrid
    #--------------

    @staticmethod
    def buildButtonGrid(labelTextArray, 
                        numColumns):
        '''
        Creates a grid of QPushButton widgets. They will be 
        
        @param labelTextArray: Button labels.
        @type labelTextArray: [string]
        @param numColumns: The desired width of the button grid.
        @type numColumns: int
        @return: 1. a grid layout with the button objects inside.
                 2. a dictionary mapping button labels to button objects.
        @rtype: QGridLayout 
        '''

        buttonLayout = QGridLayout();
        
        # Compute number of button rows:
        numRows = len(labelTextArray) / numColumns;
        # If this number of rows leaves a few buttons left over (< columnNum),
        # then add a row for those:
        if (len(labelTextArray) % numColumns) > 0:
            numRows += 1;
        
        buttonDict = {}
        
        row = 0;
        col = 0;
        for buttonLabel in labelTextArray:
            button = QPushButton(buttonLabel);
            buttonDict[buttonLabel] = button;
            buttonLayout.addWidget(button, row, col);
            col += 1;
            if (col > (numColumns - 1)):
                col = 0;
                row += 1;
                
        return (buttonLayout, buttonDict);
        
    #----------------------------------
    # getNewButtonLabel 
    #--------------

    def getNewButtonLabel(self):
        '''
        Requests a new button label from the user.
        Returns None if user canceled out, or a string
        with the new button label. 
        @return: None if user canceled, else string from input field.
        @rtype: {None | string}
        
        '''
        prompt = "New label for this button:";
        dialogBox = QInputDialog(parent=self);
        dialogBox.setStyleSheet(SpeakEasyGUI.stylesheetAppBG);
        dialogBox.setLabelText(prompt);
        dialogBox.setCancelButtonText("Keep existing label");
        userChoice = dialogBox.exec_();

        if userChoice == QDialog.Accepted:
            return dialogBox.textValue();
        else:
            return None;
        
    #----------------------------------
    # playOnceChecked 
    #--------------

    def playOnceChecked(self):
        return self.onceOrRepeatDict[SpeakEasyGUI.interactionWidgets['PLAY_ONCE']].isChecked();

    #----------------------------------
    # setPlayOnceChecked 
    #--------------
    
    def setPlayOnceChecked(self):
        self.onceOrRepeatDict[SpeakEasyGUI.interactionWidgets['PLAY_ONCE']].setChecked(True);
    
    #----------------------------------
    # playRepeatedlyChecked 
    #--------------

    def playRepeatedlyChecked(self):
        return self.onceOrRepeatDict[SpeakEasyGUI.interactionWidgets['PLAY_REPEATEDLY']].isChecked();

    #----------------------------------
    # setPlayRepeatedlyChecked 
    #--------------
    
    def setPlayRepeatedlyChecked(self):
        self.onceOrRepeatDict[SpeakEasyGUI.interactionWidgets['PLAY_REPEATEDLY']].setChecked(True);
        
    #----------------------------------
    # playRepeatPeriod 
    #--------------

    def getPlayRepeatPeriod(self):
        return self.replayPeriodSpinBox.value();
    
    #----------------------------------
    # activeVoice
    #--------------

    def activeVoice(self):
        '''
        Return the official name of the voice that is currently
        checked in the UI. This is the name that will be recognized
        by the underlying text-to-speech engine(s).
        @return: Name of voice as per the SpeakEasyGUI.voices dict.
        @rtype: string
        '''
        for voiceKey in SpeakEasyGUI.voices.keys():
            if self.voicesRadioButtonsDict[SpeakEasyGUI.interactionWidgets[voiceKey]].isChecked():
                return SpeakEasyGUI.voices[voiceKey];

    #----------------------------------
    # whereToPlay 
    #--------------
    
    def whereToPlay(self):
        '''
        Returns which of the play location options is selected: Locally or Robot.
        @return: Selection of where sound and text-to-speech output is to occur.
        @rtype: PlayLocation
        '''
        if self.playLocalityRadioButtonsDict[SpeakEasyGUI.interactionWidgets['PLAY_LOCALLY']].isChecked():
            return PlayLocation.LOCALLY;
        else:
            return PlayLocation.ROBOT;
    
    #----------------------------------
    # setWhereToPlay 
    #--------------
    
    def setWhereToPlay(self, playLocation):
        '''
        Set the option radio button that determines where sound is produced,
        locally, or at the robot. No action is taken. This method merely sets
        the appropriate radio button.
        @param playLocation: PlayLocation.LOCALLY, or PlayLocation.ROBOT
        @type playLocation: PlayLocation
        '''
        
        if playLocation == PlayLocation.LOCALLY:
            radioButton = self.playLocalityRadioButtonsDict[SpeakEasyGUI.interactionWidgets['PLAY_LOCALLY']];
        else:
            radioButton = self.playLocalityRadioButtonsDict[SpeakEasyGUI.interactionWidgets['PLAY_AT_ROBOT']];
        radioButton.setChecked(True);
            
    #----------------------------------
    # setButtonLabel 
    #--------------
    
    def setButtonLabel(self, buttonObj, label):
        buttonObj.setText(label);

    #----------------------------------
    # blinkButton
    #--------------

    def blinkButton(self, buttonObj, turnOn):
        '''
        Used to make a program button blink in some way to indicate
        that it is changing into programming mode. Since this method
        is triggered by a timer thread, it cannot make any GUI changes.
        Instead, it sends a signal to have the GUI thread place the
        button into an alternative look. It then schedules a call
        to itself for a short time later. At that point it sends a
        signal to the GUI thread to return the button to its usual
        look:
        @param buttonObj: The button to be blinked
        @type  buttonObj: QPushButton
        @param turnOn:    Indicates whether to turn the button back into 
                          its normal state, or whether to make it take on
                          its alternate look. 
        @type  turnOn:    bool
        '''
        if turnOn:
            self.showButtonSignal.emit(buttonObj);
        else:
            self.hideButtonSignal.emit(buttonObj);
            self.buttonBlinkTimer = Timer(SpeakEasyGUI.PROGRAM_BUTTON_LOOK_CHANGE_DURATION, partial(self.blinkButton, buttonObj, True));
            self.buttonBlinkTimer.start();
class FootstepTerrainControlDialog(Plugin):

    def __init__(self, context):
        super(FootstepTerrainControlDialog, self).__init__(context)
        self.setObjectName('FootstepTerrainControlDialog')
       
        self.request_seq = 0 
        self._widget = QWidget()
        vbox = QVBoxLayout()
        
        self.cube_vbox = QVBoxLayout()
        
        self.cube_groupbox = QGroupBox( "Terrain Box" )

        self.cube_min_hbox = QHBoxLayout()

        self.cube_min_label = QLabel("Bottom Left")
        self.cube_min_hbox.addWidget( self.cube_min_label )

        self.cube_min_x = QDoubleSpinBox()
        self.cube_min_x.setSingleStep(.01)
        self.cube_min_x.setRange(-10.0, 10.0)
        self.cube_min_x.setValue(-3.0)
        self.cube_min_hbox.addWidget(self.cube_min_x)

        self.cube_min_y = QDoubleSpinBox()
        self.cube_min_y.setSingleStep(.01)
        self.cube_min_y.setRange(-10.0, 10.0)
        self.cube_min_y.setValue(-3.0)
        self.cube_min_hbox.addWidget(self.cube_min_y)

        self.cube_min_z = QDoubleSpinBox()
        self.cube_min_z.setSingleStep(.01)
        self.cube_min_z.setRange(-10.0, 10.0)
        self.cube_min_z.setValue(-1.0)
        self.cube_min_hbox.addWidget(self.cube_min_z)

        self.cube_vbox.addLayout( self.cube_min_hbox )
        
        self.cube_max_hbox = QHBoxLayout()

        self.cube_max_label = QLabel("Top Right")
        self.cube_max_hbox.addWidget( self.cube_max_label )

        self.cube_max_x = QDoubleSpinBox()
        self.cube_max_x.setSingleStep(.01)
        self.cube_max_x.setRange(-10.0, 10.0)
        self.cube_max_x.setValue(3.0)
        self.cube_max_hbox.addWidget(self.cube_max_x)

        self.cube_max_y = QDoubleSpinBox()
        self.cube_max_y.setSingleStep(.01)
        self.cube_max_y.setRange(-10.0, 10.0)
        self.cube_max_y.setValue(3.0)
        self.cube_max_hbox.addWidget(self.cube_max_y)

        self.cube_max_z = QDoubleSpinBox()
        self.cube_max_z.setSingleStep(.01)
        self.cube_max_z.setRange(-10.0, 10.0)
        self.cube_max_z.setValue(1.0)
        self.cube_max_hbox.addWidget(self.cube_max_z)

        self.cube_vbox.addLayout( self.cube_max_hbox )
    
        self.cube_groupbox.setLayout( self.cube_vbox )
        vbox.addWidget( self.cube_groupbox )

        self.type_hbox = QHBoxLayout()
  
#        self.type_label = QLabel( "Type" )
#        self.type_hbox.addWidget( self.type_label )
#
#        self.type_combobox = QComboBox()
#
#        self.type_combobox.addItem( "Type 1" )
#        self.type_combobox.addItem( "Type 2" )       
#        self.type_combobox.addItem( "Type 3" ) 
#        
#        self.type_hbox.addWidget( self.type_combobox )
#       
#        vbox.addLayout( self.type_hbox )

        self.scan_number_hbox = QHBoxLayout()
        
        self.scan_number_label = QLabel("Number of Scans Used")
        self.scan_number_hbox.addWidget( self.scan_number_label )

        self.scan_number_val = QDoubleSpinBox()
        self.scan_number_val.setDecimals(0)
        self.scan_number_val.setRange(0,10000)
        self.scan_number_val.setValue(2000)
        self.scan_number_hbox.addWidget(self.scan_number_val)

        vbox.addLayout( self.scan_number_hbox )

#       self.use_terrain_checkbox = QCheckBox( "Use Terrain" )
#       vbox.addWidget( self.use_terrain_checkbox )


        button_hbox = QHBoxLayout()
        
#        button_get = QPushButton("Get Current Values")
#        button_hbox.addWidget( button_get )

        button_submit = QPushButton("Send Values")
        button_hbox.addWidget( button_submit)

        vbox.addLayout( button_hbox )

        vbox.addStretch(1)

        self._widget.setLayout(vbox)

        context.add_widget(self._widget)

        # publishers and subscribers
        self.terrain_pub = rospy.Publisher("/flor/terrain_classifier/generate_terrain_model", TerrainModelRequest, queue_size=10 )
        button_submit.pressed.connect(self.sendParams)

#        self.terrain_sub = self.stateSubscriber   = rospy.Subscriber('/ros_footstep_planner/terrains', FootstepPlannerParams, self.getParamCallbackFcn)
#        button_submit.pressed.connect(self.getParams)


    def shutdown_plugin(self):
        print "Shutdown ..." 
        print "Done!"
    
#    def getParams( self ):
#        msg = FootstepPlannerParams()
#
#        
#        self.terrain_pub.publish( msg ) 

    def sendParams( self ):
        msg = TerrainModelRequest()

        msg.use_default_region_request = False

        rr = EnvironmentRegionRequest()

        rr.header.seq = self.request_seq
        self.request_seq = self.request_seq+1

        rr.header.stamp = rospy.get_rostime()
        rr.header.frame_id = "/world"

        rr.bounding_box_min.x = self.cube_min_x.value()
        rr.bounding_box_min.y = self.cube_min_y.value()
        rr.bounding_box_min.z = self.cube_min_z.value()
        
        rr.bounding_box_max.x = self.cube_max_x.value()
        rr.bounding_box_max.y = self.cube_max_y.value()
        rr.bounding_box_max.z = self.cube_max_z.value()

        rr.resolution = 0
        rr.request_augment = 0

        msg.region_req = rr

        msg.aggregation_size = self.scan_number_val.value() 

        print msg
        self.terrain_pub.publish( msg ) 


    def getParamCallbackFcn(self, msg):
        print "Not Defined Yet"  
Пример #5
0
class SensorParamControlDialog(Plugin):

    def __init__(self, context):
        super(SensorParamControlDialog, self).__init__(context)
        self.setObjectName('SensorParamControlDialog')
        
        self._widget = QWidget()
        vbox = QVBoxLayout()


        ### Multisense ###

        ms_groupbox = QGroupBox( "Multisense" )
        
        ms_vbox = QVBoxLayout()
        
        ms_gain_hbox = QHBoxLayout()
       
        self.ms_gain_label = QLabel("Image Gain [1, 1, 8]")
        ms_gain_hbox.addWidget( self.ms_gain_label )
        
        self.ms_gain = QDoubleSpinBox()
        self.ms_gain.setSingleStep(.01)
        self.ms_gain.setRange(1,8)

        ms_gain_hbox.addWidget( self.ms_gain ) 
    
        ms_vbox.addLayout( ms_gain_hbox )       

        
        ms_exp_hbox = QHBoxLayout()
        
        self.ms_exp_auto = QCheckBox("Image Exposure [.03, 0.5]")
        ms_exp_hbox.addWidget( self.ms_exp_auto )
        
        self.ms_exp = QDoubleSpinBox()
        self.ms_exp.setSingleStep( .001 )
        self.ms_exp.setRange( .025,.5 ) 

        ms_exp_hbox.addWidget( self.ms_exp ) 
    
        ms_vbox.addLayout( ms_exp_hbox )       
        
        ms_spindle_hbox = QHBoxLayout()
        
        ms_spindle_label = QLabel("Spindle Speed [0, 5.2]")        
        
        ms_spindle_hbox.addWidget( ms_spindle_label )

        self.ms_spindle = QDoubleSpinBox()
        self.ms_spindle.setSingleStep(.01)
        self.ms_spindle.setRange( 0,15.2 )

        ms_spindle_hbox.addWidget( self.ms_spindle ) 
    
        ms_vbox.addLayout( ms_spindle_hbox )       

        ms_light_hbox = QHBoxLayout()
        
        ms_light_label = QLabel("Light Brightness")
        ms_light_hbox.addWidget(ms_light_label)

        self.ms_light = QSlider(Qt.Horizontal)
        self.ms_light.setRange(0,100)
    
        ms_light_hbox.addWidget( self.ms_light )

        ms_vbox.addLayout( ms_light_hbox )

        ms_button_hbox = QHBoxLayout()

        ms_button_hbox.addStretch(1)        

        ms_button_get = QPushButton("Get Settings")
        ms_button_get.pressed.connect(self.ms_get_callback)
        #ms_button_hbox.addWidget( ms_button_get )
        
        ms_button_set = QPushButton("Set Settings")
        ms_button_set.pressed.connect(self.ms_set_callback)
        ms_button_hbox.addWidget( ms_button_set )

        ms_vbox.addLayout( ms_button_hbox ) 

        ms_groupbox.setLayout( ms_vbox )

        vbox.addWidget( ms_groupbox )


        ### Left SA ###

        sa_left_groupbox = QGroupBox( "Left SA Camera" )
        
        sa_left_vbox = QVBoxLayout()
        
        sa_left_gain_hbox = QHBoxLayout()
        
        sa_left_gain_label = QLabel("Image Gain [0, 0, 25]")        
        
        sa_left_gain_hbox.addWidget( sa_left_gain_label )

        self.sa_left_gain = QDoubleSpinBox()
        self.sa_left_gain.setSingleStep(.01)
        self.sa_left_gain.setRange( 0, 25 )

        sa_left_gain_hbox.addWidget( self.sa_left_gain ) 
    
        sa_left_vbox.addLayout( sa_left_gain_hbox )       

        
        sa_left_exp_hbox = QHBoxLayout()
        
        sa_left_exp_label = QLabel("Image Shutter [0, 5.5, 25]")        
        
        sa_left_exp_hbox.addWidget( sa_left_exp_label )

        self.sa_left_exp = QDoubleSpinBox()
        self.sa_left_exp.setSingleStep(.01)
        self.sa_left_exp.setRange( 0, 25 )

        sa_left_exp_hbox.addWidget( self.sa_left_exp ) 
    
        sa_left_vbox.addLayout( sa_left_exp_hbox )       
        
        sa_left_button_hbox = QHBoxLayout()

        sa_left_button_hbox.addStretch(1)        

        sa_left_button_get = QPushButton("Get Settings")
        sa_left_button_get.pressed.connect(self.sa_left_get_callback)
        #sa_left_button_hbox.addWidget( sa_left_button_get )
        
        sa_left_button_set = QPushButton("Set Settings")
        sa_left_button_set.pressed.connect(self.sa_left_set_callback)
        sa_left_button_hbox.addWidget( sa_left_button_set )

        sa_left_vbox.addLayout( sa_left_button_hbox ) 

        sa_left_groupbox.setLayout( sa_left_vbox )

        vbox.addWidget(sa_left_groupbox)
        
        ### Right SA ###

        sa_right_groupbox = QGroupBox( "Right SA Camera" )
        
        sa_right_vbox = QVBoxLayout()
        
        sa_right_gain_hbox = QHBoxLayout()
        
        sa_right_gain_label = QLabel("Image Gain [0, 0, 25]")        
        
        sa_right_gain_hbox.addWidget( sa_right_gain_label )

        self.sa_right_gain = QDoubleSpinBox()
        self.sa_right_gain.setSingleStep(.01)
        self.sa_right_gain.setRange( 0, 25 )

        sa_right_gain_hbox.addWidget( self.sa_right_gain ) 
    
        sa_right_vbox.addLayout( sa_right_gain_hbox )       

        
        sa_right_exp_hbox = QHBoxLayout()
        
        sa_right_exp_label = QLabel("Image Shutter [0, 5.5, 25]")        
        
        sa_right_exp_hbox.addWidget( sa_right_exp_label )

        self.sa_right_exp = QDoubleSpinBox()
        self.sa_right_exp.setSingleStep(.01)
        self.sa_right_exp.setRange( 0, 25 )    
    
        sa_right_exp_hbox.addWidget( self.sa_right_exp ) 
    
        sa_right_vbox.addLayout( sa_right_exp_hbox )       
        
        sa_right_button_hbox = QHBoxLayout()

        sa_right_button_hbox.addStretch(1)        

        sa_right_button_get = QPushButton("Get Settings")
        sa_right_button_get.pressed.connect(self.sa_right_get_callback)
        #sa_right_button_hbox.addWidget( sa_right_button_get )
        
        sa_right_button_set = QPushButton("Set Settings")
        sa_right_button_set.pressed.connect(self.sa_right_set_callback)
        sa_right_button_hbox.addWidget( sa_right_button_set )

        sa_right_vbox.addLayout( sa_right_button_hbox ) 

        sa_right_groupbox.setLayout( sa_right_vbox )

        vbox.addWidget(sa_right_groupbox)

        vbox.addStretch(1)

        self._widget.setLayout(vbox)

        context.add_widget(self._widget)

        # publishers and subscribers

        self.ms_cam_pub = rospy.Publisher( '/multisense_sl/set_cam_parameters', Float64MultiArray, queue_size=10)
        self.ms_light_pub = rospy.Publisher( '/multisense_sl/set_light_brightness', Float64, queue_size=10)
        self.ms_spindle_pub = rospy.Publisher( '/multisense_sl/set_spindle_speed', Float64, queue_size=10)

        self.sa_left_cam_pub = rospy.Publisher( '/sa/left/set_cam_parameters', Float64MultiArray, queue_size=10)
        self.sa_right_cam_pub = rospy.Publisher( '/sa/right/set_cam_parameters', Float64MultiArray, queue_size=10)


 #       self.param_sub = self.stateSubscriber   = rospy.Subscriber('/ros_footstep_planner/params', FootstepPlannerParams, self.getParamCallbackFcn)


    def shutdown_plugin(self):
        print "Shutdown ..." 
#        self.param_pub.unregister()
        print "Done!"

    def ms_get_callback(self):
        print "fill values"

    def ms_set_callback(self):
        ms_cam = Float64MultiArray()
        
        ms_cam_dim_h = MultiArrayDimension()
        ms_cam_dim_h.label = "height"
        ms_cam_dim_h.size = 1
        ms_cam_dim_h.stride  = 2        

        ms_cam.layout.dim.append( ms_cam_dim_h )
        
        ms_cam_dim_w = MultiArrayDimension()
        ms_cam_dim_w.label = "width"
        ms_cam_dim_w.size = 2
        ms_cam_dim_w.stride  = 2        
        
        ms_cam.layout.dim.append(ms_cam_dim_w)

        ms_cam.data.append( self.ms_gain.value() )

        if self.ms_exp_auto.isChecked() :
            ms_cam.data.append( 1 )   
        else:
            ms_cam.data.append( self.ms_exp.value() )
        
        self.ms_cam_pub.publish( ms_cam )
    

        light  = Float64()
        light.data = self.ms_light.value()/100.0

        self.ms_light_pub.publish( light )

        spindle_speed = Float64()
        spindle_speed.data = self.ms_spindle.value()

        self.ms_spindle_pub.publish( spindle_speed )
        
    def sa_left_get_callback(self):
        print "fill values"

    def sa_left_set_callback(self):
        sa_left_cam = Float64MultiArray()
        
        sa_left_cam_dim_h = MultiArrayDimension()
        sa_left_cam_dim_h.label = "height"
        sa_left_cam_dim_h.size = 1
        sa_left_cam_dim_h.stride  = 2        

        sa_left_cam.layout.dim.append( sa_left_cam_dim_h )
        
        sa_left_cam_dim_w = MultiArrayDimension()
        sa_left_cam_dim_w.label = "width"
        sa_left_cam_dim_w.size = 2
        sa_left_cam_dim_w.stride  = 2        
        
        sa_left_cam.layout.dim.append(sa_left_cam_dim_w)

        sa_left_cam.data.append( self.sa_left_gain.value() )
        sa_left_cam.data.append( self.sa_left_exp.value() )
        
        self.sa_left_cam_pub.publish( sa_left_cam )
    
    def sa_right_get_callback(self):
        print "fill values"

    def sa_right_set_callback(self):
        sa_right_cam = Float64MultiArray()
        
        sa_right_cam_dim_h = MultiArrayDimension()
        sa_right_cam_dim_h.label = "height"
        sa_right_cam_dim_h.size = 1
        sa_right_cam_dim_h.stride  = 2        

        sa_right_cam.layout.dim.append( sa_right_cam_dim_h )
        
        sa_right_cam_dim_w = MultiArrayDimension()
        sa_right_cam_dim_w.label = "width"
        sa_right_cam_dim_w.size = 2
        sa_right_cam_dim_w.stride  = 2        
        
        sa_right_cam.layout.dim.append(sa_right_cam_dim_w)

        sa_right_cam.data.append( self.sa_right_gain.value() )
        sa_right_cam.data.append( self.sa_right_exp.value() )
        
        self.sa_right_cam_pub.publish( sa_right_cam )