def addOnceOrRepeat_And_VoiceRadioButtons(self, layout):
'''
Creates radio buttons for selecting whether a
sound is to play once, or repeatedly until stopped.
Also adds radio buttons for selecting voices.
Places all in a horizontal box layout. Adds
that hbox layout to the passed-in layout.
Sets instance variables:
1. C{self.onceOrRepeatDict}
2. C{self.voicesRadioButtonsDict}
@param layout: Layout object to which the label/txt-field C{hbox} is to be added.
@type layout: QLayout
'''
hbox = QHBoxLayout();
(self.onceOrRepeatGroup, onceOrRepeatButtonLayout, self.onceOrRepeatDict) =\
self.buildRadioButtons([SpeakEasyGUI.interactionWidgets['PLAY_ONCE'],
SpeakEasyGUI.interactionWidgets['PLAY_REPEATEDLY']
],
Orientation.HORIZONTAL,
Alignment.LEFT,
activeButtons=[SpeakEasyGUI.interactionWidgets['PLAY_ONCE']],
behavior=CheckboxGroupBehavior.RADIO_BUTTONS);
self.replayPeriodSpinBox = QDoubleSpinBox(self);
self.replayPeriodSpinBox.setRange(0.0, 99.9); # seconds
self.replayPeriodSpinBox.setSingleStep(0.5);
self.replayPeriodSpinBox.setDecimals(1);
onceOrRepeatButtonLayout.addWidget(self.replayPeriodSpinBox);
secondsLabel = QLabel("secs delay");
onceOrRepeatButtonLayout.addWidget(secondsLabel);
# Create an array of voice radio button labels:
voiceRadioButtonLabels = [];
for voiceKey in SpeakEasyGUI.voices.keys():
voiceRadioButtonLabels.append(SpeakEasyGUI.interactionWidgets[voiceKey]);
(self.voicesGroup, voicesButtonLayout, self.voicesRadioButtonsDict) =\
self.buildRadioButtons(voiceRadioButtonLabels,
Orientation.HORIZONTAL,
Alignment.RIGHT,
activeButtons=[SpeakEasyGUI.interactionWidgets['VOICE_1']],
behavior=CheckboxGroupBehavior.RADIO_BUTTONS);
# Style all the radio buttons:
for playFreqButton in self.onceOrRepeatDict.values():
playFreqButton.setStyleSheet(SpeakEasyGUI.playOnceRepeatButtonStylesheet);
for playFreqButton in self.voicesRadioButtonsDict.values():
playFreqButton.setStyleSheet(SpeakEasyGUI.voiceButtonStylesheet);
#...and the replay delay spinbox:
self.replayPeriodSpinBox.setStyleSheet(SpeakEasyGUI.playRepeatSpinboxStylesheet);
#****** replayPeriodSpinBox styling
hbox.addLayout(onceOrRepeatButtonLayout);
hbox.addStretch(1);
hbox.addLayout(voicesButtonLayout);
layout.addLayout(hbox);
def add_layout_with_frame(parent, layout, text = ""):
box_layout = QHBoxLayout()
box_layout.addLayout(layout)
group_box = QGroupBox()
group_box.setStyleSheet("QGroupBox { border: 1px solid gray; border-radius: 4px; margin-top: 0.5em; } QGroupBox::title { subcontrol-origin: margin; left: 10px; padding: 0 3px 0 3px; }")
group_box.setTitle(text)
group_box.setLayout(box_layout)
parent.addWidget(group_box)
def add_layout_with_frame(parent, layout, text=""):
box_layout = QHBoxLayout()
box_layout.addLayout(layout)
group_box = QGroupBox()
group_box.setStyleSheet(
"QGroupBox { border: 1px solid gray; border-radius: 4px; margin-top: 0.5em; } QGroupBox::title { subcontrol-origin: margin; left: 10px; padding: 0 3px 0 3px; }"
)
group_box.setTitle(text)
group_box.setLayout(box_layout)
parent.addWidget(group_box)
def buildOptionsRadioButtons(self, layout):
hbox = QHBoxLayout();
(self.playLocalityGroup, playLocalityButtonLayout, self.playLocalityRadioButtonsDict) =\
self.buildRadioButtons([SpeakEasyGUI.interactionWidgets['PLAY_LOCALLY'],
SpeakEasyGUI.interactionWidgets['PLAY_AT_ROBOT']
],
Orientation.HORIZONTAL,
Alignment.LEFT,
activeButtons=[SpeakEasyGUI.interactionWidgets[DEFAULT_PLAY_LOCATION]],
behavior=CheckboxGroupBehavior.RADIO_BUTTONS);
#behavior=CheckboxGroupBehavior.CHECKBOXES);
# Style all the radio buttons:
for playLocalityButton in self.playLocalityRadioButtonsDict.values():
playLocalityButton.setStyleSheet(SpeakEasyGUI.voiceButtonStylesheet);
hbox.addLayout(playLocalityButtonLayout);
hbox.addStretch(1);
self.buildConvenienceButtons(hbox);
layout.addLayout(hbox);
def __init__(self, context, add_execute_widget=True):
super(StepInterfaceWidget, self).__init__()
# init signal mapper
self.command_mapper = QSignalMapper(self)
self.command_mapper.mapped.connect(self._publish_step_plan_request)
# start widget
widget = context
error_status_widget = QErrorStatusWidget()
self.logger = Logger(error_status_widget)
vbox = QVBoxLayout()
# start control box
controls_hbox = QHBoxLayout()
# left coloumn
left_controls_vbox = QVBoxLayout()
left_controls_vbox.setMargin(0)
self.add_command_button(left_controls_vbox, "Rotate Left",
PatternParameters.ROTATE_LEFT)
self.add_command_button(left_controls_vbox, "Strafe Left",
PatternParameters.STRAFE_LEFT)
self.add_command_button(left_controls_vbox, "Step Up",
PatternParameters.STEP_UP)
self.add_command_button(left_controls_vbox, "Center on Left",
PatternParameters.FEET_REALIGN_ON_LEFT)
left_controls_vbox.addStretch()
controls_hbox.addLayout(left_controls_vbox, 1)
# center coloumn
center_controls_vbox = QVBoxLayout()
center_controls_vbox.setMargin(0)
self.add_command_button(center_controls_vbox, "Forward",
PatternParameters.FORWARD)
self.add_command_button(center_controls_vbox, "Backward",
PatternParameters.BACKWARD)
self.add_command_button(center_controls_vbox, "Step Over",
PatternParameters.STEP_OVER)
self.add_command_button(center_controls_vbox, "Center Feet",
PatternParameters.FEET_REALIGN_ON_CENTER)
self.add_command_button(center_controls_vbox, "Wide Stance",
PatternParameters.WIDE_STANCE)
center_controls_vbox.addStretch()
controls_hbox.addLayout(center_controls_vbox, 1)
# right coloumn
right_controls_vbox = QVBoxLayout()
right_controls_vbox.setMargin(0)
self.add_command_button(right_controls_vbox, "Rotate Right",
PatternParameters.ROTATE_RIGHT)
self.add_command_button(right_controls_vbox, "Strafe Right",
PatternParameters.STRAFE_RIGHT)
self.add_command_button(right_controls_vbox, "Step Down",
PatternParameters.STEP_DOWN)
self.add_command_button(right_controls_vbox, "Center on Right",
PatternParameters.FEET_REALIGN_ON_RIGHT)
right_controls_vbox.addStretch()
controls_hbox.addLayout(right_controls_vbox, 1)
# end control box
add_layout_with_frame(vbox, controls_hbox, "Commands:")
# start settings
settings_hbox = QHBoxLayout()
settings_hbox.setMargin(0)
# start left column
left_settings_vbox = QVBoxLayout()
left_settings_vbox.setMargin(0)
# frame id
self.frame_id_line_edit = QLineEdit("/world")
add_widget_with_frame(left_settings_vbox, self.frame_id_line_edit,
"Frame ID:")
# do closing step
self.close_step_checkbox = QCheckBox()
self.close_step_checkbox.setText("Do closing step")
self.close_step_checkbox.setChecked(True)
left_settings_vbox.addWidget(self.close_step_checkbox)
# extra seperation
self.extra_seperation_checkbox = QCheckBox()
self.extra_seperation_checkbox.setText("Extra Seperation")
self.extra_seperation_checkbox.setChecked(False)
left_settings_vbox.addWidget(self.extra_seperation_checkbox)
left_settings_vbox.addStretch()
# number of steps
self.step_number = generate_q_double_spin_box(1, 1, 50, 0, 1.0)
add_widget_with_frame(left_settings_vbox, self.step_number,
"Number Steps:")
# start step index
self.start_step_index = generate_q_double_spin_box(0, 0, 1000, 0, 1.0)
add_widget_with_frame(left_settings_vbox, self.start_step_index,
"Start Step Index:")
# end left column
settings_hbox.addLayout(left_settings_vbox, 1)
# start center column
center_settings_vbox = QVBoxLayout()
center_settings_vbox.setMargin(0)
# start foot selection
self.start_foot_selection_combo_box = QComboBox()
self.start_foot_selection_combo_box.addItem("AUTO")
self.start_foot_selection_combo_box.addItem("LEFT")
self.start_foot_selection_combo_box.addItem("RIGHT")
add_widget_with_frame(center_settings_vbox,
self.start_foot_selection_combo_box,
"Start foot selection:")
center_settings_vbox.addStretch()
# step Distance
self.step_distance = generate_q_double_spin_box(0.0, 0.0, 0.5, 2, 0.01)
add_widget_with_frame(center_settings_vbox, self.step_distance,
"Step Distance (m):")
# side step distance
self.side_step = generate_q_double_spin_box(0.0, 0.0, 0.2, 2, 0.01)
add_widget_with_frame(center_settings_vbox, self.side_step,
"Side Step (m):")
# rotation per step
self.step_rotation = generate_q_double_spin_box(
0.0, -30.0, 30.0, 0, 1.0)
add_widget_with_frame(center_settings_vbox, self.step_rotation,
"Step Rotation (deg):")
# end center column
settings_hbox.addLayout(center_settings_vbox, 1)
# start right column
right_settings_vbox = QVBoxLayout()
right_settings_vbox.setMargin(0)
# roll
self.roll = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
add_widget_with_frame(right_settings_vbox, self.roll, "Roll (deg):")
# pitch
self.pitch = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
add_widget_with_frame(right_settings_vbox, self.pitch, "Pitch (deg):")
# use terrain model
self.use_terrain_model_checkbox = QCheckBox()
self.use_terrain_model_checkbox.setText("Use Terrain Model")
self.use_terrain_model_checkbox.setChecked(False)
self.use_terrain_model_checkbox.stateChanged.connect(
self.use_terrain_model_changed)
right_settings_vbox.addWidget(self.use_terrain_model_checkbox)
# override mode
self.override_checkbox = QCheckBox()
self.override_checkbox.setText("Override 3D")
self.override_checkbox.setChecked(False)
right_settings_vbox.addWidget(self.override_checkbox)
right_settings_vbox.addStretch()
# delta z
self.dz = generate_q_double_spin_box(0.0, -0.5, 0.5, 2, 0.01)
add_widget_with_frame(right_settings_vbox, self.dz,
"delta z per step (m):")
# end right column
settings_hbox.addLayout(right_settings_vbox, 1)
# end settings
add_layout_with_frame(vbox, settings_hbox, "Settings:")
# parameter set selection
self.parameter_set_widget = QParameterSetWidget(logger=self.logger)
add_widget_with_frame(vbox, self.parameter_set_widget,
"Parameter Set:")
# execute option
if add_execute_widget:
add_widget_with_frame(
vbox,
QExecuteStepPlanWidget(logger=self.logger,
step_plan_topic="step_plan"),
"Execute:")
# add error status widget
add_widget_with_frame(vbox, error_status_widget, "Status:")
# end widget
widget.setLayout(vbox)
#context.add_widget(widget)
# init widget
self.parameter_set_widget.param_cleared_signal.connect(
self.param_cleared)
self.parameter_set_widget.param_changed_signal.connect(
self.param_selected)
self.commands_set_enabled(False)
# subscriber
self.start_feet_sub = rospy.Subscriber("set_start_feet", Feet,
self.set_start_feet_callback)
# publisher
self.step_plan_pub = rospy.Publisher("step_plan",
StepPlan,
queue_size=1)
# action clients
self.step_plan_request_client = actionlib.SimpleActionClient(
"step_plan_request", StepPlanRequestAction)
def __init__(self, context):
super(PatternGeneratorWidget, self).__init__()
# publisher
self.pattern_generator_params_pub = rospy.Publisher(
'pattern_generator/set_params',
PatternGeneratorParameters,
queue_size=1)
# start widget
widget = context
# start upper part
hbox = QHBoxLayout()
# start left column
left_vbox = QVBoxLayout()
# start button
start_command = QPushButton("Start")
left_vbox.addWidget(start_command)
# simulation checkbox
self.simulation_mode_checkbox = QCheckBox()
self.simulation_mode_checkbox.setText("Simulation Mode")
self.simulation_mode_checkbox.setChecked(False)
left_vbox.addWidget(self.simulation_mode_checkbox)
# realtime checkbox
self.realtime_mode_checkbox = QCheckBox()
self.realtime_mode_checkbox.setText("Realtime Mode")
self.realtime_mode_checkbox.setChecked(False)
left_vbox.addWidget(self.realtime_mode_checkbox)
# joystick checkbox
self.joystick_mode_checkbox = QCheckBox()
self.joystick_mode_checkbox.setText("Joystick Mode")
self.joystick_mode_checkbox.setChecked(False)
left_vbox.addWidget(self.joystick_mode_checkbox)
# ignore invalid steps checkbox
self.ignore_invalid_steps_checkbox = QCheckBox()
self.ignore_invalid_steps_checkbox.setText("Ignore Invalid Steps")
self.ignore_invalid_steps_checkbox.setChecked(False)
left_vbox.addWidget(self.ignore_invalid_steps_checkbox)
# foot seperation
self.foot_seperation = generate_q_double_spin_box(
0.2, 0.15, 0.3, 2, 0.01)
self.foot_seperation.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.foot_seperation,
"Foot Seperation (m):")
# delta x
self.delta_x = generate_q_double_spin_box(0.0, -0.4, 0.4, 2, 0.01)
self.delta_x.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.delta_x, "dX (m):")
# delta y
self.delta_y = generate_q_double_spin_box(0.0, -2.2, 2.2, 2, 0.01)
self.delta_y.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.delta_y, "dY (m):")
# delta yaw
self.delta_yaw = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
self.delta_yaw.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.delta_yaw, "dYaw (deg):")
# roll
self.roll = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
self.roll.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.roll, "Roll (deg):")
# pitch
self.pitch = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
self.pitch.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.pitch, "Pitch (deg):")
# end left column
left_vbox.addStretch()
hbox.addLayout(left_vbox, 1)
# start right column
right_vbox = QVBoxLayout()
# stop button
stop_command = QPushButton("Stop")
right_vbox.addWidget(stop_command)
# ignore collision
self.collision_checkbox = QCheckBox()
self.collision_checkbox.setText("Ignore Collision")
self.collision_checkbox.setChecked(True)
right_vbox.addWidget(self.collision_checkbox)
# override 3D
self.override_checkbox = QCheckBox()
self.override_checkbox.setText("Override 3D")
self.override_checkbox.setChecked(False)
right_vbox.addWidget(self.override_checkbox)
# end right coloumn
right_vbox.addStretch()
hbox.addLayout(right_vbox, 1)
# add upper part
hbox.setMargin(0)
vbox = QVBoxLayout()
vbox.addLayout(hbox)
# parameter set selection
self.parameter_set_widget = QParameterSetWidget()
add_widget_with_frame(vbox, self.parameter_set_widget,
"Parameter Set:")
# end widget
widget.setLayout(vbox)
#context.add_widget(widget)
# signal connections
start_command.clicked.connect(self.start_command_callback)
stop_command.clicked.connect(self.stop_command_callback)
self.joystick_mode_checkbox.clicked.connect(
self.joystick_mode_check_callback)
self.ignore_invalid_steps_checkbox.clicked.connect(
self._publish_parameters)
def __init__(self, context):
super(PatternGeneratorWidget, self).__init__()
# publisher
self.pattern_generator_params_pub = rospy.Publisher('pattern_generator/set_params', PatternGeneratorParameters, queue_size = 1)
# start widget
widget = context
# start upper part
hbox = QHBoxLayout()
# start left column
left_vbox = QVBoxLayout()
# start button
start_command = QPushButton("Start")
start_command.clicked.connect(self.start_command_callback)
left_vbox.addWidget(start_command)
# simulation checkbox
self.simulation_mode_checkbox = QCheckBox()
self.simulation_mode_checkbox.setText("Simulation Mode")
self.simulation_mode_checkbox.setChecked(False)
left_vbox.addWidget(self.simulation_mode_checkbox)
# realtime checkbox
self.realtime_mode_checkbox = QCheckBox()
self.realtime_mode_checkbox.setText("Realtime Mode")
self.realtime_mode_checkbox.setChecked(False)
left_vbox.addWidget(self.realtime_mode_checkbox)
# joystick checkbox
self.joystick_mode_checkbox = QCheckBox()
self.joystick_mode_checkbox.setText("Joystick Mode")
self.joystick_mode_checkbox.setChecked(False)
self.joystick_mode_checkbox.clicked.connect(self.joystick_mode_check_callback)
left_vbox.addWidget(self.joystick_mode_checkbox)
# foot seperation
self.foot_seperation = generate_q_double_spin_box(0.2, 0.15, 0.3, 2, 0.01)
self.foot_seperation.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.foot_seperation, "Foot Seperation (m):")
# delta x
self.delta_x = generate_q_double_spin_box(0.0, -0.4, 0.4, 2, 0.01)
self.delta_x.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.delta_x, "dX (m):")
# delta y
self.delta_y = generate_q_double_spin_box(0.0, -2.2, 2.2, 2, 0.01)
self.delta_y.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.delta_y, "dY (m):")
# delta yaw
self.delta_yaw = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
self.delta_yaw.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.delta_yaw, "dYaw (deg):")
# roll
self.roll = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
self.roll.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.roll, "Roll (deg):")
# pitch
self.pitch = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
self.pitch.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.pitch, "Pitch (deg):")
# end left column
left_vbox.addStretch()
hbox.addLayout(left_vbox, 1)
# start right column
right_vbox = QVBoxLayout()
# stop button
stop_command = QPushButton("Stop")
stop_command.clicked.connect(self.stop_command_callback)
right_vbox.addWidget(stop_command)
# ignore collision
self.collision_checkbox = QCheckBox()
self.collision_checkbox.setText("Ignore Collision")
self.collision_checkbox.setChecked(True)
right_vbox.addWidget(self.collision_checkbox)
# override 3D
self.override_checkbox = QCheckBox()
self.override_checkbox.setText("Override 3D")
self.override_checkbox.setChecked(False)
right_vbox.addWidget(self.override_checkbox)
# end right coloumn
right_vbox.addStretch()
hbox.addLayout(right_vbox, 1)
# add upper part
hbox.setMargin(0)
vbox = QVBoxLayout()
vbox.addLayout(hbox)
# parameter set selection
self.parameter_set_widget = QParameterSetWidget()
add_widget_with_frame(vbox, self.parameter_set_widget, "Parameter Set:")
# end widget
widget.setLayout(vbox)
def __init__(self, context, add_execute_widget = True):
super(StepInterfaceWidget, self).__init__()
# init signal mapper
self.command_mapper = QSignalMapper(self)
self.command_mapper.mapped.connect(self._publish_step_plan_request)
# start widget
widget = context
error_status_widget = QErrorStatusWidget()
self.logger = Logger(error_status_widget)
vbox = QVBoxLayout()
# start control box
controls_hbox = QHBoxLayout()
# left coloumn
left_controls_vbox = QVBoxLayout()
left_controls_vbox.setMargin(0)
self.add_command_button(left_controls_vbox, "Rotate Left", PatternParameters.ROTATE_LEFT)
self.add_command_button(left_controls_vbox, "Strafe Left", PatternParameters.STRAFE_LEFT)
self.add_command_button(left_controls_vbox, "Step Up", PatternParameters.STEP_UP)
self.add_command_button(left_controls_vbox, "Center on Left", PatternParameters.FEET_REALIGN_ON_LEFT)
left_controls_vbox.addStretch()
controls_hbox.addLayout(left_controls_vbox, 1)
# center coloumn
center_controls_vbox = QVBoxLayout()
center_controls_vbox.setMargin(0)
self.add_command_button(center_controls_vbox, "Forward", PatternParameters.FORWARD)
self.add_command_button(center_controls_vbox, "Backward", PatternParameters.BACKWARD)
self.add_command_button(center_controls_vbox, "Step Over", PatternParameters.STEP_OVER)
self.add_command_button(center_controls_vbox, "Center Feet", PatternParameters.FEET_REALIGN_ON_CENTER)
self.add_command_button(center_controls_vbox, "Wide Stance", PatternParameters.WIDE_STANCE)
center_controls_vbox.addStretch()
controls_hbox.addLayout(center_controls_vbox, 1)
# right coloumn
right_controls_vbox = QVBoxLayout()
right_controls_vbox.setMargin(0)
self.add_command_button(right_controls_vbox, "Rotate Right", PatternParameters.ROTATE_RIGHT)
self.add_command_button(right_controls_vbox, "Strafe Right", PatternParameters.STRAFE_RIGHT)
self.add_command_button(right_controls_vbox, "Step Down", PatternParameters.STEP_DOWN)
self.add_command_button(right_controls_vbox, "Center on Right", PatternParameters.FEET_REALIGN_ON_RIGHT)
right_controls_vbox.addStretch()
controls_hbox.addLayout(right_controls_vbox, 1)
# end control box
add_layout_with_frame(vbox, controls_hbox, "Commands:")
# start settings
settings_hbox = QHBoxLayout()
settings_hbox.setMargin(0)
# start left column
left_settings_vbox = QVBoxLayout()
left_settings_vbox.setMargin(0)
# frame id
self.frame_id_line_edit = QLineEdit("/world")
add_widget_with_frame(left_settings_vbox, self.frame_id_line_edit, "Frame ID:")
# do closing step
self.close_step_checkbox = QCheckBox()
self.close_step_checkbox.setText("Do closing step")
self.close_step_checkbox.setChecked(True)
left_settings_vbox.addWidget(self.close_step_checkbox)
# extra seperation
self.extra_seperation_checkbox = QCheckBox()
self.extra_seperation_checkbox.setText("Extra Seperation")
self.extra_seperation_checkbox.setChecked(False)
left_settings_vbox.addWidget(self.extra_seperation_checkbox)
left_settings_vbox.addStretch()
# number of steps
self.step_number = generate_q_double_spin_box(1, 1, 50, 0, 1.0)
add_widget_with_frame(left_settings_vbox, self.step_number, "Number Steps:")
# start step index
self.start_step_index = generate_q_double_spin_box(0, 0, 1000, 0, 1.0)
add_widget_with_frame(left_settings_vbox, self.start_step_index, "Start Step Index:")
# end left column
settings_hbox.addLayout(left_settings_vbox, 1)
# start center column
center_settings_vbox = QVBoxLayout()
center_settings_vbox.setMargin(0)
# start foot selection
self.start_foot_selection_combo_box = QComboBox()
self.start_foot_selection_combo_box.addItem("AUTO")
self.start_foot_selection_combo_box.addItem("LEFT")
self.start_foot_selection_combo_box.addItem("RIGHT")
add_widget_with_frame(center_settings_vbox, self.start_foot_selection_combo_box, "Start foot selection:")
center_settings_vbox.addStretch()
# step Distance
self.step_distance = generate_q_double_spin_box(0.20, 0.0, 0.5, 2, 0.01)
add_widget_with_frame(center_settings_vbox, self.step_distance, "Step Distance (m):")
# side step distance
self.side_step = generate_q_double_spin_box(0.0, 0.0, 0.2, 2, 0.01)
add_widget_with_frame(center_settings_vbox, self.side_step, "Side Step (m):")
# rotation per step
self.step_rotation = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
add_widget_with_frame(center_settings_vbox, self.step_rotation, "Step Rotation (deg):")
# end center column
settings_hbox.addLayout(center_settings_vbox, 1)
# start right column
right_settings_vbox = QVBoxLayout()
right_settings_vbox.setMargin(0)
# roll
self.roll = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
add_widget_with_frame(right_settings_vbox, self.roll, "Roll (deg):")
# pitch
self.pitch = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
add_widget_with_frame(right_settings_vbox, self.pitch, "Pitch (deg):")
# use terrain model
self.use_terrain_model_checkbox = QCheckBox()
self.use_terrain_model_checkbox.setText("Use Terrain Model")
self.use_terrain_model_checkbox.setChecked(False)
self.use_terrain_model_checkbox.stateChanged.connect(self.use_terrain_model_changed)
right_settings_vbox.addWidget(self.use_terrain_model_checkbox)
# override mode
self.override_checkbox = QCheckBox()
self.override_checkbox.setText("Override 3D")
self.override_checkbox.setChecked(False)
right_settings_vbox.addWidget(self.override_checkbox)
right_settings_vbox.addStretch()
# delta z
self.dz = generate_q_double_spin_box(0.0, -0.5, 0.5, 2, 0.01)
add_widget_with_frame(right_settings_vbox, self.dz, "delta z per step (m):")
# end right column
settings_hbox.addLayout(right_settings_vbox, 1)
# end settings
add_layout_with_frame(vbox, settings_hbox, "Settings:")
# parameter set selection
self.parameter_set_widget = QParameterSetWidget(logger = self.logger)
add_widget_with_frame(vbox, self.parameter_set_widget, "Parameter Set:")
# execute option
if add_execute_widget:
add_widget_with_frame(vbox, QExecuteStepPlanWidget(logger = self.logger, step_plan_topic = "step_plan"), "Execute:")
# add error status widget
add_widget_with_frame(vbox, error_status_widget, "Status:")
# end widget
widget.setLayout(vbox)
#context.add_widget(widget)
# init widget
self.parameter_set_widget.param_cleared_signal.connect(self.param_cleared)
self.parameter_set_widget.param_changed_signal.connect(self.param_selected)
self.commands_set_enabled(False)
# subscriber
self.start_feet_sub = rospy.Subscriber("set_start_feet", Feet, self.set_start_feet_callback)
# publisher
self.step_plan_pub = rospy.Publisher("step_plan", StepPlan, queue_size=1)
# action clients
self.step_plan_request_client = actionlib.SimpleActionClient("step_plan_request", StepPlanRequestAction)
def __init__(self, context):
super(SinusoidalTrajectoryDialog, self).__init__(context)
self.setObjectName('SinusoidalTrajectoryDialog')
#self.updateStateSignal = Signal(object)
self.updateStateSignal.connect(self.on_updateState)
self.robot = URDF.from_parameter_server()
self.joint_list = {}
for ndx,jnt in enumerate(self.robot.joints):
self.joint_list[jnt.name] = ndx
#print jnt.name, " ",self.joint_list[jnt.name]
self.chain=[]
self.chain_file = rospy.get_param("chain_file")
self.chain_name = rospy.get_param("chain_name")
yaml_file = self.chain_file+self.chain_name+"_chain.yaml"
print yaml_file
#define structures
self.robot_state = JointState()
self.robot_command = JointTrajectory()
stream = open(yaml_file, "r")
jointChain = yaml.load_all(stream)
print '\n\n'
for ndx, data in enumerate(jointChain):
print ndx," : ", data
self.delay_time = data["delay_time"]
self.amplitude = data["amplitude"]
self.frequency = data["frequency"]
self.frequency_limit = data["frequency_limit"]
self.iterations = data["iterations"]
self.joint_state_topic = data["joint_state_topic"]
self.trajectory_topic = data["trajectory_topic"]
joints = rospy.get_param(data["chain_param_name"])
for joint in joints:
print joint
self.robot_state.name.append(joint)
self.chain.append(JointData(self, joint) )
self.robot_command.joint_names = self.robot_state.name
stream.close()
self.robot_state.position = [0.0]*len(self.robot_state.name)
self.robot_state.velocity = [0.0]*len(self.robot_state.name)
self.robot_state.effort = [0.0]*len(self.robot_state.name)
self.robot_joint_state = JointState()
print "delay_time =",self.delay_time
print "amplitude =",self.amplitude
print "frequency =",self.frequency
print "iterations =",self.iterations
print "Robot State Structure",self.robot_state
print "Robot Command Structure",self.robot_command
# initialize structure to hold widget handles
self.cur_position_spinbox=[]
self.amplitude_spinbox=[]
self.frequency_spinbox=[]
self.iterations_spinbox=[]
self._widget = QWidget()
vbox = QVBoxLayout()
# Push buttons
hbox = QHBoxLayout()
snap_command = QPushButton("Snap Position")
snap_command.clicked.connect(self.snap_current_callback)
hbox.addWidget(snap_command)
check_limits = QPushButton("Check Limits Gains")
check_limits.clicked.connect(self.check_limits_callback)
hbox.addWidget(check_limits)
apply_command = QPushButton("Send Trajectory")
apply_command.clicked.connect(self.apply_command_callback)
hbox.addWidget(apply_command)
save_trajectory = QPushButton("Save Trajectory")
save_trajectory.clicked.connect(self.save_trajectory_callback)
hbox.addWidget(save_trajectory)
zero_ramp = QPushButton("Zero Values")
zero_ramp.clicked.connect(self.zero_values_callback)
hbox.addWidget(zero_ramp)
vbox.addLayout(hbox)
time_hbox = QHBoxLayout()
vbox_frequqency = QVBoxLayout()
vbox_frequqency.addWidget(QLabel("Frequency"))
self.frequency_spinbox = QDoubleSpinBox()
self.frequency_spinbox.setDecimals(5)
self.frequency_spinbox.setRange(0, self.frequency_limit)
self.frequency_spinbox.setSingleStep(0.05)
self.frequency_spinbox.valueChanged.connect(self.on_frequency_value)
self.frequency_spinbox.setValue(self.frequency)
vbox_frequqency.addWidget(self.frequency_spinbox)
time_hbox.addLayout(vbox_frequqency)
vbox_iterations = QVBoxLayout()
vbox_iterations.addWidget(QLabel("Iterations"))
self.iterations_spinbox = QDoubleSpinBox()
self.iterations_spinbox.setDecimals(5)
self.iterations_spinbox.setRange(0, 10)
self.iterations_spinbox.setSingleStep(1)
self.iterations_spinbox.valueChanged.connect(self.on_iterations_value)
self.iterations_spinbox.setValue(self.iterations)
vbox_iterations.addWidget(self.iterations_spinbox)
time_hbox.addLayout(vbox_iterations)
vbox.addLayout(time_hbox)
# Joints title
title_frame = QFrame()
title_frame.setFrameShape(QFrame.StyledPanel);
title_frame.setFrameShadow(QFrame.Raised);
title_hbox = QHBoxLayout()
title_hbox.addWidget(QLabel("Joints"))
title_hbox.addWidget(QLabel("Current Position"))
title_hbox.addWidget(QLabel("Amplitude"))
title_frame.setLayout(title_hbox)
vbox.addWidget(title_frame)
# Define the widgets for each joint
for i,joint in enumerate(self.chain):
#print i,",",joint
self.joint_widget( vbox, i)
#add stretch at end so all GUI elements are at top of dialog
vbox.addStretch(1)
self._widget.setLayout(vbox)
# Define the connections to the outside world
self.jointSubscriber = rospy.Subscriber(self.joint_state_topic, JointState, self.stateCallbackFnc)
self.commandPublisher = rospy.Publisher(self.trajectory_topic , JointTrajectory, queue_size=10)
# Add the widget
context.add_widget(self._widget)
def __init__(self, context):
super(TrapezoidalTrajectoryDialog, self).__init__(context)
self.setObjectName("TrapezoidalTrajectoryDialog")
# self.updateStateSignal = Signal(object)
self.updateStateSignal.connect(self.on_updateState)
self.robot = URDF.from_parameter_server()
self.joint_list = {}
for ndx, jnt in enumerate(self.robot.joints):
self.joint_list[jnt.name] = ndx
self.chain = []
self.chain_file = rospy.get_param("~chain_file")
self.chain_name = rospy.get_param("~chain_name")
print
print "Define order of splines used to approximate trapezoid"
self.spline_order = rospy.get_param("~spline_order", 5) # 1 # 3 # 5 # linear, cubic, quintic
if (self.spline_order == 1) or (self.spline_order == 3) or (self.spline_order == 5):
print "Spline order=", self.spline_order
else:
print "Invalid spline order! Must be 1, 3, or 5"
print "Spline order=", self.spline_order
sys.exit(-1)
yaml_file = self.chain_file + self.chain_name + "_chain.yaml"
print "Chain definition file:"
print yaml_file
print
# define structures
self.robot_state = JointState()
self.robot_command = JointTrajectory()
stream = open(yaml_file, "r")
jointChain = yaml.load_all(stream)
for ndx, data in enumerate(jointChain):
print ndx, " : ", data
self.delay_time = data["delay_time"]
self.ramp_up_time = data["ramp_up_time"]
self.dwell_time = data["dwell_time"]
self.ramp_down_time = data["ramp_down_time"]
self.hold_time = data["hold_time"]
self.ramp_start_fraction = data["ramp_start_fraction"]
self.ramp_end_fraction = data["ramp_end_fraction"]
self.joint_state_topic = data["joint_state_topic"]
self.trajectory_topic = data["trajectory_topic"]
if rospy.search_param(data["chain_param_name"]):
print "Found ", data["chain_param_name"]
else:
print "Failed to find the ", data["chain_param_name"], " in the parameter server!"
sys.exit(-1)
joint_names = rospy.get_param(data["chain_param_name"])
for joint in joint_names:
print joint
self.robot_state.name.append(joint)
self.chain.append(JointData(self, joint))
self.robot_command.joint_names = self.robot_state.name
stream.close()
self.robot_state.position = [0.0] * len(self.robot_state.name)
self.robot_state.velocity = [0.0] * len(self.robot_state.name)
self.robot_state.effort = [0.0] * len(self.robot_state.name)
self.robot_joint_state = JointState()
print "delay_time =", self.delay_time
print "ramp_up_time =", self.ramp_up_time
print "dwell_time =", self.dwell_time
print "ramp_down_time=", self.ramp_down_time
print "hold_time =", self.hold_time
print "spline order =", self.spline_order
if (
(self.ramp_start_fraction < 0.001)
or (self.ramp_end_fraction > 0.999)
or (self.ramp_start_fraction >= self.ramp_end_fraction)
):
print "Invalid ramp fractions - abort!"
print "0.0 < ", self.ramp_start_fraction, " < ", self.ramp_end_fraction, " < 1.0"
return
print "Robot State Structure", self.robot_state
print "Robot Command Structure", self.robot_command
# initialize structure to hold widget handles
self.cmd_spinbox = []
self.ramp_up_spinbox = []
self.ramp_down_spinbox = []
self._widget = QWidget()
vbox = QVBoxLayout()
# Push buttons
hbox = QHBoxLayout()
snap_command = QPushButton("Snap Position")
snap_command.clicked.connect(self.snap_current_callback)
hbox.addWidget(snap_command)
check_limits = QPushButton("Check Limits Gains")
check_limits.clicked.connect(self.check_limits_callback)
hbox.addWidget(check_limits)
apply_command = QPushButton("Send Trajectory")
apply_command.clicked.connect(self.apply_command_callback)
hbox.addWidget(apply_command)
save_trajectory = QPushButton("Save Trajectory")
save_trajectory.clicked.connect(self.save_trajectory_callback)
hbox.addWidget(save_trajectory)
zero_ramp = QPushButton("Zero Ramps")
zero_ramp.clicked.connect(self.zero_ramp_callback)
hbox.addWidget(zero_ramp)
vbox.addLayout(hbox)
time_hbox = QHBoxLayout()
vbox_delay = QVBoxLayout()
vbox_delay.addWidget(QLabel("Delay"))
self.delay_time_spinbox = QDoubleSpinBox()
self.delay_time_spinbox.setDecimals(5)
self.delay_time_spinbox.setRange(0, 10.0)
self.delay_time_spinbox.setSingleStep(0.1)
self.delay_time_spinbox.valueChanged.connect(self.on_delay_time_value)
self.delay_time_spinbox.setValue(self.delay_time)
vbox_delay.addWidget(self.delay_time_spinbox)
time_hbox.addLayout(vbox_delay)
vbox_ramp_up = QVBoxLayout()
vbox_ramp_up.addWidget(QLabel("Ramp Up"))
self.ramp_up_time_spinbox = QDoubleSpinBox()
self.ramp_up_time_spinbox.setDecimals(5)
self.ramp_up_time_spinbox.setRange(0, 10.0)
self.ramp_up_time_spinbox.setSingleStep(0.1)
self.ramp_up_time_spinbox.valueChanged.connect(self.on_ramp_up_time_value)
self.ramp_up_time_spinbox.setValue(self.ramp_up_time)
vbox_ramp_up.addWidget(self.ramp_up_time_spinbox)
time_hbox.addLayout(vbox_ramp_up)
#
vbox_dwell = QVBoxLayout()
vbox_dwell.addWidget(QLabel("Dwell"))
self.dwell_time_spinbox = QDoubleSpinBox()
self.dwell_time_spinbox.setDecimals(5)
self.dwell_time_spinbox.setRange(0, 10.0)
self.dwell_time_spinbox.setSingleStep(0.1)
self.dwell_time_spinbox.valueChanged.connect(self.on_dwell_time_value)
self.dwell_time_spinbox.setValue(self.dwell_time)
vbox_dwell.addWidget(self.dwell_time_spinbox)
time_hbox.addLayout(vbox_dwell)
vbox_ramp_down = QVBoxLayout()
vbox_ramp_down.addWidget(QLabel("Down"))
self.ramp_down_time_spinbox = QDoubleSpinBox()
self.ramp_down_time_spinbox.setDecimals(5)
self.ramp_down_time_spinbox.setRange(0, 10.0)
self.ramp_down_time_spinbox.setSingleStep(0.1)
self.ramp_down_time_spinbox.valueChanged.connect(self.on_ramp_down_time_value)
self.ramp_down_time_spinbox.setValue(self.ramp_down_time)
vbox_ramp_down.addWidget(self.ramp_down_time_spinbox)
time_hbox.addLayout(vbox_ramp_down)
vbox_hold = QVBoxLayout()
vbox_hold.addWidget(QLabel("Hold"))
self.hold_time_spinbox = QDoubleSpinBox()
self.hold_time_spinbox.setDecimals(5)
self.hold_time_spinbox.setRange(0, 10.0)
self.hold_time_spinbox.setSingleStep(0.1)
self.hold_time_spinbox.valueChanged.connect(self.on_hold_time_value)
self.hold_time_spinbox.setValue(self.hold_time)
vbox_hold.addWidget(self.hold_time_spinbox)
time_hbox.addLayout(vbox_hold)
vbox.addLayout(time_hbox)
# Joints title
title_frame = QFrame()
title_frame.setFrameShape(QFrame.StyledPanel)
title_frame.setFrameShadow(QFrame.Raised)
title_hbox = QHBoxLayout()
title_hbox.addWidget(QLabel("Joints"))
title_hbox.addWidget(QLabel("Start"))
title_hbox.addWidget(QLabel("Ramp Up"))
title_hbox.addWidget(QLabel("Ramp Down"))
title_frame.setLayout(title_hbox)
vbox.addWidget(title_frame)
# Define the widgets for each joint
for i, joint in enumerate(self.chain):
# print i,",",joint
self.joint_widget(vbox, i)
# add stretch at end so all GUI elements are at top of dialog
vbox.addStretch(1)
self._widget.setLayout(vbox)
# Define the connections to the outside world
self.jointSubscriber = rospy.Subscriber(self.joint_state_topic, JointState, self.stateCallbackFnc)
self.commandPublisher = rospy.Publisher(self.trajectory_topic, JointTrajectory, queue_size=10)
# Add the widget
context.add_widget(self._widget)
