def generate_q_double_spin_box(default_val, range_min, range_max, decimals, single_step):
    spin_box = QDoubleSpinBox()
    spin_box.setValue(default_val)
    spin_box.setRange(range_min, range_max)
    spin_box.setDecimals(decimals)
    spin_box.setSingleStep(single_step)
    #spin_box.valueChanged[unicode].connect(self.callback_spin_box)
    return spin_box
def generate_q_double_spin_box(default_val, range_min, range_max, decimals,
                               single_step):
    spin_box = QDoubleSpinBox()
    spin_box.setValue(default_val)
    spin_box.setRange(range_min, range_max)
    spin_box.setDecimals(decimals)
    spin_box.setSingleStep(single_step)
    #spin_box.valueChanged[unicode].connect(self.callback_spin_box)
    return spin_box
Exemplo n.º 3
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 )
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"  
Exemplo n.º 6
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 SinusoidalTrajectoryDialog(Plugin):
    updateStateSignal = Signal(object)

    def __init__(self, context):
        super(SinusoidalTrajectoryDialog, self).__init__(context)
        self.setObjectName('SinusoidalTrajectoryDialog')
        #self.updateStateSignal = Signal(object)
        self.updateStateSignal.connect(self.on_updateState)

        self.robot = URDF.from_parameter_server()
        self.joint_list = {}

    	for ndx,jnt in enumerate(self.robot.joints):
            self.joint_list[jnt.name] = ndx
            #print jnt.name, " ",self.joint_list[jnt.name]

        self.chain=[]
        self.chain_file = rospy.get_param("chain_file")
        self.chain_name = rospy.get_param("chain_name")

        yaml_file = self.chain_file+self.chain_name+"_chain.yaml"
        print yaml_file

        #define structures
        self.robot_state = JointState()
        self.robot_command = JointTrajectory()

        stream = open(yaml_file, "r")
        jointChain = yaml.load_all(stream)
        print '\n\n'
        for ndx, data in enumerate(jointChain):
            print ndx," : ", data
            self.delay_time          = data["delay_time"]
            self.amplitude           = data["amplitude"]
            self.frequency           = data["frequency"]
            self.frequency_limit     = data["frequency_limit"]
            self.iterations          = data["iterations"]
            self.joint_state_topic   = data["joint_state_topic"]
            self.trajectory_topic    = data["trajectory_topic"]

            joints = rospy.get_param(data["chain_param_name"])
            for joint in joints:
                print joint
                self.robot_state.name.append(joint)
                self.chain.append(JointData(self, joint) )
        self.robot_command.joint_names = self.robot_state.name

        stream.close()


        self.robot_state.position = [0.0]*len(self.robot_state.name)
        self.robot_state.velocity = [0.0]*len(self.robot_state.name)
        self.robot_state.effort   = [0.0]*len(self.robot_state.name)
        self.robot_joint_state = JointState()

        print "delay_time  =",self.delay_time
        print "amplitude   =",self.amplitude
        print "frequency   =",self.frequency
        print "iterations  =",self.iterations

        print "Robot State Structure",self.robot_state
        print "Robot Command Structure",self.robot_command

        # initialize structure to hold widget handles
        self.cur_position_spinbox=[]
        self.amplitude_spinbox=[]
        self.frequency_spinbox=[]
        self.iterations_spinbox=[]

        self._widget = QWidget()
        vbox = QVBoxLayout()

        # Push buttons
        hbox = QHBoxLayout()

        snap_command = QPushButton("Snap Position")
        snap_command.clicked.connect(self.snap_current_callback)
        hbox.addWidget(snap_command)

        check_limits = QPushButton("Check Limits Gains")
        check_limits.clicked.connect(self.check_limits_callback)
        hbox.addWidget(check_limits)

        apply_command = QPushButton("Send Trajectory")
        apply_command.clicked.connect(self.apply_command_callback)
        hbox.addWidget(apply_command)

        save_trajectory = QPushButton("Save Trajectory")
        save_trajectory.clicked.connect(self.save_trajectory_callback)
        hbox.addWidget(save_trajectory)

        zero_ramp = QPushButton("Zero Values")
        zero_ramp.clicked.connect(self.zero_values_callback)
        hbox.addWidget(zero_ramp)

        vbox.addLayout(hbox)

        time_hbox = QHBoxLayout()

        vbox_frequqency = QVBoxLayout()
        vbox_frequqency.addWidget(QLabel("Frequency"))
        self.frequency_spinbox = QDoubleSpinBox()
        self.frequency_spinbox.setDecimals(5)
        self.frequency_spinbox.setRange(0, self.frequency_limit)
        self.frequency_spinbox.setSingleStep(0.05)
        self.frequency_spinbox.valueChanged.connect(self.on_frequency_value)
        self.frequency_spinbox.setValue(self.frequency)
        vbox_frequqency.addWidget(self.frequency_spinbox)
        time_hbox.addLayout(vbox_frequqency)

        vbox_iterations = QVBoxLayout()
        vbox_iterations.addWidget(QLabel("Iterations"))
        self.iterations_spinbox = QDoubleSpinBox()
        self.iterations_spinbox.setDecimals(5)
        self.iterations_spinbox.setRange(0, 10)
        self.iterations_spinbox.setSingleStep(1)
        self.iterations_spinbox.valueChanged.connect(self.on_iterations_value)
        self.iterations_spinbox.setValue(self.iterations)
        vbox_iterations.addWidget(self.iterations_spinbox)
        time_hbox.addLayout(vbox_iterations)

        vbox.addLayout(time_hbox)

        # Joints title
        title_frame = QFrame()
        title_frame.setFrameShape(QFrame.StyledPanel);
        title_frame.setFrameShadow(QFrame.Raised);

        title_hbox = QHBoxLayout()
        title_hbox.addWidget(QLabel("Joints"))
        title_hbox.addWidget(QLabel("Current Position"))
        title_hbox.addWidget(QLabel("Amplitude"))
        title_frame.setLayout(title_hbox)
        vbox.addWidget(title_frame)


        # Define the widgets for each joint
        for i,joint in enumerate(self.chain):
            #print i,",",joint
            self.joint_widget( vbox, i)


        #add stretch at end so all GUI elements are at top of dialog
        vbox.addStretch(1)

        self._widget.setLayout(vbox)


        # Define the connections to the outside world
        self.jointSubscriber  = rospy.Subscriber(self.joint_state_topic, JointState, self.stateCallbackFnc)
        self.commandPublisher = rospy.Publisher(self.trajectory_topic  , JointTrajectory, queue_size=10)

        # Add the widget
        context.add_widget(self._widget)

    @Slot()
    def snap_current_callback(self):
        self.blockSignals(True)
        print "Snapping positions to actual values"
        for index, joint in enumerate(self.chain):
            for index_state, name_state in enumerate(self.robot_joint_state.name):
                if (name_state == joint.name):
                    joint.position = copy.deepcopy(self.robot_joint_state.position[index_state])
                    self.cur_position_spinbox[index].setValue(joint.position)
                    break

        self.blockSignals(False)

    @Slot()
    def check_limits_callback(self):
        self.blockSignals(True)
        print "Check limits callback ..."
        valid = True
        for index, joint in enumerate(self.chain):

            ramp_up   = joint.position + joint.amplitude
            ramp_down = joint.position - joint.amplitude

            p = self.amplitude_spinbox[index].palette()
            if (ramp_up > joint.upper_limit) or (ramp_up < joint.lower_limit):
                print "Joint ",joint.name, " is beyond upper limit!"
                valid=False
                p.setColor(QPalette.Window, Qt.red)  # <<<<<<<<<<<----------------- This isn't working as intended
            else:
                p.setColor(QPalette.Window, Qt.white)# <<<<<<<<<<<----------------- This isn't working as intended
                #print joint.lower_limit," < ", ramp_up, " < ",joint.upper_limit

            self.amplitude_spinbox[index].setPalette(p)

            if (ramp_down > joint.upper_limit) or (ramp_down < joint.lower_limit):
                print "Joint ",joint.name, " is beyond lower limit!"
                valid=False
                p.setColor(QPalette.Window, Qt.red)  # <<<<<<<<<<<----------------- This isn't working as intended
            else:
                p.setColor(QPalette.Window, Qt.white)# <<<<<<<<<<<----------------- This isn't working as intended
                #print joint.lower_limit," < ", ramp_down, " < ",joint.upper_limit

        if (self.frequency > self.frequency_limit or self.frequency <= 0):
            print "invalid frequency. must be between 0 and ", self.frequency_limit, ". value is: ", self.frequency
            valid = False

        if (not valid):
            print "ERROR: Invalid input!"
        else:
            print "   Valid values!"

        self.blockSignals(False)
        return valid

    @Slot()
    def zero_values_callback(self):
        self.blockSignals(True)
        print "Zero ramps callback ..."
        for index, joint in enumerate(self.chain):
            self.amplitude_spinbox[index].setValue(0.0)
        self.blockSignals(False)

    @Slot()
    def apply_command_callback(self):
        self.blockSignals(True)
        print "Send trajectory"

        if self.calculate_trajectory():
            #print self.robot_command;
            self.commandPublisher.publish(self.robot_command)
        else:
            print "Trajectory calculation failure - do not publish!"

        self.blockSignals(False)

    @Slot()
    def save_trajectory_callback(self):
        self.blockSignals(True)
        print "Save gains"
        # Save data to file that we can read in
        # TODO - invalid file names

        fileName = QFileDialog.getSaveFileName()

        #if fileName[0] checks to ensure that the file dialog has not been canceled
        if fileName[0]:
            if self.calculate_trajectory():
                print self.robot_command;
                newFileptr = open(fileName[0], 'w')
                # Suggested format for files to make it easy to combine different outputs
                newFileptr.write('# Trajectory \n')
                newFileptr.write(self.robot_command)
                newFileptr.write('\n')
                newFileptr.close()
            else:
                print "Trajectory calculation failure - do not save!"
        else:
            print "Save cancelled."

        newFilePtr.close()

        self.blockSignals(False)

    #
    @Slot()
    def stateCallbackFnc(self, atlasState_msg):
        # Handle the state message for actual positions
        time = atlasState_msg.header.stamp.to_sec()
        if ((time - self.prior_time) > 0.02):
            # Only update at 50hz
            # relay the ROS messages through a Qt Signal to switch to the GUI thread
            self.updateStateSignal.emit(atlasState_msg)
            self.prior_time = time

    # this runs in the Qt GUI thread and can therefore update the GUI
    def on_updateState(self, joint_state_msg):
        #print joint_state_msg
        self.robot_joint_state = joint_state_msg

    def on_delay_time_value(self, value):
        self.delay_time = copy.deepcopy(value)

    def on_frequency_value(self, value):
        self.frequency = copy.deepcopy(value)

    def on_iterations_value(self, value):
        self.iterations = copy.deepcopy(value)


    def calculate_trajectory(self):

        if not self.check_limits_callback():
            print "Invalid limits for trajectory!"
            return False

        knot_points = 8*self.iterations
        if (knot_points < 2):
            print "Invalid trajectory - knot_points = ",knot_points
            return False

        #print "Knot points=",knot_points
        self.robot_command.points = []
        self.robot_command.points.append(JointTrajectoryPoint())

        ndx = 0
        self.robot_command.points[ndx].time_from_start = rospy.Duration(0.0)
        for jnt, joint in enumerate(self.chain):
            self.robot_command.points[ndx].positions.append(joint.position)
            self.robot_command.points[ndx].velocities.append(0.0)
            self.robot_command.points[ndx].accelerations.append(0.0)
        ndx += 1


        dt = np.pi / (4.0 * self.frequency)
        time_offset = dt

        print "dt = ", dt
        while ndx < knot_points-1:
            self.robot_command.points.append(JointTrajectoryPoint())
            time_offset += dt
            print " time = ", time_offset
            self.robot_command.points[ndx].time_from_start = rospy.Duration(time_offset)
            for jnt, joint in enumerate(self.chain):
                self.robot_command.points[ndx].positions.append(joint.position + joint.amplitude * np.sin(self.frequency * time_offset))
                self.robot_command.points[ndx].velocities.append(self.frequency * joint.amplitude * np.cos(self.frequency * time_offset))
                self.robot_command.points[ndx].accelerations.append(-self.frequency * self.frequency * joint.amplitude * np.sin(self.frequency * time_offset))
            ndx += 1

            
        #end position
        self.robot_command.points.append(JointTrajectoryPoint())
        time_offset += dt
        self.robot_command.points[ndx].time_from_start = rospy.Duration(time_offset)
        for jnt, joint in enumerate(self.chain):
            self.robot_command.points[ndx].positions.append(joint.position)
            self.robot_command.points[ndx].velocities.append(0.0)
            self.robot_command.points[ndx].accelerations.append(0.0)

        print self.robot_command
        print "Ndx=",ndx, " of ",knot_points, "=",len(self.robot_command.points)
        if (ndx != len(self.robot_command.points)-1) or (len(self.robot_command.points) != knot_points):
            print "Invalid number of knot points - ignore trajectory"
            print self.robot_command
            return False

        self.robot_command.header.stamp = rospy.Time.now() + rospy.Duration(0.1)

        return True;



    def shutdown_plugin(self):
    #Just make sure to stop timers and publishers, unsubscribe from Topics etc in the shutdown_plugin method.
    #TODO, is the above comment why the plugin gets stuck when we try to shutdown?
        print "Shutdown ..."
        rospy.sleep(0.1)
        self.jointSubscriber.unregister()
        self.commandPublisher.unregister()
        rospy.sleep(0.5)
        print "Done!"


    # Define collection of widgets for joint group
    def joint_widget( self, main_vbox, index):
        joint = self.chain[index]
        frame = QFrame()
        frame.setFrameShape(QFrame.StyledPanel);
        frame.setFrameShadow(QFrame.Raised);

        hbox = QHBoxLayout()
        #hbox.addWidget(frame)

        self.prior_time = 0.0

        robot_joint = self.robot.joints[self.joint_list[joint.name]]
        joint.lower_limit = robot_joint.limit.lower
        joint.upper_limit = robot_joint.limit.upper
        amplitude_range = (robot_joint.limit.upper-robot_joint.limit.lower)
        frequency_range = self.frequency_limit
        iterations_range = 10000

        print "  ",joint.name, "  limits(", joint.lower_limit,", ",joint.upper_limit,") range=",amplitude_range

        self.cur_position_spinbox.append(QDoubleSpinBox())
        self.cur_position_spinbox[index].setDecimals(5)
        self.cur_position_spinbox[index].setRange(joint.lower_limit, joint.upper_limit)
        self.cur_position_spinbox[index].setSingleStep((robot_joint.limit.upper-robot_joint.limit.lower)/50.0)
        self.cur_position_spinbox[index].valueChanged.connect(joint.on_position_value)

        self.amplitude_spinbox.append(QDoubleSpinBox())
        self.amplitude_spinbox[index].setDecimals(5)
        self.amplitude_spinbox[index].setRange(-amplitude_range, amplitude_range)
        self.amplitude_spinbox[index].setSingleStep(amplitude_range/50.0)
        self.amplitude_spinbox[index].valueChanged.connect(joint.on_amplitude_value)
        self.amplitude_spinbox[index].setValue(joint.amplitude)

        hbox.addWidget(QLabel(joint.name))
        hbox.addWidget(self.cur_position_spinbox[index])
        hbox.addWidget(self.amplitude_spinbox[index])

        # Add horizontal layout to frame for this joint group
        frame.setLayout(hbox)

        # Add frame to main vertical layout
        main_vbox.addWidget(frame)
