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()
