class Top(Plugin): def __init__(self, context): super(Top, self).__init__(context) # Give QObjects reasonable names self.setObjectName('Top') # Process standalone plugin command-line arguments from argparse import ArgumentParser parser = ArgumentParser() # Add argument(s) to the parser. parser.add_argument("-q", "--quiet", action="store_true", dest="quiet", help="Put plugin in silent mode") args, unknowns = parser.parse_known_args(context.argv()) rospy.wait_for_service('PlayTrajectoryState') self.play_traj = rospy.ServiceProxy('PlayTrajectoryState', PlayTrajectoryState) # Create a container widget and give it a layout self._container = QWidget() self._layout = QVBoxLayout() self._container.setLayout(self._layout) context.add_widget(self._container) # merge sort to pick files generate files to pick traj_dir = rospy.get_param('traj_dir') self.traj_list = sorted(os.listdir(traj_dir)) #self.traj_list = self.traj_list[:5] self.length = len(self.traj_list) self.work_list = copy.deepcopy(self.traj_list) self.merge_width = 1 self.merge_ind = 0 self._i0 = self.merge_ind self._i1 = min(self.merge_ind + self.merge_width, self.length) self._j = 0 self.comp_res = True self.valid_comp = False self.merge() #random.seed() #sys.stdin.readline() # use merge sort to sample traj fiels #self.merge_traj_files() # regenerate the trajectories #self.rand_traj_files() self.write_to_file = "" # Add a button to play first trajcetory self._play_A_btn = QPushButton('Play Trajectory A') self._layout.addWidget(self._play_A_btn) self._play_A_btn.clicked.connect(self._play_traj_A) # Add a slider to trace the first trajectory self._slider_A = QSlider(Qt.Horizontal) self._layout.addWidget(self._slider_A) self._slider_A.setMinimum(1) self._slider_A.setMaximum(100) self._slider_A.valueChanged.connect(self._change_slider_A) # Add a button to play second trajcetory self._play_B_btn = QPushButton('Play Trajectory B') self._layout.addWidget(self._play_B_btn) self._play_B_btn.clicked.connect(self._play_traj_B) # Add a slider to trace the second trajectory self._slider_B = QSlider(Qt.Horizontal) self._layout.addWidget(self._slider_B) self._slider_B.setMinimum(1) self._slider_B.setMaximum(100) self._slider_B.valueChanged.connect(self._change_slider_B) # Add a button to pause current trajectories self._pause_btn = QPushButton('Pause current trajectory') self._layout.addWidget(self._pause_btn) self._pause_btn.clicked.connect(self._pause_traj) self.paused = False # Add a button to pick first trajcetory self._pick_A_btn = QPushButton('Pick Trajectory A') self._layout.addWidget(self._pick_A_btn) self._pick_A_btn.clicked.connect(self._pick_traj_A) # Add a button to pick second trajcetory self._pick_B_btn = QPushButton('Pick Trajectory B') self._layout.addWidget(self._pick_B_btn) self._pick_B_btn.clicked.connect(self._pick_traj_B) # Add a button to pick neither of trajcetories self._pick_none_btn = QPushButton('Pick None of Trajectories') self._layout.addWidget(self._pick_none_btn) self._pick_none_btn.clicked.connect(self._pick_none) def merge_traj_files(self): print "___________________________________________" if self.merge_width < self.length: print "Width = %f" % self.merge_width if self.merge_ind < self.length: print "Merge index = %f" % self.merge_ind # take pair of sublists and merge them while True: if self._j < min(self.merge_ind + 2 * self.merge_width, self.length): if (self._i0 < min(self.merge_ind + self.merge_width, self.length)): if self._i1 >= min( self.merge_ind + 2 * self.merge_width, self.length): self.work_list[self._j] = self.traj_list[ self._i0] print "it's here 1" print "B %f assigned A %f" % (self._j, self._i0) self._i0 += 1 self.valid_comp = False else: if self.valid_comp: if self.comp_res: self.work_list[ self._j] = self.traj_list[self._i0] print "it's here 2" print "B %f assigned A %f" % (self._j, self._i0) self._i0 += 1 self.valid_comp = False else: self.work_list[ self._j] = self.traj_list[self._i1] print "it's here 3" print "B %f assigned A %f" % (self._j, self._i1) self._i1 += 1 self.valid_comp = False else: self.merge() break else: self.work_list[self._j] = self.traj_list[self._i1] print "it's here 4" print "B %f assigned A %f" % (self._j, self._i1) self._i1 += 1 self.valid_comp = False self._j += 1 else: self.merge_ind = self.merge_ind + 2 * self.merge_width self._i0 = self.merge_ind self._i1 = min(self.merge_ind + self.merge_width, self.length) self._j = self.merge_ind self.merge() print "lists are merged" print self.traj_list print self.work_list temp = copy.deepcopy(self.traj_list) self.traj_list = copy.deepcopy(self.work_list) #self.work_list = temp #self.merge_traj_files() break else: # copyArray() temp = copy.deepcopy(self.traj_list) self.traj_list = copy.deepcopy(self.work_list) self.work_list = temp self.merge_ind = 0 self.merge_width = 2 * self.merge_width self._i0 = self.merge_ind self._i1 = min(self.merge_ind + self.merge_width, self.length) self._j = self.merge_ind print "ALL LISTS ARE MERGED" print self.traj_list self.merge_traj_files() else: print "ALL TRAJECTORIES ARE RANKED" f_ = open('ranked_traj.txt', 'w') for item in self.traj_list: f_.write("%s\n" % item) f_.close() def merge(self): print self._i0 print self._i1 try: self.fileA = self.traj_list[self._i0] self.fileB = self.traj_list[self._i1] except: try: self.fileA = self.work_list[0] self.fileB = self.work_list[min(2 * self.merge_width, self.length)] except: self.fileA = self.traj_list[0] self.fileB = self.traj_list[1] print " new files assigned are: %s, %s " % (self.fileA, self.fileB) def _play_traj_A(self): if (self.paused): self._pause_btn.setText("Pause current trajectory") self.paused = False req = UiState(Header(), self.fileA, True, False, -1) resp = self.play_traj(req) def _change_slider_A(self): self._slider_B.setValue(0) req = UiState(Header(), self.fileA, False, True, float(self._slider_A.value())) resp = self.play_traj(req) def _play_traj_B(self): if (self.paused): self._pause_btn.setText("Pause current trajectory") self.paused = False req = UiState(Header(), self.fileB, True, False, -1) resp = self.play_traj(req) def _change_slider_B(self): self._slider_A.setValue(0) req = UiState(Header(), self.fileB, False, True, float(self._slider_B.value())) resp = self.play_traj(req) def _pause_traj(self): if (self.paused): req = UiState(Header(), "", False, False, -1) resp = self.play_traj(req) self._pause_btn.setText("Pause current trajectory") self.paused = False else: req = UiState(Header(), "", False, True, -1) resp = self.play_traj(req) self._pause_btn.setText("Continue playing current trajectory") self.paused = True def _pick_traj_A(self): self.comp_res = True self.valid_comp = True self.merge_traj_files() def _pick_traj_B(self): self.comp_res = False self.valid_comp = True self.merge_traj_files() def _pick_none(self): #reinvoke the traj ranodm generator self.rand_traj_files() # produce two new filenames from the trajectory directory def rand_traj_files(self): first_to_pick = random.randint(0, len(self.traj_list) - 1) while (True): second_to_pick = random.randint(0, len(self.traj_list) - 1) if (second_to_pick != first_to_pick): break self.fileA = self.traj_list[first_to_pick] self.fileB = self.traj_list[second_to_pick]
def __init__(self, context): super(Top, self).__init__(context) # Give QObjects reasonable names self.setObjectName('Top') # Process standalone plugin command-line arguments from argparse import ArgumentParser parser = ArgumentParser() # Add argument(s) to the parser. parser.add_argument("-q", "--quiet", action="store_true", dest="quiet", help="Put plugin in silent mode") args, unknowns = parser.parse_known_args(context.argv()) rospy.wait_for_service('PlayTrajectoryState') self.play_traj = rospy.ServiceProxy('PlayTrajectoryState', PlayTrajectoryState) # Create a container widget and give it a layout self._container = QWidget() self._layout = QVBoxLayout() self._container.setLayout(self._layout) context.add_widget(self._container) # merge sort to pick files generate files to pick traj_dir = rospy.get_param('traj_dir') self.traj_list = sorted(os.listdir(traj_dir)) #self.traj_list = self.traj_list[:5] self.length = len(self.traj_list) self.work_list = copy.deepcopy(self.traj_list) self.merge_width = 1 self.merge_ind = 0 self._i0 = self.merge_ind self._i1 = min(self.merge_ind + self.merge_width, self.length) self._j = 0 self.comp_res = True self.valid_comp = False self.merge() #random.seed() #sys.stdin.readline() # use merge sort to sample traj fiels #self.merge_traj_files() # regenerate the trajectories #self.rand_traj_files() self.write_to_file = "" # Add a button to play first trajcetory self._play_A_btn = QPushButton('Play Trajectory A') self._layout.addWidget(self._play_A_btn) self._play_A_btn.clicked.connect(self._play_traj_A) # Add a slider to trace the first trajectory self._slider_A = QSlider(Qt.Horizontal) self._layout.addWidget(self._slider_A) self._slider_A.setMinimum(1) self._slider_A.setMaximum(100) self._slider_A.valueChanged.connect(self._change_slider_A) # Add a button to play second trajcetory self._play_B_btn = QPushButton('Play Trajectory B') self._layout.addWidget(self._play_B_btn) self._play_B_btn.clicked.connect(self._play_traj_B) # Add a slider to trace the second trajectory self._slider_B = QSlider(Qt.Horizontal) self._layout.addWidget(self._slider_B) self._slider_B.setMinimum(1) self._slider_B.setMaximum(100) self._slider_B.valueChanged.connect(self._change_slider_B) # Add a button to pause current trajectories self._pause_btn = QPushButton('Pause current trajectory') self._layout.addWidget(self._pause_btn) self._pause_btn.clicked.connect(self._pause_traj) self.paused = False # Add a button to pick first trajcetory self._pick_A_btn = QPushButton('Pick Trajectory A') self._layout.addWidget(self._pick_A_btn) self._pick_A_btn.clicked.connect(self._pick_traj_A) # Add a button to pick second trajcetory self._pick_B_btn = QPushButton('Pick Trajectory B') self._layout.addWidget(self._pick_B_btn) self._pick_B_btn.clicked.connect(self._pick_traj_B) # Add a button to pick neither of trajcetories self._pick_none_btn = QPushButton('Pick None of Trajectories') self._layout.addWidget(self._pick_none_btn) self._pick_none_btn.clicked.connect(self._pick_none)
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)
def _load_parameter(self,param_index,param_name, param_type,param_value, param_min, param_max, param_desc): x_check = 160 x ,y ,w ,h = 4,4 + 40 * param_index,150,20 label = QLabel(param_name,self.parameters) label.setGeometry(x,y,w,h) label.setToolTip(param_desc) label.show() if param_type == 'Boolean': checkbox = QCheckBox('',self.parameters) checkbox.setGeometry(x_check,y,w,h) checkbox.setChecked(param_value == '1') checkbox.setToolTip(param_desc) checkbox.stateChanged.connect(partial(self._handle_bool_param_changed,param_name)) checkbox.show() elif param_type == 'Integer': if len(param_min) > 0 and len(param_max) > 0 and int(param_max) < 5: slider = QSlider(Qt.Horizontal,self.parameters) slider.setMinimum(int(param_min)) slider.setMaximum(int(param_max)) slider.setValue(int(param_value)) slider.setTickPosition(QSlider.TicksBelow) slider.setGeometry(x_check,y,w,h) slider.setToolTip(param_desc) slider.setTickInterval(1) slider.setSingleStep(1) slider.setPageStep(1) slider.valueChanged.connect(partial(self._handle_param_changed,param_name)) slider.show() else: spin = QSpinBox(self.parameters) if len(param_min) > 0: spin.setMinimum(int(param_min)) if len(param_max) > 0: spin.setMaximum(int(param_max)) spin.setValue(int(param_value)) spin.setToolTip(param_desc) spin.setGeometry(x_check,y,w,h) spin.valueChanged.connect(partial(self._handle_param_changed,param_name)) spin.show() elif param_type == 'Double': spin = QDoubleSpinBox(self.parameters) if len(param_min) > 0: spin.setMinimum(float(param_min)) if len(param_max) > 0: spin.setMaximum(float(param_max)) spin.setValue(float(param_value)) spin.valueChanged.connect(partial(self._handle_param_changed,param_name)) spin.setGeometry(x_check,y,w,h) spin.show() elif param_type == 'String': lineEdit = QLineEdit(self.parameters) lineEdit.setText(param_value) lineEdit.setToolTip(param_desc) lineEdit.setGeometry(x_check,y,w,h) lineEdit.textChanged.connect(partial(self._handle_param_changed,param_name)) lineEdit.show() self.parameters.update()
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 )
def __init__(self, context): super(Top, self).__init__(context) # Give QObjects reasonable names self.setObjectName('Top') # Process standalone plugin command-line arguments from argparse import ArgumentParser parser = ArgumentParser() # Add argument(s) to the parser. parser.add_argument("-q", "--quiet", action="store_true", dest="quiet", help="Put plugin in silent mode") args, unknowns = parser.parse_known_args(context.argv()) rospy.wait_for_service('PlayTrajectoryState') self.play_traj = rospy.ServiceProxy('PlayTrajectoryState', PlayTrajectoryState) # Create a container widget and give it a layout self._container = QWidget() self._layout = QVBoxLayout() self._container.setLayout(self._layout) context.add_widget(self._container) # merge sort to pick files generate files to pick traj_dir = rospy.get_param('traj_dir') self.traj_list = sorted( os.listdir(traj_dir) ) #self.traj_list = self.traj_list[:5] self.length = len( self.traj_list ) self.work_list = copy.deepcopy(self.traj_list) self.merge_width = 1 self.merge_ind = 0 self._i0 = self.merge_ind self._i1 = min(self.merge_ind + self.merge_width, self.length) self._j = 0 self.comp_res = True self.valid_comp = False self.merge() #random.seed() #sys.stdin.readline() # use merge sort to sample traj fiels #self.merge_traj_files() # regenerate the trajectories #self.rand_traj_files() self.write_to_file = "" # Add a button to play first trajcetory self._play_A_btn = QPushButton('Play Trajectory A') self._layout.addWidget(self._play_A_btn) self._play_A_btn.clicked.connect(self._play_traj_A) # Add a slider to trace the first trajectory self._slider_A = QSlider(Qt.Horizontal) self._layout.addWidget(self._slider_A) self._slider_A.setMinimum(1) self._slider_A.setMaximum(100) self._slider_A.valueChanged.connect(self._change_slider_A) # Add a button to play second trajcetory self._play_B_btn = QPushButton('Play Trajectory B') self._layout.addWidget(self._play_B_btn) self._play_B_btn.clicked.connect(self._play_traj_B) # Add a slider to trace the second trajectory self._slider_B = QSlider(Qt.Horizontal) self._layout.addWidget(self._slider_B) self._slider_B.setMinimum(1) self._slider_B.setMaximum(100) self._slider_B.valueChanged.connect(self._change_slider_B) # Add a button to pause current trajectories self._pause_btn = QPushButton('Pause current trajectory') self._layout.addWidget(self._pause_btn) self._pause_btn.clicked.connect(self._pause_traj) self.paused = False # Add a button to pick first trajcetory self._pick_A_btn = QPushButton('Pick Trajectory A') self._layout.addWidget(self._pick_A_btn) self._pick_A_btn.clicked.connect(self._pick_traj_A) # Add a button to pick second trajcetory self._pick_B_btn = QPushButton('Pick Trajectory B') self._layout.addWidget(self._pick_B_btn) self._pick_B_btn.clicked.connect(self._pick_traj_B) # Add a button to pick neither of trajcetories self._pick_none_btn = QPushButton('Pick None of Trajectories') self._layout.addWidget(self._pick_none_btn) self._pick_none_btn.clicked.connect(self._pick_none)
class Top(Plugin): def __init__(self, context): super(Top, self).__init__(context) # Give QObjects reasonable names self.setObjectName('Top') # Process standalone plugin command-line arguments from argparse import ArgumentParser parser = ArgumentParser() # Add argument(s) to the parser. parser.add_argument("-q", "--quiet", action="store_true", dest="quiet", help="Put plugin in silent mode") args, unknowns = parser.parse_known_args(context.argv()) rospy.wait_for_service('PlayTrajectoryState') self.play_traj = rospy.ServiceProxy('PlayTrajectoryState', PlayTrajectoryState) # Create a container widget and give it a layout self._container = QWidget() self._layout = QVBoxLayout() self._container.setLayout(self._layout) context.add_widget(self._container) # merge sort to pick files generate files to pick traj_dir = rospy.get_param('traj_dir') self.traj_list = sorted( os.listdir(traj_dir) ) #self.traj_list = self.traj_list[:5] self.length = len( self.traj_list ) self.work_list = copy.deepcopy(self.traj_list) self.merge_width = 1 self.merge_ind = 0 self._i0 = self.merge_ind self._i1 = min(self.merge_ind + self.merge_width, self.length) self._j = 0 self.comp_res = True self.valid_comp = False self.merge() #random.seed() #sys.stdin.readline() # use merge sort to sample traj fiels #self.merge_traj_files() # regenerate the trajectories #self.rand_traj_files() self.write_to_file = "" # Add a button to play first trajcetory self._play_A_btn = QPushButton('Play Trajectory A') self._layout.addWidget(self._play_A_btn) self._play_A_btn.clicked.connect(self._play_traj_A) # Add a slider to trace the first trajectory self._slider_A = QSlider(Qt.Horizontal) self._layout.addWidget(self._slider_A) self._slider_A.setMinimum(1) self._slider_A.setMaximum(100) self._slider_A.valueChanged.connect(self._change_slider_A) # Add a button to play second trajcetory self._play_B_btn = QPushButton('Play Trajectory B') self._layout.addWidget(self._play_B_btn) self._play_B_btn.clicked.connect(self._play_traj_B) # Add a slider to trace the second trajectory self._slider_B = QSlider(Qt.Horizontal) self._layout.addWidget(self._slider_B) self._slider_B.setMinimum(1) self._slider_B.setMaximum(100) self._slider_B.valueChanged.connect(self._change_slider_B) # Add a button to pause current trajectories self._pause_btn = QPushButton('Pause current trajectory') self._layout.addWidget(self._pause_btn) self._pause_btn.clicked.connect(self._pause_traj) self.paused = False # Add a button to pick first trajcetory self._pick_A_btn = QPushButton('Pick Trajectory A') self._layout.addWidget(self._pick_A_btn) self._pick_A_btn.clicked.connect(self._pick_traj_A) # Add a button to pick second trajcetory self._pick_B_btn = QPushButton('Pick Trajectory B') self._layout.addWidget(self._pick_B_btn) self._pick_B_btn.clicked.connect(self._pick_traj_B) # Add a button to pick neither of trajcetories self._pick_none_btn = QPushButton('Pick None of Trajectories') self._layout.addWidget(self._pick_none_btn) self._pick_none_btn.clicked.connect(self._pick_none) def merge_traj_files(self): print "___________________________________________" if self.merge_width < self.length: print "Width = %f" % self.merge_width if self.merge_ind < self.length: print "Merge index = %f" % self.merge_ind # take pair of sublists and merge them while True: if self._j < min(self.merge_ind + 2 * self.merge_width, self.length): if (self._i0 < min(self.merge_ind + self.merge_width, self.length)): if self._i1 >= min(self.merge_ind + 2*self.merge_width, self.length): self.work_list[self._j] = self.traj_list[self._i0] print "it's here 1" print "B %f assigned A %f" % (self._j, self._i0) self._i0 += 1 self.valid_comp = False else: if self.valid_comp: if self.comp_res: self.work_list[self._j] = self.traj_list[self._i0] print "it's here 2" print "B %f assigned A %f" % (self._j, self._i0) self._i0 += 1 self.valid_comp = False else: self.work_list[self._j] = self.traj_list[self._i1] print "it's here 3" print "B %f assigned A %f" % (self._j, self._i1) self._i1 += 1 self.valid_comp = False else: self.merge() break else: self.work_list[self._j] = self.traj_list[self._i1] print "it's here 4" print "B %f assigned A %f" % (self._j, self._i1) self._i1 += 1 self.valid_comp = False self._j += 1 else: self.merge_ind = self.merge_ind + 2 * self.merge_width self._i0 = self.merge_ind self._i1 = min(self.merge_ind + self.merge_width, self.length) self._j = self.merge_ind self.merge() print "lists are merged" print self.traj_list print self.work_list temp = copy.deepcopy(self.traj_list) self.traj_list = copy.deepcopy(self.work_list) #self.work_list = temp #self.merge_traj_files() break else: # copyArray() temp = copy.deepcopy(self.traj_list) self.traj_list = copy.deepcopy(self.work_list) self.work_list = temp self.merge_ind = 0 self.merge_width = 2 * self.merge_width self._i0 = self.merge_ind self._i1 = min(self.merge_ind + self.merge_width, self.length) self._j = self.merge_ind print "ALL LISTS ARE MERGED" print self.traj_list self.merge_traj_files() else: print "ALL TRAJECTORIES ARE RANKED" f_ = open('ranked_traj.txt','w') for item in self.traj_list: f_.write("%s\n" % item) f_.close() def merge(self): print self._i0 print self._i1 try: self.fileA = self.traj_list[self._i0] self.fileB = self.traj_list[self._i1] except: try: self.fileA = self.work_list[0] self.fileB = self.work_list[min(2 * self.merge_width, self.length)] except: self.fileA = self.traj_list[0] self.fileB = self.traj_list[1] print " new files assigned are: %s, %s " % (self.fileA, self.fileB) def _play_traj_A(self): if (self.paused): self._pause_btn.setText("Pause current trajectory") self.paused = False req = UiState(Header(), self.fileA, True, False,-1) resp = self.play_traj(req) def _change_slider_A(self): self._slider_B.setValue(0) req = UiState(Header(), self.fileA, False, True, float(self._slider_A.value())) resp = self.play_traj(req) def _play_traj_B(self): if (self.paused): self._pause_btn.setText("Pause current trajectory") self.paused = False req = UiState(Header(), self.fileB, True, False,-1) resp = self.play_traj(req) def _change_slider_B(self): self._slider_A.setValue(0) req = UiState(Header(), self.fileB, False, True, float(self._slider_B.value())) resp = self.play_traj(req) def _pause_traj(self): if (self.paused): req = UiState(Header(), "", False, False,-1) resp = self.play_traj(req) self._pause_btn.setText("Pause current trajectory") self.paused = False else: req = UiState(Header(), "", False, True,-1) resp = self.play_traj(req) self._pause_btn.setText("Continue playing current trajectory") self.paused = True def _pick_traj_A(self): self.comp_res = True self.valid_comp = True self.merge_traj_files() def _pick_traj_B(self): self.comp_res = False self.valid_comp = True self.merge_traj_files() def _pick_none(self): #reinvoke the traj ranodm generator self.rand_traj_files() # produce two new filenames from the trajectory directory def rand_traj_files(self): first_to_pick = random.randint(0,len(self.traj_list)-1) while (True): second_to_pick = random.randint(0,len(self.traj_list)-1) if (second_to_pick != first_to_pick): break self.fileA = self.traj_list[first_to_pick] self.fileB = self.traj_list[second_to_pick]
def __init__(self, context): # super(BDIPelvisPoseWidget, self).__init__(context) # self.setObjectName('BDIPelvisPoseWidget') super(BDIPelvisPoseWidget, self).__init__() self.name = "BDIPelvisPoseWidget" self.updateStateSignal.connect(self.on_updateState) # self._widget = QWidget() self._widget = context vbox = QVBoxLayout() self.forward_position = 0.0 self.lateral_position = 0.0 self.height_position = 0.91 self.roll_position = 0.0 self.pitch_position = 0.0 self.yaw_position = 0.0 self.currentForward = 0.0 self.currentLateral = 0.0 self.currentHeight = 0.91 self.currentRoll = 0.0 self.currentPitch = 0.0 self.currentYaw = 0.0 # Define checkboxes vbox = QVBoxLayout() label = QLabel() label.setText("BDI Pelvis Height (Manipulate Mode Only)") # todo - connect controller mode vbox.addWidget(label) self.enable_checkbox = QCheckBox("Enable") self.enable_checkbox.stateChanged.connect(self.on_enable_check) vbox.addWidget(self.enable_checkbox) self.snap_to_current_button = QPushButton("Snap to Current") self.snap_to_current_button.pressed.connect(self.on_snapCurrentPressed) vbox.addWidget(self.snap_to_current_button) self.roll_slider = QSlider(Qt.Horizontal) self.roll_label = QLabel() self.roll_label.setText("Roll") vbox.addWidget(self.roll_label) self.roll_slider.setRange(int(-100), int(101)) self.roll_slider.setValue(int(0)) self.roll_slider.setSingleStep((200) / 50) self.roll_slider.setTickInterval(25) self.roll_slider.valueChanged.connect(self.on_rollSliderMoved) vbox.addWidget(self.roll_slider) self.roll_progress_bar = QProgressBar() self.roll_progress_bar.setRange(int(-100), int(101)) self.roll_progress_bar.setValue((6.0 / math.pi) * self.currentRoll * 100) self.roll_progress_bar.setFormat("%.6f" % self.currentRoll) vbox.addWidget(self.roll_progress_bar) self.pitch_slider = QSlider(Qt.Horizontal) self.pitch_label = QLabel() self.pitch_label.setText("Pitch") vbox.addWidget(self.pitch_label) self.pitch_slider.setRange(int(-100), int(101)) self.pitch_slider.setValue(int(0)) self.pitch_slider.setSingleStep((200) / 50) self.pitch_slider.setTickInterval(25) self.pitch_slider.valueChanged.connect(self.on_pitchSliderMoved) vbox.addWidget(self.pitch_slider) self.pitch_progress_bar = QProgressBar() self.pitch_progress_bar.setRange(int(-100), int(101)) self.pitch_progress_bar.setValue((6.0 / math.pi) * self.currentPitch * 100) self.pitch_progress_bar.setFormat("%.6f" % self.currentPitch) vbox.addWidget(self.pitch_progress_bar) self.yaw_slider = QSlider(Qt.Horizontal) self.yaw_label = QLabel() self.yaw_label.setText("Yaw") vbox.addWidget(self.yaw_label) self.yaw_slider.setRange(int(-100), int(101)) self.yaw_slider.setValue(int(0)) self.yaw_slider.setSingleStep((200) / 50) self.yaw_slider.setTickInterval(25) self.yaw_slider.valueChanged.connect(self.on_yawSliderMoved) vbox.addWidget(self.yaw_slider) self.yaw_progress_bar = QProgressBar() self.yaw_progress_bar.setRange(int(-100), int(101)) self.yaw_progress_bar.setValue((4.0 / math.pi) * self.currentYaw * 100) self.yaw_progress_bar.setFormat("%.6f" % self.currentYaw) vbox.addWidget(self.yaw_progress_bar) self.forward_label = QLabel() self.forward_label.setText("Forward") vbox.addWidget(self.forward_label) widget = QWidget() hbox = QHBoxLayout() hbox.addStretch() self.forward_slider = QSlider(Qt.Vertical) # self.forward_slider.setText("Height") self.forward_slider.setRange(int(-101), int(100)) self.forward_slider.setValue(int(0)) self.forward_slider.setSingleStep(1) self.forward_slider.setTickInterval(10) self.forward_slider.valueChanged.connect(self.on_forwardSliderMoved) hbox.addWidget(self.forward_slider) self.forward_progress_bar = QProgressBar() self.forward_progress_bar.setOrientation(Qt.Vertical) self.forward_progress_bar.setRange(int(-100), int(101)) self.forward_progress_bar.setValue((self.currentForward / 0.075) * 100) self.forward_progress_bar.setTextVisible(False) hbox.addWidget(self.forward_progress_bar) self.forward_progress_bar_label = QLabel() self.forward_progress_bar_label.setText("%.6f" % self.currentForward) hbox.addWidget(self.forward_progress_bar_label) hbox.addStretch() widget.setLayout(hbox) vbox.addWidget(widget) self.lateral_label = QLabel() self.lateral_label.setText("Lateral") vbox.addWidget(self.lateral_label) self.lateral_slider = QSlider(Qt.Horizontal) # self.lateral_slider.setText("Lateral") self.lateral_slider.setRange(int(-100), int(101)) self.lateral_slider.setValue(int(0)) self.lateral_slider.setSingleStep((200) / 50) self.lateral_slider.setTickInterval(25) self.lateral_slider.valueChanged.connect(self.on_lateralSliderMoved) vbox.addWidget(self.lateral_slider) self.lateral_progress_bar = QProgressBar() self.lateral_progress_bar.setRange(int(-100), int(101)) self.lateral_progress_bar.setValue((self.currentLateral / 0.15) * 100) self.lateral_progress_bar.setFormat("%.6f" % self.currentLateral) vbox.addWidget(self.lateral_progress_bar) self.height_label = QLabel() self.height_label.setText("Height") vbox.addWidget(self.height_label) widget = QWidget() hbox = QHBoxLayout() hbox.addStretch() self.height_slider = QSlider(Qt.Vertical) # self.height_slider.setText("Height") self.height_slider.setRange(int(55), int(105)) self.height_slider.setValue(int(91)) self.height_slider.setSingleStep(2) self.height_slider.setTickInterval(10) self.height_slider.valueChanged.connect(self.on_heightSliderMoved) hbox.addWidget(self.height_slider) self.height_progress_bar = QProgressBar() self.height_progress_bar.setOrientation(Qt.Vertical) self.height_progress_bar.setRange(int(55), int(105)) self.height_progress_bar.setValue(self.currentHeight * 100) self.height_progress_bar.setTextVisible(False) hbox.addWidget(self.height_progress_bar) self.height_progress_bar_label = QLabel() self.height_progress_bar_label.setText("%.6f" % self.currentHeight) hbox.addWidget(self.height_progress_bar_label) hbox.addStretch() widget.setLayout(hbox) vbox.addWidget(widget) vbox.addStretch() self._widget.setLayout(vbox) # context.add_widget(self._widget) self.height_position = 0.91 self.lateral_position = 0.0 self.yaw_position = 0.0 self.first_time = True self.enable_checkbox.setChecked(False) self.yaw_slider.setEnabled(False) self.roll_slider.setEnabled(False) self.pitch_slider.setEnabled(False) self.forward_slider.setEnabled(False) self.lateral_slider.setEnabled(False) self.height_slider.setEnabled(False) self.pub_robot = rospy.Publisher("/flor/controller/bdi_desired_pelvis_pose", PoseStamped, queue_size=10) # self.stateSubscriber = rospy.Subscriber('/flor/pelvis_controller/current_states',JointState, self.stateCallbackFnc) # self.pub_bdi_pelvis = rospy.Publisher('/bdi_manipulate_pelvis_pose_rpy',PoseStamped) self.pelvis_trajectory_pub = rospy.Publisher( "/robot_controllers/pelvis_traj_controller/command", JointTrajectory, queue_size=10 ) self.stateSubscriber = rospy.Subscriber( "/robot_controllers/pelvis_traj_controller/state", JointTrajectoryControllerState, self.stateCallbackFnc )
class BDIPelvisPoseWidget(QObject): updateStateSignal = Signal(object) def __init__(self, context): # super(BDIPelvisPoseWidget, self).__init__(context) # self.setObjectName('BDIPelvisPoseWidget') super(BDIPelvisPoseWidget, self).__init__() self.name = "BDIPelvisPoseWidget" self.updateStateSignal.connect(self.on_updateState) # self._widget = QWidget() self._widget = context vbox = QVBoxLayout() self.forward_position = 0.0 self.lateral_position = 0.0 self.height_position = 0.91 self.roll_position = 0.0 self.pitch_position = 0.0 self.yaw_position = 0.0 self.currentForward = 0.0 self.currentLateral = 0.0 self.currentHeight = 0.91 self.currentRoll = 0.0 self.currentPitch = 0.0 self.currentYaw = 0.0 # Define checkboxes vbox = QVBoxLayout() label = QLabel() label.setText("BDI Pelvis Height (Manipulate Mode Only)") # todo - connect controller mode vbox.addWidget(label) self.enable_checkbox = QCheckBox("Enable") self.enable_checkbox.stateChanged.connect(self.on_enable_check) vbox.addWidget(self.enable_checkbox) self.snap_to_current_button = QPushButton("Snap to Current") self.snap_to_current_button.pressed.connect(self.on_snapCurrentPressed) vbox.addWidget(self.snap_to_current_button) self.roll_slider = QSlider(Qt.Horizontal) self.roll_label = QLabel() self.roll_label.setText("Roll") vbox.addWidget(self.roll_label) self.roll_slider.setRange(int(-100), int(101)) self.roll_slider.setValue(int(0)) self.roll_slider.setSingleStep((200) / 50) self.roll_slider.setTickInterval(25) self.roll_slider.valueChanged.connect(self.on_rollSliderMoved) vbox.addWidget(self.roll_slider) self.roll_progress_bar = QProgressBar() self.roll_progress_bar.setRange(int(-100), int(101)) self.roll_progress_bar.setValue((6.0 / math.pi) * self.currentRoll * 100) self.roll_progress_bar.setFormat("%.6f" % self.currentRoll) vbox.addWidget(self.roll_progress_bar) self.pitch_slider = QSlider(Qt.Horizontal) self.pitch_label = QLabel() self.pitch_label.setText("Pitch") vbox.addWidget(self.pitch_label) self.pitch_slider.setRange(int(-100), int(101)) self.pitch_slider.setValue(int(0)) self.pitch_slider.setSingleStep((200) / 50) self.pitch_slider.setTickInterval(25) self.pitch_slider.valueChanged.connect(self.on_pitchSliderMoved) vbox.addWidget(self.pitch_slider) self.pitch_progress_bar = QProgressBar() self.pitch_progress_bar.setRange(int(-100), int(101)) self.pitch_progress_bar.setValue((6.0 / math.pi) * self.currentPitch * 100) self.pitch_progress_bar.setFormat("%.6f" % self.currentPitch) vbox.addWidget(self.pitch_progress_bar) self.yaw_slider = QSlider(Qt.Horizontal) self.yaw_label = QLabel() self.yaw_label.setText("Yaw") vbox.addWidget(self.yaw_label) self.yaw_slider.setRange(int(-100), int(101)) self.yaw_slider.setValue(int(0)) self.yaw_slider.setSingleStep((200) / 50) self.yaw_slider.setTickInterval(25) self.yaw_slider.valueChanged.connect(self.on_yawSliderMoved) vbox.addWidget(self.yaw_slider) self.yaw_progress_bar = QProgressBar() self.yaw_progress_bar.setRange(int(-100), int(101)) self.yaw_progress_bar.setValue((4.0 / math.pi) * self.currentYaw * 100) self.yaw_progress_bar.setFormat("%.6f" % self.currentYaw) vbox.addWidget(self.yaw_progress_bar) self.forward_label = QLabel() self.forward_label.setText("Forward") vbox.addWidget(self.forward_label) widget = QWidget() hbox = QHBoxLayout() hbox.addStretch() self.forward_slider = QSlider(Qt.Vertical) # self.forward_slider.setText("Height") self.forward_slider.setRange(int(-101), int(100)) self.forward_slider.setValue(int(0)) self.forward_slider.setSingleStep(1) self.forward_slider.setTickInterval(10) self.forward_slider.valueChanged.connect(self.on_forwardSliderMoved) hbox.addWidget(self.forward_slider) self.forward_progress_bar = QProgressBar() self.forward_progress_bar.setOrientation(Qt.Vertical) self.forward_progress_bar.setRange(int(-100), int(101)) self.forward_progress_bar.setValue((self.currentForward / 0.075) * 100) self.forward_progress_bar.setTextVisible(False) hbox.addWidget(self.forward_progress_bar) self.forward_progress_bar_label = QLabel() self.forward_progress_bar_label.setText("%.6f" % self.currentForward) hbox.addWidget(self.forward_progress_bar_label) hbox.addStretch() widget.setLayout(hbox) vbox.addWidget(widget) self.lateral_label = QLabel() self.lateral_label.setText("Lateral") vbox.addWidget(self.lateral_label) self.lateral_slider = QSlider(Qt.Horizontal) # self.lateral_slider.setText("Lateral") self.lateral_slider.setRange(int(-100), int(101)) self.lateral_slider.setValue(int(0)) self.lateral_slider.setSingleStep((200) / 50) self.lateral_slider.setTickInterval(25) self.lateral_slider.valueChanged.connect(self.on_lateralSliderMoved) vbox.addWidget(self.lateral_slider) self.lateral_progress_bar = QProgressBar() self.lateral_progress_bar.setRange(int(-100), int(101)) self.lateral_progress_bar.setValue((self.currentLateral / 0.15) * 100) self.lateral_progress_bar.setFormat("%.6f" % self.currentLateral) vbox.addWidget(self.lateral_progress_bar) self.height_label = QLabel() self.height_label.setText("Height") vbox.addWidget(self.height_label) widget = QWidget() hbox = QHBoxLayout() hbox.addStretch() self.height_slider = QSlider(Qt.Vertical) # self.height_slider.setText("Height") self.height_slider.setRange(int(55), int(105)) self.height_slider.setValue(int(91)) self.height_slider.setSingleStep(2) self.height_slider.setTickInterval(10) self.height_slider.valueChanged.connect(self.on_heightSliderMoved) hbox.addWidget(self.height_slider) self.height_progress_bar = QProgressBar() self.height_progress_bar.setOrientation(Qt.Vertical) self.height_progress_bar.setRange(int(55), int(105)) self.height_progress_bar.setValue(self.currentHeight * 100) self.height_progress_bar.setTextVisible(False) hbox.addWidget(self.height_progress_bar) self.height_progress_bar_label = QLabel() self.height_progress_bar_label.setText("%.6f" % self.currentHeight) hbox.addWidget(self.height_progress_bar_label) hbox.addStretch() widget.setLayout(hbox) vbox.addWidget(widget) vbox.addStretch() self._widget.setLayout(vbox) # context.add_widget(self._widget) self.height_position = 0.91 self.lateral_position = 0.0 self.yaw_position = 0.0 self.first_time = True self.enable_checkbox.setChecked(False) self.yaw_slider.setEnabled(False) self.roll_slider.setEnabled(False) self.pitch_slider.setEnabled(False) self.forward_slider.setEnabled(False) self.lateral_slider.setEnabled(False) self.height_slider.setEnabled(False) self.pub_robot = rospy.Publisher("/flor/controller/bdi_desired_pelvis_pose", PoseStamped, queue_size=10) # self.stateSubscriber = rospy.Subscriber('/flor/pelvis_controller/current_states',JointState, self.stateCallbackFnc) # self.pub_bdi_pelvis = rospy.Publisher('/bdi_manipulate_pelvis_pose_rpy',PoseStamped) self.pelvis_trajectory_pub = rospy.Publisher( "/robot_controllers/pelvis_traj_controller/command", JointTrajectory, queue_size=10 ) self.stateSubscriber = rospy.Subscriber( "/robot_controllers/pelvis_traj_controller/state", JointTrajectoryControllerState, self.stateCallbackFnc ) def stateCallbackFnc(self, jointState_msg): self.updateStateSignal.emit(jointState_msg) def shutdown_plugin(self): # Just make sure to stop timers and publishers, unsubscribe from Topics etc in the shutdown_plugin method. print "Shutdown BDI pelvis height " self.pub_robot.unregister() self.stateSubscriber.unregister() def publishRobotPelvisPose(self): print "publishing new pelvis pose ..." bdi_pelvis_pose = PoseStamped() bdi_pelvis_pose.header.stamp = rospy.Time.now() bdi_pelvis_pose.pose.position.x = self.forward_position bdi_pelvis_pose.pose.position.y = self.lateral_position bdi_pelvis_pose.pose.position.z = self.height_position # Use BDI yaw*roll*pitch concatenation xaxis, yaxis, zaxis = (1, 0, 0), (0, 1, 0), (0, 0, 1) Rx = tf.transformations.rotation_matrix(self.roll_position, xaxis) Ry = tf.transformations.rotation_matrix(self.pitch_position, yaxis) Rz = tf.transformations.rotation_matrix(self.yaw_position, zaxis) R = tf.transformations.concatenate_matrices(Rz, Rx, Ry) q = tf.transformations.quaternion_from_matrix(R) bdi_pelvis_pose.pose.orientation.w = q[3] bdi_pelvis_pose.pose.orientation.x = q[0] bdi_pelvis_pose.pose.orientation.y = q[1] bdi_pelvis_pose.pose.orientation.z = q[2] # w = math.cos(self.yaw_position*0.5) # bdi_pelvis_pose.pose.orientation.x = 0.0 # bdi_pelvis_pose.pose.orientation.y = 0.0 # bdi_pelvis_pose.pose.orientation.z = math.sin(self.yaw_position*0.5) print bdi_pelvis_pose print q euler = tf.transformations.euler_from_quaternion(q) print euler self.pub_robot.publish(bdi_pelvis_pose) # Now publish the trajectory form for the new controllers trajectory = JointTrajectory() trajectory.header.stamp = rospy.Time.now() trajectory.joint_names = ["com_v1", "com_v0", "pelvis_height", "pelvis_roll", "pelvis_pitch", "pelvis_yaw"] trajectory.points = [JointTrajectoryPoint()] trajectory.points[0].positions = [ self.lateral_position, self.forward_position, self.height_position, self.roll_position, self.pitch_position, self.yaw_position, ] trajectory.points[0].velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] trajectory.points[0].time_from_start = rospy.Duration(0.75) self.pelvis_trajectory_pub.publish(trajectory) ################################### def on_snapCurrentPressed(self): print "Snap ", self.name, " values to current pelvis positions" self.blockSignals(True) self.resetCurrentPelvisSliders() self.blockSignals(False) def blockSignals(self, block): self.yaw_slider.blockSignals(block) self.roll_slider.blockSignals(block) self.pitch_slider.blockSignals(block) self.forward_slider.blockSignals(block) self.lateral_slider.blockSignals(block) self.height_slider.blockSignals(block) def resetCurrentPelvisSliders(self): self.yaw_slider.setValue((4.0 / math.pi) * self.currentYaw * 100) self.roll_slider.setValue((6.0 / math.pi) * self.currentRoll * 100) self.pitch_slider.setValue((6.0 / math.pi) * self.currentPitch * 100) self.forward_slider.setValue((self.currentForward / 0.075) * 100) self.lateral_slider.setValue((self.currentLateral / 0.15) * 100) self.height_slider.setValue(self.currentHeight * 100) self.yaw_label.setText("Yaw: " + str(self.currentYaw)) self.roll_label.setText("Roll: " + str(self.currentRoll)) self.pitch_label.setText("Pitch: " + str(self.currentPitch)) self.forward_label.setText("Forward: " + str(self.currentForward)) self.lateral_label.setText("Lateral: " + str(self.currentLateral)) self.height_label.setText("Height: " + str(self.currentHeight)) self.forward_position = self.currentForward self.lateral_position = self.currentLateral self.height_position = self.currentHeight self.roll_position = self.currentRoll self.pitch_position = self.currentPitch self.yaw_position = self.currentYaw def on_updateState(self, jointState_msg): self.currentLateral = jointState_msg.actual.positions[0] self.currentForward = jointState_msg.actual.positions[1] self.currentHeight = jointState_msg.actual.positions[2] self.currentRoll = jointState_msg.actual.positions[3] self.currentPitch = jointState_msg.actual.positions[4] self.currentYaw = jointState_msg.actual.positions[5] self.yaw_progress_bar.setValue((4.0 / math.pi) * self.currentYaw * 100) self.yaw_progress_bar.setFormat("%.6f" % self.currentYaw) self.roll_progress_bar.setValue((6.0 / math.pi) * self.currentRoll * 100) self.roll_progress_bar.setFormat("%.6f" % self.currentRoll) self.pitch_progress_bar.setValue((6.0 / math.pi) * self.currentPitch * 100) self.pitch_progress_bar.setFormat("%.6f" % self.currentPitch) self.forward_progress_bar.setValue((self.currentForward / 0.075) * 100) self.forward_progress_bar_label.setText("%.6f" % self.currentForward) self.lateral_progress_bar.setValue((self.currentLateral / 0.15) * 100) self.lateral_progress_bar.setFormat("%.6f" % self.currentYaw) self.height_progress_bar.setValue(self.currentHeight * 100) self.height_progress_bar_label.setText("%.6f" % self.currentHeight) #################################### def on_enable_check(self, value): print "Toggle the enabling checkbox - current state is ", value self.yaw_slider.setEnabled(self.enable_checkbox.isChecked()) self.roll_slider.setEnabled(self.enable_checkbox.isChecked()) self.pitch_slider.setEnabled(self.enable_checkbox.isChecked()) self.forward_slider.setEnabled(self.enable_checkbox.isChecked()) self.lateral_slider.setEnabled(self.enable_checkbox.isChecked()) self.height_slider.setEnabled(self.enable_checkbox.isChecked()) def on_yawSliderMoved(self, value): self.yaw_position = (value / 100.0) * math.pi / 4.0 # print "New yaw=",self.yaw_position self.yaw_label.setText("Yaw: " + str(self.yaw_position)) if self.enable_checkbox.isChecked(): self.publishRobotPelvisPose() def on_rollSliderMoved(self, value): self.roll_position = (value / 100.0) * math.pi / 6.0 self.roll_label.setText("Roll: " + str(self.roll_position)) if self.enable_checkbox.isChecked(): self.publishRobotPelvisPose() def on_pitchSliderMoved(self, value): self.pitch_position = (value / 100.0) * math.pi / 6.0 self.pitch_label.setText("Pitch: " + str(self.pitch_position)) if self.enable_checkbox.isChecked(): self.publishRobotPelvisPose() def on_forwardSliderMoved(self, value): self.forward_position = (value / 100.0) * 0.075 # print "New forward=",self.forward_position self.forward_label.setText("Forward: " + str(self.forward_position)) if self.enable_checkbox.isChecked(): self.publishRobotPelvisPose() def on_lateralSliderMoved(self, value): self.lateral_position = (value / 100.0) * 0.15 # print "New lateral=",self.lateral_position self.lateral_label.setText("Lateral: " + str(self.lateral_position)) if self.enable_checkbox.isChecked(): self.publishRobotPelvisPose() def on_heightSliderMoved(self, value): self.height_position = value / 100.0 # print "New height=",self.height_position self.height_label.setText("Height: " + str(self.height_position)) if self.enable_checkbox.isChecked(): self.publishRobotPelvisPose()