class TrapezoidalTrajectoryDialog(Plugin):
    updateStateSignal = Signal(object)

    def __init__(self, context):
        super(TrapezoidalTrajectoryDialog, self).__init__(context)
        self.setObjectName("TrapezoidalTrajectoryDialog")
        # self.updateStateSignal = Signal(object)
        self.updateStateSignal.connect(self.on_updateState)

        self.robot = URDF.from_parameter_server()
        self.joint_list = {}

        for ndx, jnt in enumerate(self.robot.joints):
            self.joint_list[jnt.name] = ndx

        self.chain = []
        self.chain_file = rospy.get_param("~chain_file")
        self.chain_name = rospy.get_param("~chain_name")

        print
        print "Define order of splines used to approximate trapezoid"
        self.spline_order = rospy.get_param("~spline_order", 5)  # 1 # 3 # 5 # linear, cubic, quintic
        if (self.spline_order == 1) or (self.spline_order == 3) or (self.spline_order == 5):
            print "Spline order=", self.spline_order
        else:
            print "Invalid spline order!  Must be 1, 3, or 5"
            print "Spline order=", self.spline_order
            sys.exit(-1)

        yaml_file = self.chain_file + self.chain_name + "_chain.yaml"
        print "Chain definition file:"
        print yaml_file
        print

        # define structures
        self.robot_state = JointState()
        self.robot_command = JointTrajectory()

        stream = open(yaml_file, "r")
        jointChain = yaml.load_all(stream)

        for ndx, data in enumerate(jointChain):
            print ndx, " : ", data
            self.delay_time = data["delay_time"]
            self.ramp_up_time = data["ramp_up_time"]
            self.dwell_time = data["dwell_time"]
            self.ramp_down_time = data["ramp_down_time"]
            self.hold_time = data["hold_time"]
            self.ramp_start_fraction = data["ramp_start_fraction"]
            self.ramp_end_fraction = data["ramp_end_fraction"]

            self.joint_state_topic = data["joint_state_topic"]
            self.trajectory_topic = data["trajectory_topic"]

            if rospy.search_param(data["chain_param_name"]):
                print "Found ", data["chain_param_name"]
            else:
                print "Failed to find the ", data["chain_param_name"], " in the parameter server!"
                sys.exit(-1)

            joint_names = rospy.get_param(data["chain_param_name"])
            for joint in joint_names:
                print joint
                self.robot_state.name.append(joint)
                self.chain.append(JointData(self, joint))

        self.robot_command.joint_names = self.robot_state.name

        stream.close()

        self.robot_state.position = [0.0] * len(self.robot_state.name)
        self.robot_state.velocity = [0.0] * len(self.robot_state.name)
        self.robot_state.effort = [0.0] * len(self.robot_state.name)
        self.robot_joint_state = JointState()

        print "delay_time    =", self.delay_time
        print "ramp_up_time  =", self.ramp_up_time
        print "dwell_time    =", self.dwell_time
        print "ramp_down_time=", self.ramp_down_time
        print "hold_time     =", self.hold_time
        print "spline order  =", self.spline_order

        if (
            (self.ramp_start_fraction < 0.001)
            or (self.ramp_end_fraction > 0.999)
            or (self.ramp_start_fraction >= self.ramp_end_fraction)
        ):
            print "Invalid ramp fractions - abort!"
            print "0.0 < ", self.ramp_start_fraction, " < ", self.ramp_end_fraction, " < 1.0"
            return

        print "Robot State Structure", self.robot_state
        print "Robot Command Structure", self.robot_command

        # initialize structure to hold widget handles
        self.cmd_spinbox = []
        self.ramp_up_spinbox = []
        self.ramp_down_spinbox = []

        self._widget = QWidget()
        vbox = QVBoxLayout()

        # Push buttons
        hbox = QHBoxLayout()

        snap_command = QPushButton("Snap Position")
        snap_command.clicked.connect(self.snap_current_callback)
        hbox.addWidget(snap_command)

        check_limits = QPushButton("Check Limits Gains")
        check_limits.clicked.connect(self.check_limits_callback)
        hbox.addWidget(check_limits)

        apply_command = QPushButton("Send Trajectory")
        apply_command.clicked.connect(self.apply_command_callback)
        hbox.addWidget(apply_command)

        save_trajectory = QPushButton("Save Trajectory")
        save_trajectory.clicked.connect(self.save_trajectory_callback)
        hbox.addWidget(save_trajectory)

        zero_ramp = QPushButton("Zero Ramps")
        zero_ramp.clicked.connect(self.zero_ramp_callback)
        hbox.addWidget(zero_ramp)

        vbox.addLayout(hbox)

        time_hbox = QHBoxLayout()

        vbox_delay = QVBoxLayout()
        vbox_delay.addWidget(QLabel("Delay"))
        self.delay_time_spinbox = QDoubleSpinBox()
        self.delay_time_spinbox.setDecimals(5)
        self.delay_time_spinbox.setRange(0, 10.0)
        self.delay_time_spinbox.setSingleStep(0.1)
        self.delay_time_spinbox.valueChanged.connect(self.on_delay_time_value)
        self.delay_time_spinbox.setValue(self.delay_time)
        vbox_delay.addWidget(self.delay_time_spinbox)
        time_hbox.addLayout(vbox_delay)

        vbox_ramp_up = QVBoxLayout()
        vbox_ramp_up.addWidget(QLabel("Ramp Up"))
        self.ramp_up_time_spinbox = QDoubleSpinBox()
        self.ramp_up_time_spinbox.setDecimals(5)
        self.ramp_up_time_spinbox.setRange(0, 10.0)
        self.ramp_up_time_spinbox.setSingleStep(0.1)
        self.ramp_up_time_spinbox.valueChanged.connect(self.on_ramp_up_time_value)
        self.ramp_up_time_spinbox.setValue(self.ramp_up_time)
        vbox_ramp_up.addWidget(self.ramp_up_time_spinbox)
        time_hbox.addLayout(vbox_ramp_up)

        #
        vbox_dwell = QVBoxLayout()
        vbox_dwell.addWidget(QLabel("Dwell"))
        self.dwell_time_spinbox = QDoubleSpinBox()
        self.dwell_time_spinbox.setDecimals(5)
        self.dwell_time_spinbox.setRange(0, 10.0)
        self.dwell_time_spinbox.setSingleStep(0.1)
        self.dwell_time_spinbox.valueChanged.connect(self.on_dwell_time_value)
        self.dwell_time_spinbox.setValue(self.dwell_time)
        vbox_dwell.addWidget(self.dwell_time_spinbox)
        time_hbox.addLayout(vbox_dwell)

        vbox_ramp_down = QVBoxLayout()
        vbox_ramp_down.addWidget(QLabel("Down"))
        self.ramp_down_time_spinbox = QDoubleSpinBox()
        self.ramp_down_time_spinbox.setDecimals(5)
        self.ramp_down_time_spinbox.setRange(0, 10.0)
        self.ramp_down_time_spinbox.setSingleStep(0.1)
        self.ramp_down_time_spinbox.valueChanged.connect(self.on_ramp_down_time_value)
        self.ramp_down_time_spinbox.setValue(self.ramp_down_time)
        vbox_ramp_down.addWidget(self.ramp_down_time_spinbox)
        time_hbox.addLayout(vbox_ramp_down)

        vbox_hold = QVBoxLayout()
        vbox_hold.addWidget(QLabel("Hold"))
        self.hold_time_spinbox = QDoubleSpinBox()
        self.hold_time_spinbox.setDecimals(5)
        self.hold_time_spinbox.setRange(0, 10.0)
        self.hold_time_spinbox.setSingleStep(0.1)
        self.hold_time_spinbox.valueChanged.connect(self.on_hold_time_value)
        self.hold_time_spinbox.setValue(self.hold_time)
        vbox_hold.addWidget(self.hold_time_spinbox)
        time_hbox.addLayout(vbox_hold)

        vbox.addLayout(time_hbox)

        # Joints title
        title_frame = QFrame()
        title_frame.setFrameShape(QFrame.StyledPanel)
        title_frame.setFrameShadow(QFrame.Raised)

        title_hbox = QHBoxLayout()
        title_hbox.addWidget(QLabel("Joints"))
        title_hbox.addWidget(QLabel("Start"))
        title_hbox.addWidget(QLabel("Ramp Up"))
        title_hbox.addWidget(QLabel("Ramp Down"))
        title_frame.setLayout(title_hbox)
        vbox.addWidget(title_frame)

        # Define the widgets for each joint
        for i, joint in enumerate(self.chain):
            # print i,",",joint
            self.joint_widget(vbox, i)

        # add stretch at end so all GUI elements are at top of dialog
        vbox.addStretch(1)

        self._widget.setLayout(vbox)

        # Define the connections to the outside world
        self.jointSubscriber = rospy.Subscriber(self.joint_state_topic, JointState, self.stateCallbackFnc)
        self.commandPublisher = rospy.Publisher(self.trajectory_topic, JointTrajectory, queue_size=10)

        # Add the widget
        context.add_widget(self._widget)

    @Slot()
    def snap_current_callback(self):
        self.blockSignals(True)
        print "Snapping positions to actual values"
        for index, joint in enumerate(self.chain):
            for index_state, name_state in enumerate(self.robot_joint_state.name):
                if name_state == joint.name:
                    joint.position = copy.deepcopy(self.robot_joint_state.position[index_state])
                    self.cmd_spinbox[index].setValue(joint.position)
                    break

        self.blockSignals(False)

    @Slot()
    def check_limits_callback(self):
        self.blockSignals(True)
        print "Check limits callback ..."
        valid = True
        for index, joint in enumerate(self.chain):

            ramp_up = joint.position + joint.ramp_up
            ramp_down = joint.ramp_down + ramp_up

            p = self.ramp_up_spinbox[index].palette()
            if (ramp_up > joint.upper_limit) or (ramp_up < joint.lower_limit):
                print "Joint ", joint.name, " is over limit on ramp up!"
                valid = False
                p.setColor(QPalette.Window, Qt.red)  # <<<<<<<<<<<----------------- This isn't working as intended
            else:
                p.setColor(QPalette.Window, Qt.white)  # <<<<<<<<<<<----------------- This isn't working as intended
                # print joint.lower_limit," < ", ramp_up, " < ",joint.upper_limit

            self.ramp_up_spinbox[index].setPalette(p)

            if (ramp_down > joint.upper_limit) or (ramp_down < joint.lower_limit):
                print "Joint ", joint.name, " is over limit on ramp down!"
                valid = False
                p.setColor(QPalette.Window, Qt.red)  # <<<<<<<<<<<----------------- This isn't working as intended
            else:
                p.setColor(QPalette.Window, Qt.white)  # <<<<<<<<<<<----------------- This isn't working as intended
                # print joint.lower_limit," < ", ramp_down, " < ",joint.upper_limit

        if not valid:
            print "Invalid joint limits on ramp!"
        else:
            print "   Valid ramp!"

        self.blockSignals(False)
        return valid

    @Slot()
    def zero_ramp_callback(self):
        self.blockSignals(True)
        print "Zero ramps callback ..."
        for index, joint in enumerate(self.chain):
            self.ramp_up_spinbox[index].setValue(0.0)
            self.ramp_down_spinbox[index].setValue(0.0)
        self.blockSignals(False)

    @Slot()
    def apply_command_callback(self):
        self.blockSignals(True)
        print "Send trajectory"

        if self.calculate_trajectory():
            # print self.robot_command;
            self.commandPublisher.publish(self.robot_command)
        else:
            print "Trajectory calculation failure - do not publish!"

        self.blockSignals(False)

    @Slot()
    def save_trajectory_callback(self):
        self.blockSignals(True)
        print "Save gains"
        # Save data to file that we can read in
        # TODO - invalid file names

        fileName = QFileDialog.getSaveFileName()

        # if fileName[0] checks to ensure that the file dialog has not been canceled
        if fileName[0]:
            if self.calculate_trajectory():
                print self.robot_command
                newFileptr = open(fileName[0], "w")
                # Suggested format for files to make it easy to combine different outputs
                newFileptr.write("# Trajectory \n")
                newFileptr.write(self.robot_command)
                newFileptr.write("\n")
                newFileptr.close()
            else:
                print "Trajectory calculation failure - do not save!"
        else:
            print "Save cancelled."

        newFilePtr.close()

        self.blockSignals(False)

    #
    @Slot()
    def stateCallbackFnc(self, atlasState_msg):
        # Handle the state message for actual positions
        time = atlasState_msg.header.stamp.to_sec()
        if (time - self.prior_time) > 0.02:
            # Only update at 50hz
            # relay the ROS messages through a Qt Signal to switch to the GUI thread
            self.updateStateSignal.emit(atlasState_msg)
            self.prior_time = time

    # this runs in the Qt GUI thread and can therefore update the GUI
    def on_updateState(self, joint_state_msg):
        # print joint_state_msg
        self.robot_joint_state = joint_state_msg

    def on_delay_time_value(self, value):
        self.delay_time = copy.deepcopy(value)

    def on_ramp_up_time_value(self, value):
        self.ramp_up_time = copy.deepcopy(value)

    def on_ramp_down_time_value(self, value):
        self.ramp_down_time = copy.deepcopy(value)

    def on_dwell_time_value(self, value):
        self.dwell_time = copy.deepcopy(value)

    def on_hold_time_value(self, value):
        self.hold_time = copy.deepcopy(value)

    def calculate_trajectory(self):

        if not self.check_limits_callback():
            print "Invalid limits for trajectory!"
            return False

        knot_points = 1
        if self.delay_time > 0.002:
            knot_points += 1

        if self.ramp_up_time > 0.002:
            knot_points += 1

        if self.dwell_time > 0.002:
            knot_points += 1

        if self.ramp_down_time > 0.002:
            knot_points += 1

        if self.hold_time > 0.002:
            knot_points += 1

        if knot_points < 2:
            print "Invalid trajectory - knot_points = ", knot_points
            return False

        # print "Minimum knot points=",knot_points
        self.robot_command.points = []
        self.robot_command.points.append(JointTrajectoryPoint())

        ros_time_offset = rospy.Duration(0.0)
        ndx = 0
        self.robot_command.points[ndx].time_from_start = ros_time_offset
        for jnt, joint in enumerate(self.chain):
            self.robot_command.points[ndx].positions.append(joint.position)
            if self.spline_order > 1:
                self.robot_command.points[ndx].velocities.append(0.0)
            if self.spline_order > 3:
                self.robot_command.points[ndx].accelerations.append(0.0)
        ndx += 1

        if self.delay_time > 0.002:
            ros_time_offset += rospy.Duration(self.delay_time)
            self.robot_command.points.append(copy.deepcopy(self.robot_command.points[ndx - 1]))
            self.robot_command.points[ndx].time_from_start = ros_time_offset
            ndx += 1

        if self.ramp_up_time > 0.002:
            ramp_up_count = self.calculate_ramp_up_section(ndx)
            if ramp_up_count:
                ndx += ramp_up_count
                ros_time_offset += rospy.Duration(self.ramp_up_time)
            else:
                return False  # Invalid ramp calculation

        if self.dwell_time > 0.002:
            ros_time_offset += rospy.Duration(self.dwell_time)
            self.robot_command.points.append(copy.deepcopy(self.robot_command.points[ndx - 1]))
            self.robot_command.points[ndx].time_from_start = ros_time_offset
            ndx += 1

        if self.ramp_down_time > 0.002:
            ramp_dn_count = self.calculate_ramp_down_section(ndx)
            if ramp_dn_count > 0:
                ndx += ramp_dn_count
                ros_time_offset += rospy.Duration(self.ramp_down_time)
            else:
                return False  # Invalid ramp calculation

        if self.hold_time > 0.002:
            ros_time_offset += rospy.Duration(self.hold_time)
            self.robot_command.points.append(copy.deepcopy(self.robot_command.points[ndx - 1]))
            self.robot_command.points[ndx].time_from_start = ros_time_offset
            ndx += 1

        # print self.robot_command
        # print "Ndx=",ndx, " of ",knot_points, "=",len(self.robot_command.points)

        if ndx != len(self.robot_command.points):
            print "Invalid number of knot points - ignore trajectory"
            print "Ndx=", ndx, " of ", len(self.robot_command.points), " = ", knot_points
            print self.robot_command
            return False

        self.robot_command.header.stamp = rospy.Time.now() + rospy.Duration(0.1)

        return True

    def calculate_ramp_up_section(self, ndx):
        prior = self.robot_command.points[ndx - 1]

        # Create next 3 knot points
        self.robot_command.points.append(copy.deepcopy(prior))
        if self.spline_order > 1:
            # Add transition points
            self.robot_command.points.append(copy.deepcopy(prior))
            self.robot_command.points.append(copy.deepcopy(prior))
            dT0 = self.ramp_start_fraction * self.ramp_up_time
            dT1 = (self.ramp_end_fraction - self.ramp_start_fraction) * self.ramp_up_time
            dT2 = (1.0 - self.ramp_end_fraction) * self.ramp_up_time
            self.robot_command.points[ndx].time_from_start = prior.time_from_start + rospy.Duration(dT0)
            self.robot_command.points[ndx + 1].time_from_start = self.robot_command.points[
                ndx
            ].time_from_start + rospy.Duration(dT1)
            self.robot_command.points[ndx + 2].time_from_start = self.robot_command.points[
                ndx + 1
            ].time_from_start + rospy.Duration(dT2)
        else:
            dT0 = self.ramp_up_time
            self.robot_command.points[ndx].time_from_start = prior.time_from_start + rospy.Duration(dT0)

        # Now calculate the interior values for each joint individually
        for jnt, joint in enumerate(self.chain):
            if joint.ramp_up != 0.0:
                starting_position = copy.deepcopy(prior.positions[jnt])
                ending_position = starting_position + joint.ramp_up

                print "calculating ramp up for ", joint.name, " from ", starting_position, " to ", ending_position, " over ", self.ramp_up_time, " seconds"

                if self.spline_order > 1:
                    # Either quintic or cubic - either way we have transition -> linear -> transition
                    # for quintic,
                    #  Ax=b  where x=[a0 b0 c0 d0 e0 f0 e1 f1 a2 b2 c2 d2 e2 f2] with p=a*t^5 + b*t^4 + c*t^3 + d*t^2 + e*t +f for first and last, and p=e*t + f for middle
                    #  6 + 2 + 6 unknowns
                    x = self.solve_ramp_matrices(dT0, dT1, dT2, starting_position, ending_position)
                    # first interior point
                    self.robot_command.points[ndx].positions[jnt] = copy.deepcopy(x[7][0])
                    self.robot_command.points[ndx].velocities[jnt] = copy.deepcopy(x[6][0])
                    # linear segment
                    self.robot_command.points[ndx + 1].positions[jnt] = copy.deepcopy(x[13][0])
                    self.robot_command.points[ndx + 1].velocities[jnt] = copy.deepcopy(x[6][0])
                    # last interior point
                    self.robot_command.points[ndx + 2].positions[jnt] = ending_position
                    self.robot_command.points[ndx + 2].velocities[jnt] = 0.0
                else:
                    # Piecewise linear
                    self.robot_command.points[ndx].positions[jnt] = copy.deepcopy(ending_position)

                if self.spline_order > 3:
                    # Quintic spline
                    self.robot_command.points[ndx].accelerations[jnt] = 0.0
                    self.robot_command.points[ndx + 1].accelerations[jnt] = 0.0
                    self.robot_command.points[ndx + 2].accelerations[jnt] = 0.0

        if self.spline_order < 3:
            return 1

        return 3  # At least cubic and has 3 transition segments

    def calculate_ramp_down_section(self, ndx):
        prior = self.robot_command.points[ndx - 1]

        # Create next knot points
        self.robot_command.points.append(copy.deepcopy(prior))
        if self.spline_order > 1:
            # Add transition points
            self.robot_command.points.append(copy.deepcopy(prior))
            self.robot_command.points.append(copy.deepcopy(prior))
            dT0 = self.ramp_start_fraction * self.ramp_down_time
            dT1 = (self.ramp_end_fraction - self.ramp_start_fraction) * self.ramp_down_time
            dT2 = (1.0 - self.ramp_end_fraction) * self.ramp_down_time
            self.robot_command.points[ndx].time_from_start = prior.time_from_start + rospy.Duration(dT0)
            self.robot_command.points[ndx + 1].time_from_start = self.robot_command.points[
                ndx
            ].time_from_start + rospy.Duration(dT1)
            self.robot_command.points[ndx + 2].time_from_start = self.robot_command.points[
                ndx + 1
            ].time_from_start + rospy.Duration(dT2)
        else:
            dT0 = self.ramp_down_time
            self.robot_command.points[ndx].time_from_start = prior.time_from_start + rospy.Duration(dT0)

        # Now calculate the interior values for each joint individually
        for jnt, joint in enumerate(self.chain):
            if joint.ramp_up != 0.0:
                starting_position = copy.deepcopy(prior.positions[jnt])
                ending_position = starting_position + joint.ramp_down

                print "calculating ramp down for ", joint.name, " from ", starting_position, " to ", ending_position, " over ", self.ramp_down_time, " seconds"

                if self.spline_order > 1:
                    # Ax=b  where x=[a0 b0 c0 d0 e0 f0 e1 f1 a2 b2 c2 d2 e2 f2] with p=a*t^5 + b*t^4 + c*t^3 + d*t^2 + e*t +f for first and last, and p=e*t + f for middle
                    #  6 + 2 + 6 unknowns
                    x = self.solve_ramp_matrices(dT0, dT1, dT2, starting_position, ending_position)
                    # first interior point
                    self.robot_command.points[ndx].positions[jnt] = copy.deepcopy(x[7][0])
                    self.robot_command.points[ndx].velocities[jnt] = copy.deepcopy(x[6][0])
                    # linear segment
                    self.robot_command.points[ndx + 1].positions[jnt] = copy.deepcopy(x[13][0])
                    self.robot_command.points[ndx + 1].velocities[jnt] = copy.deepcopy(x[6][0])
                    # last interior point
                    self.robot_command.points[ndx + 2].positions[jnt] = ending_position
                    self.robot_command.points[ndx + 2].velocities[jnt] = 0.0
                else:
                    self.robot_command.points[ndx].positions[jnt] = copy.deepcopy(ending_position)

                if self.spline_order > 3:
                    self.robot_command.points[ndx + 2].accelerations[jnt] = 0.0
                    self.robot_command.points[ndx].accelerations[jnt] = 0.0
                    self.robot_command.points[ndx + 1].accelerations[jnt] = 0.0

        if self.spline_order < 3:
            return 1

        return 3  # At least cubic and has 3 transition segments

    def solve_ramp_matrices(self, dT0, dT1, dT2, starting_position, ending_position):

        if self.spline_order < 4:
            # assume cubic spline
            return self.solve_ramp_cubic_matrices(dT0, dT1, dT2, starting_position, ending_position)

        # This is the quintic version
        # Ax=b  where x=[a0 b0 c0 d0 e0 f0 e1 f1 a2 b2 c2 d2 e2 f2] with p=a*t^5 + b*t^4 + c*t^3 + d*t^2 + e*t +f for first and last, and p=e*t + f for middle
        #  6 + 2 + 6 = 14 unknowns
        A = np.zeros((14, 14))
        b = np.zeros((14, 1))

        # Eqn 1  - Known starting position
        A[0][5] = 1
        b[0][0] = starting_position

        # Eqn 2  - Zero velocity at start
        A[1][4] = 1.0

        # Eqn 3  - Zero acceleration at start
        A[2][3] = 2.0

        # Eqn 4  - Continuous velocity at first interior knot point
        A[3][0] = 5 * dT0 ** 4
        A[3][1] = 4 * dT0 ** 3
        A[3][2] = 3 * dT0 ** 2
        A[3][3] = 2 * dT0
        A[3][4] = 1.0
        A[3][5] = 0.0
        A[3][6] = -1.0

        # Eqn 5  - Zero acceleration at first interior knot point
        A[4][0] = 20 * dT0 ** 3
        A[4][1] = 12 * dT0 ** 2
        A[4][2] = 6 * dT0
        A[4][3] = 2
        A[4][4] = 0.0
        A[4][5] = 0.0
        A[4][6] = 0.0

        # Eqn 6  - Zero jerk at first interior knot point
        A[5][0] = 60 * dT0 ** 2
        A[5][1] = 24 * dT0
        A[5][2] = 6
        A[5][3] = 0.0
        A[5][4] = 0.0
        A[5][5] = 0.0
        A[5][6] = 0.0

        # Eqn 7  - Continuous velocity at second interior knot point
        A[6][8] = 0.0
        A[6][9] = 0.0
        A[6][10] = 0.0
        A[6][11] = 0.0
        A[6][12] = 1.0
        A[6][13] = 0.0
        A[6][6] = -1.0

        # Eqn 8  -  Zero acceleration at second interior knot point
        A[7][8] = 0.0
        A[7][9] = 0.0
        A[7][10] = 0.0
        A[7][11] = 2
        A[7][12] = 0.0
        A[7][13] = 0.0
        A[7][6] = 0.0

        # Eqn 9  - Zero jerk at second interior knot point
        A[8][8] = 0.0
        A[8][9] = 0.0
        A[8][10] = 6
        A[8][11] = 0.0
        A[8][12] = 0.0
        A[8][13] = 0.0
        A[8][6] = 0.0

        # Eqn 10 - Equal (but unknown) positions at first interior knot point
        A[9][0] = dT0 ** 5
        A[9][1] = dT0 ** 4
        A[9][2] = dT0 ** 3
        A[9][3] = dT0 ** 2
        A[9][4] = dT0
        A[9][5] = 1.0
        A[9][7] = -1.0

        # Eqn 11 - Equal (but unknown) positions at second interior knot point
        A[10][13] = 1.0
        A[10][6] = -dT1
        A[10][7] = -1.0

        # Eqn 12 - Known final position
        A[11][8] = dT2 ** 5
        A[11][9] = dT2 ** 4
        A[11][10] = dT2 ** 3
        A[11][11] = dT2 ** 2
        A[11][12] = dT2
        A[11][13] = 1.0
        b[11][0] = ending_position

        # Eqn 13 - Known final velocity = 0
        A[12][8] = 5 * dT2 ** 4
        A[12][9] = 4 * dT2 ** 3
        A[12][10] = 3 * dT2 ** 2
        A[12][11] = 2 * dT2
        A[12][12] = 1.0
        A[12][13] = 0.0

        # Eqn 14 - Known final acceleration = 0
        A[13][8] = 20 * dT2 ** 3
        A[13][9] = 12 * dT2 ** 2
        A[13][10] = 6 * dT2
        A[13][11] = 2.0
        A[13][12] = 0.0
        A[13][13] = 0.0

        params = np.linalg.solve(A, b)

        # np.set_printoptions(precision=9, suppress=True, linewidth=250)
        # print "A=",A
        # print "b=",b
        # print "condition number(A)=",np.linalg.cond(A)
        # print "x=",params

        return params

    def solve_ramp_cubic_matrices(self, dT0, dT1, dT2, starting_position, ending_position):

        # This is the cubic version
        # Quintic version: Ax=b  where x=[a0 b0 c0 d0 e0 f0 e1 f1 a2 b2 c2 d2 e2 f2] with p=a*t^5 + b*t^4 + c*t^3 + d*t^2 + e*t +f for first and last, and p=e*t + f for middle
        # Cubic   version: Ax=b  where x=[c0 d0 e0 f0 e1 f1 c2 d2 e2 f2] with p= c*t^3 + d*t^2 + e*t +f for first and last, and p=e*t + f for middle
        #
        #  4 + 2 + 4 = 10 unknowns
        A = np.zeros((10, 10))
        b = np.zeros((10, 1))

        # Eqn 1  - Known starting position
        A[0][3] = 1
        b[0][0] = starting_position

        # Eqn 2  - Zero velocity at start
        A[1][2] = 1.0

        # Eqn 3  - Continuous velocity at first interior knot point
        A[2][0] = 3 * dT0 ** 2
        A[2][1] = 2 * dT0
        A[2][2] = 1.0
        A[2][3] = 0.0
        A[2][4] = -1.0

        # Eqn 4  - Zero acceleration at first interior knot point
        A[3][0] = 6 * dT0
        A[3][1] = 2
        A[3][2] = 0.0
        A[3][3] = 0.0

        # Eqn 5  - Continuous velocity at second interior knot point
        A[4][6] = 0.0
        A[4][7] = 0.0
        A[4][8] = 1.0
        A[4][9] = 0.0
        A[4][4] = -1.0

        # Eqn 6  -  Zero acceleration at second interior knot point
        A[5][6] = 0.0
        A[5][7] = 2.0
        A[5][8] = 0.0
        A[5][9] = 0.0

        # Eqn 7 - Equal (but unknown) positions at first interior knot point
        A[6][0] = dT0 ** 3
        A[6][1] = dT0 ** 2
        A[6][2] = dT0
        A[6][3] = 1.0
        A[6][5] = -1.0

        # Eqn 8 - Equal (but unknown) positions at second interior knot point
        A[7][9] = 1.0
        A[7][4] = -dT1
        A[7][5] = -1.0

        # Eqn 9 - Known final position
        A[8][6] = dT2 ** 3
        A[8][7] = dT2 ** 2
        A[8][8] = dT2
        A[8][9] = 1.0
        b[8][0] = ending_position

        # Eqn 10 - Known final velocity = 0
        A[9][6] = 3 * dT2 ** 2
        A[9][7] = 2 * dT2
        A[9][8] = 1.0
        A[9][9] = 0.0

        params = np.linalg.solve(A, b)

        quintic_params = np.zeros((14, 1))
        quintic_params[2:8] = params[0:6]
        quintic_params[10:14] = params[6:10]

        np.set_printoptions(precision=9, suppress=True, linewidth=250)
        print "A=", A
        print "b=", b
        print "condition number(A)=", np.linalg.cond(A)
        print "x=", params
        # print "-----------------------------------------------------------------------"
        # print "--------- Cubic solve -------------------------------------------------"
        # print "cubic params=",params
        # print "quintic params = ", quintic_params
        # print "-----------------------------------------------------------------------"
        return quintic_params

    def shutdown_plugin(self):
        # Just make sure to stop timers and publishers, unsubscribe from Topics etc in the shutdown_plugin method.
        # TODO, is the above comment why the plugin gets stuck when we try to shutdown?
        print "Shutdown ..."
        rospy.sleep(0.1)
        self.jointSubscriber.unregister()
        self.commandPublisher.unregister()
        rospy.sleep(0.5)
        print "Done!"

    # Define collection of widgets for joint group
    def joint_widget(self, main_vbox, index):
        joint = self.chain[index]
        frame = QFrame()
        frame.setFrameShape(QFrame.StyledPanel)
        frame.setFrameShadow(QFrame.Raised)

        hbox = QHBoxLayout()
        # hbox.addWidget(frame)

        self.prior_time = 0.0

        robot_joint = self.robot.joints[self.joint_list[joint.name]]
        joint.lower_limit = robot_joint.limit.lower
        joint.upper_limit = robot_joint.limit.upper
        ramp_range = robot_joint.limit.upper - robot_joint.limit.lower

        print "  ", joint.name, "  limits(", joint.lower_limit, ", ", joint.upper_limit, ") range=", ramp_range

        self.cmd_spinbox.append(QDoubleSpinBox())
        self.cmd_spinbox[index].setDecimals(5)
        self.cmd_spinbox[index].setRange(joint.lower_limit, joint.upper_limit)
        self.cmd_spinbox[index].setSingleStep((robot_joint.limit.upper - robot_joint.limit.lower) / 50.0)
        self.cmd_spinbox[index].valueChanged.connect(joint.on_cmd_value)

        self.ramp_up_spinbox.append(QDoubleSpinBox())
        self.ramp_up_spinbox[index].setDecimals(5)
        self.ramp_up_spinbox[index].setRange(-ramp_range, ramp_range)
        self.ramp_up_spinbox[index].setSingleStep(ramp_range / 50.0)
        self.ramp_up_spinbox[index].valueChanged.connect(joint.on_ramp_up_value)

        self.ramp_down_spinbox.append(QDoubleSpinBox())
        self.ramp_down_spinbox[index].setDecimals(5)
        self.ramp_down_spinbox[index].setRange(-ramp_range, ramp_range)
        self.ramp_down_spinbox[index].setSingleStep(ramp_range / 50.0)
        self.ramp_down_spinbox[index].valueChanged.connect(joint.on_ramp_down_value)

        hbox.addWidget(QLabel(joint.name))
        hbox.addWidget(self.cmd_spinbox[index])
        hbox.addWidget(self.ramp_up_spinbox[index])
        hbox.addWidget(self.ramp_down_spinbox[index])

        # Add horizontal layout to frame for this joint group
        frame.setLayout(hbox)

        # Add frame to main vertical layout
        main_vbox.addWidget(frame)
Exemplo n.º 9
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 )