class PathEditor(QWidget):
'''
This is a path editor used as ItemDeligate in settings view. This editor
provides an additional button for directory selection dialog.
'''
editing_finished_signal = Signal()
def __init__(self, path, parent=None):
QWidget.__init__(self, parent)
self.path = path
self._layout = QHBoxLayout(self)
self._layout.setContentsMargins(0, 0, 0, 0)
self._layout.setSpacing(0)
self._button = QPushButton('...')
self._button.setMaximumSize(QSize(24, 20))
self._button.clicked.connect(self._on_path_select_clicked)
self._layout.addWidget(self._button)
self._lineedit = QLineEdit(path)
self._lineedit.returnPressed.connect(self._on_editing_finished)
self._layout.addWidget(self._lineedit)
self.setLayout(self._layout)
self.setFocusProxy(self._button)
self.setAutoFillBackground(True)
def _on_path_select_clicked(self):
# Workaround for QFileDialog.getExistingDirectory because it do not
# select the configuration folder in the dialog
self.dialog = QFileDialog(self, caption='Select a new settings folder')
self.dialog.setOption(QFileDialog.HideNameFilterDetails, True)
self.dialog.setFileMode(QFileDialog.Directory)
self.dialog.setDirectory(self.path)
if self.dialog.exec_():
fileNames = self.dialog.selectedFiles()
path = fileNames[0]
if os.path.isfile(path):
path = os.path.basename(path)
self._lineedit.setText(path)
self.path = dir
self.editing_finished_signal.emit()
def _on_editing_finished(self):
if self._lineedit.text():
self.path = self._lineedit.text()
self.editing_finished_signal.emit()
class PathEditor(QWidget):
'''
This is a path editor used as ItemDeligate in settings view. This editor
provides an additional button for directory selection dialog.
'''
editing_finished_signal = Signal()
def __init__(self, path, parent=None):
QWidget.__init__(self, parent)
self.path = path
self._layout = QHBoxLayout(self)
self._layout.setContentsMargins(0, 0, 0, 0)
self._layout.setSpacing(0)
self._button = QPushButton('...')
self._button.setMaximumSize(QSize(24, 20))
self._button.clicked.connect(self._on_path_select_clicked)
self._layout.addWidget(self._button)
self._lineedit = QLineEdit(path)
self._lineedit.returnPressed.connect(self._on_editing_finished)
self._layout.addWidget(self._lineedit)
self.setLayout(self._layout)
self.setFocusProxy(self._button)
self.setAutoFillBackground(True)
def _on_path_select_clicked(self):
# Workaround for QFileDialog.getExistingDirectory because it do not
# select the configuration folder in the dialog
self.dialog = QFileDialog(self, caption='Select a new settings folder')
self.dialog.setOption(QFileDialog.HideNameFilterDetails, True)
self.dialog.setFileMode(QFileDialog.Directory)
self.dialog.setDirectory(self.path)
if self.dialog.exec_():
fileNames = self.dialog.selectedFiles()
path = fileNames[0]
if os.path.isfile(path):
path = os.path.basename(path)
self._lineedit.setText(path)
self.path = dir
self.editing_finished_signal.emit()
def _on_editing_finished(self):
if self._lineedit.text():
self.path = self._lineedit.text()
self.editing_finished_signal.emit()
class Top(Plugin):
NODE_FIELDS = [ 'pid', 'get_cpu_percent', 'get_memory_percent', 'get_num_threads']
OUT_FIELDS = ['node_name', 'pid', 'cpu_percent', 'memory_percent', 'num_threads' ]
FORMAT_STRS = ['%s', '%s', '%0.2f', '%0.2f', '%s' ]
NODE_LABELS = ['Node', 'PID', 'CPU %', 'Mem %', 'Num Threads' ]
SORT_TYPE = [str, str, float, float, float ]
TOOLTIPS = {
0: ('cmdline', lambda x: '\n'.join(textwrap.wrap(' '.join(x)))),
3: ('memory_info', lambda x: ('Resident: %0.2f MiB, Virtual: %0.2f MiB' % (x[0]/2**20, x[1]/2**20)))
}
_node_info = NodeInfo()
name_filter = re.compile('')
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())
# if not args.quiet:
# print 'arguments: ', args
# print 'unknowns: ', unknowns
self._selected_node = ''
self._selected_node_lock = RLock()
# Setup the toolbar
self._toolbar = QToolBar()
self._filter_box = QLineEdit()
self._regex_box = QCheckBox()
self._regex_box.setText('regex')
self._toolbar.addWidget(QLabel('Filter'))
self._toolbar.addWidget(self._filter_box)
self._toolbar.addWidget(self._regex_box)
self._filter_box.returnPressed.connect(self.update_filter)
self._regex_box.stateChanged.connect(self.update_filter)
# Create a container widget and give it a layout
self._container = QWidget()
self._container.setWindowTitle('Process Monitor')
self._layout = QVBoxLayout()
self._container.setLayout(self._layout)
self._layout.addWidget(self._toolbar)
# Create the table widget
self._table_widget = QTreeWidget()
self._table_widget.setObjectName('TopTable')
self._table_widget.setColumnCount(len(self.NODE_LABELS))
self._table_widget.setHeaderLabels(self.NODE_LABELS)
self._table_widget.itemClicked.connect(self._tableItemClicked)
self._table_widget.setSortingEnabled(True)
self._table_widget.setAlternatingRowColors(True)
self._layout.addWidget(self._table_widget)
context.add_widget(self._container)
# Add a button for killing nodes
self._kill_button = QPushButton('Kill Node')
self._layout.addWidget(self._kill_button)
self._kill_button.clicked.connect(self._kill_node)
# Update twice since the first cpu% lookup will always return 0
self.update_table()
self.update_table()
self._table_widget.resizeColumnToContents(0)
# Start a timer to trigger updates
self._update_timer = QTimer()
self._update_timer.setInterval(1000)
self._update_timer.timeout.connect(self.update_table)
self._update_timer.start()
def _tableItemClicked(self, item, column):
with self._selected_node_lock:
self._selected_node = item.text(0)
def update_filter(self, *args):
if self._regex_box.isChecked():
expr = self._filter_box.text()
else:
expr = re.escape(self._filter_box.text())
self.name_filter = re.compile(expr)
self.update_table()
def _kill_node(self):
self._node_info.kill_node(self._selected_node)
def update_one_item(self, row, info):
twi = TopWidgetItem()
for col, field in enumerate(self.OUT_FIELDS):
val = info[field]
twi.setText(col, self.FORMAT_STRS[col] % val)
self._table_widget.insertTopLevelItem(row, twi)
for col, (key, func) in self.TOOLTIPS.iteritems():
twi.setToolTip(col, func(info[key]))
with self._selected_node_lock:
if twi.text(0) == self._selected_node:
twi.setSelected(True)
self._table_widget.setItemHidden(twi, len(self.name_filter.findall(info['node_name'])) == 0)
def update_table(self):
self._table_widget.clear()
infos = self._node_info.get_all_node_fields(self.NODE_FIELDS)
for nx, info in enumerate(infos):
self.update_one_item(nx, info)
def shutdown_plugin(self):
self._update_timer.stop()
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('filter_text', self._filter_box.text())
instance_settings.set_value('is_regex', int(self._regex_box.checkState()))
def restore_settings(self, plugin_settings, instance_settings):
self._filter_box.setText(instance_settings.value('filter_text'))
is_regex_int = instance_settings.value('is_regex')
if is_regex_int:
self._regex_box.setCheckState(Qt.CheckState(is_regex_int))
else:
self._regex_box.setCheckState(Qt.CheckState(0))
self.update_filter()
class QuestionDialogPlugin(Plugin):
def __init__(self, context):
super(QuestionDialogPlugin, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('QuestionDialogPlugin')
# Create QWidget
self._widget = QWidget()
self._widget.setFont(QFont("Times", 14, QFont.Bold))
self._layout = QVBoxLayout(self._widget)
self._text_browser = QTextBrowser(self._widget)
self._layout.addWidget(self._text_browser)
self._button_layout = QHBoxLayout()
self._layout.addLayout(self._button_layout)
# layout = QVBoxLayout(self._widget)
# layout.addWidget(self.button)
self._widget.setObjectName('QuestionDialogPluginUI')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
context.add_widget(self._widget)
# Setup service provider
self.service = rospy.Service('question_dialog', QuestionDialog,
self.service_callback)
self.response_ready = False
self.response = None
self.buttons = []
self.text_label = None
self.text_input = None
self.connect(self._widget, SIGNAL("update"), self.update)
self.connect(self._widget, SIGNAL("timeout"), self.timeout)
def shutdown_plugin(self):
self.service.shutdown()
def service_callback(self, req):
self.response_ready = False
self.request = req
self._widget.emit(SIGNAL("update"))
# Start timer against wall clock here instead of the ros clock.
start_time = time.time()
while not self.response_ready:
if req.timeout != QuestionDialogRequest.NO_TIMEOUT:
current_time = time.time()
if current_time - start_time > req.timeout:
self._widget.emit(SIGNAL("timeout"))
return QuestionDialogResponse(
QuestionDialogRequest.TIMED_OUT, "")
time.sleep(0.2)
return self.response
def update(self):
self.clean()
req = self.request
self._text_browser.setText(req.message)
if req.type == QuestionDialogRequest.DISPLAY:
# All done, nothing more too see here.
self.response = QuestionDialogResponse(
QuestionDialogRequest.NO_RESPONSE, "")
self.response_ready = True
elif req.type == QuestionDialogRequest.CHOICE_QUESTION:
for index, options in enumerate(req.options):
button = QPushButton(options, self._widget)
button.clicked.connect(partial(self.handle_button, index))
self._button_layout.addWidget(button)
self.buttons.append(button)
elif req.type == QuestionDialogRequest.TEXT_QUESTION:
self.text_label = QLabel("Enter here: ", self._widget)
self._button_layout.addWidget(self.text_label)
self.text_input = QLineEdit(self._widget)
self.text_input.editingFinished.connect(self.handle_text)
self._button_layout.addWidget(self.text_input)
def timeout(self):
self._text_browser.setText("Oh no! The request timed out.")
self.clean()
def clean(self):
while self._button_layout.count():
item = self._button_layout.takeAt(0)
item.widget().deleteLater()
self.buttons = []
self.text_input = None
self.text_label = None
def handle_button(self, index):
self.response = QuestionDialogResponse(index, "")
self.clean()
self.response_ready = True
def handle_text(self):
self.response = QuestionDialogResponse(
QuestionDialogRequest.TEXT_RESPONSE,
self.text_input.text())
self.clean()
self.response_ready = True
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
class StepInterfaceWidget(QObject):
command_buttons = []
start_feet = Feet()
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 shutdown_plugin(self):
print "Shutting down ..."
print "Done!"
def add_command_button(self, parent, text, command):
button = QPushButton(text)
self.command_mapper.setMapping(button, command)
button.clicked.connect(self.command_mapper.map)
parent.addWidget(button)
self.command_buttons.append(button)
return button
def set_start_feet_callback(self, feet):
self.start_feet = feet
# message publisher
def _publish_step_plan_request(self, walk_command):
params = PatternParameters()
params.steps = self.step_number.value()
params.mode = walk_command
params.close_step = self.close_step_checkbox.isChecked()
params.extra_seperation = self.extra_seperation_checkbox.isChecked()
params.use_terrain_model = self.use_terrain_model_checkbox.isChecked()
params.override = self.override_checkbox.isChecked(
) and not self.use_terrain_model_checkbox.isChecked()
params.roll = math.radians(self.roll.value())
params.pitch = math.radians(self.pitch.value())
params.dz = self.dz.value()
params.step_distance_forward = self.step_distance.value()
params.step_distance_sideward = self.side_step.value()
params.turn_angle = math.radians(self.step_rotation.value())
request = StepPlanRequest()
request.header = std_msgs.msg.Header()
request.header.stamp = rospy.Time.now()
request.header.frame_id = self.frame_id_line_edit.text()
request.start = self.start_feet
request.start_step_index = self.start_step_index.value()
if self.start_foot_selection_combo_box.currentText() == "AUTO":
request.start_foot_selection = StepPlanRequest.AUTO
elif self.start_foot_selection_combo_box.currentText() == "LEFT":
request.start_foot_selection = StepPlanRequest.LEFT
elif self.start_foot_selection_combo_box.currentText() == "RIGHT":
request.start_foot_selection = StepPlanRequest.RIGHT
else:
self.logger.log_error(
"Unknown start foot selection mode ('" +
self.start_foot_selection_combo_box.currentText() + "')!")
return
if walk_command == PatternParameters.FORWARD:
params.mode = PatternParameters.SAMPLING
elif walk_command == PatternParameters.BACKWARD:
params.mode = PatternParameters.SAMPLING
params.step_distance_forward *= -1
params.step_distance_sideward *= -1
params.turn_angle *= -1
request.pattern_parameters = params
request.planning_mode = StepPlanRequest.PLANNING_MODE_PATTERN
request.parameter_set_name.data = self.parameter_set_widget.current_parameter_set_name(
)
print "Send request = ", request
# send request
if self.step_plan_request_client.wait_for_server(rospy.Duration(0.5)):
self.logger.log_info("Sending footstep plan request...")
goal = StepPlanRequestGoal()
goal.plan_request = request
self.step_plan_request_client.send_goal(goal)
if self.step_plan_request_client.wait_for_result(
rospy.Duration(5.0)):
self.logger.log_info("Received footstep plan!")
self.logger.log(
self.step_plan_request_client.get_result().status)
self.step_plan_pub.publish(
self.step_plan_request_client.get_result().step_plan)
else:
self.logger.log_error(
"Didn't received any results. Check communcation!")
else:
self.logger.log_error(
"Can't connect to footstep planner action server!")
def commands_set_enabled(self, enable):
for button in self.command_buttons:
button.setEnabled(enable)
@Slot()
def param_cleared(self):
self.commands_set_enabled(False)
@Slot(str)
def param_selected(self, name):
self.commands_set_enabled(True)
@Slot(int)
def use_terrain_model_changed(self, state):
enable_override = True
if state == Qt.Checked:
enable_override = False
self.roll.setEnabled(enable_override)
self.pitch.setEnabled(enable_override)
self.override_checkbox.setEnabled(enable_override)
self.dz.setEnabled(enable_override)
class QuestionDialogPlugin(Plugin):
def __init__(self, context):
super(QuestionDialogPlugin, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('QuestionDialogPlugin')
font_size = rospy.get_param("~font_size", 40)
# Create QWidget
self._widget = QWidget()
self._widget.setFont(QFont("Times", font_size, QFont.Bold))
self._layout = QVBoxLayout(self._widget)
self._text_browser = QTextBrowser(self._widget)
self._layout.addWidget(self._text_browser)
self._button_layout = QHBoxLayout()
self._layout.addLayout(self._button_layout)
# layout = QVBoxLayout(self._widget)
# layout.addWidget(self.button)
self._widget.setObjectName('QuestionDialogPluginUI')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
context.add_widget(self._widget)
# Setup service provider
self.service = rospy.Service('question_dialog', QuestionDialog,
self.service_callback)
self.response_ready = False
self.response = None
self.buttons = []
self.text_label = None
self.text_input = None
self.connect(self._widget, SIGNAL("update"), self.update)
self.connect(self._widget, SIGNAL("timeout"), self.timeout)
def shutdown_plugin(self):
self.service.shutdown()
def service_callback(self, req):
self.response_ready = False
self.request = req
self._widget.emit(SIGNAL("update"))
# Start timer against wall clock here instead of the ros clock.
start_time = time.time()
while not self.response_ready:
if self.request != req:
# The request got preempted by a new request.
return QuestionDialogResponse(QuestionDialogRequest.PREEMPTED, "")
if req.timeout != QuestionDialogRequest.NO_TIMEOUT:
current_time = time.time()
if current_time - start_time > req.timeout:
self._widget.emit(SIGNAL("timeout"))
return QuestionDialogResponse(
QuestionDialogRequest.TIMED_OUT, "")
time.sleep(0.2)
return self.response
def update(self):
self.clean()
req = self.request
self._text_browser.setText(req.message)
if req.type == QuestionDialogRequest.DISPLAY:
# All done, nothing more too see here.
self.response = QuestionDialogResponse(
QuestionDialogRequest.NO_RESPONSE, "")
self.response_ready = True
elif req.type == QuestionDialogRequest.CHOICE_QUESTION:
for index, options in enumerate(req.options):
button = QPushButton(options, self._widget)
button.clicked.connect(partial(self.handle_button, index))
self._button_layout.addWidget(button)
self.buttons.append(button)
elif req.type == QuestionDialogRequest.TEXT_QUESTION:
self.text_label = QLabel("Enter here: ", self._widget)
self._button_layout.addWidget(self.text_label)
self.text_input = QLineEdit(self._widget)
self.text_input.editingFinished.connect(self.handle_text)
self._button_layout.addWidget(self.text_input)
def timeout(self):
self._text_browser.setText("Oh no! The request timed out.")
self.clean()
def clean(self):
while self._button_layout.count():
item = self._button_layout.takeAt(0)
item.widget().deleteLater()
self.buttons = []
self.text_input = None
self.text_label = None
def handle_button(self, index):
self.response = QuestionDialogResponse(index, "")
self.clean()
self.response_ready = True
def handle_text(self):
self.response = QuestionDialogResponse(
QuestionDialogRequest.TEXT_RESPONSE,
self.text_input.text())
self.clean()
self.response_ready = True
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
class LocationFunction(object):
EDIT_LOCATION_PROPERITIES = 'Edit Location Properties'
ADD_LOCATION_AREA = 'Add Location'
EDIT_EXISTING_AREA = 'Edit Location'
def __init__(self, location_file, map, widget, subfunction_layout,
configuration_layout, image):
self.edit_area_button = None
self.edit_area_selection_color = Qt.black
# Dictionary of polygons
self.locations = {}
# Dictionary that maps location names to their colors
self.location_colors = {}
self.draw_location = {}
self.unique_loc_counter = 1
self.editing_area = False
self.edit_existing_location = None
self.editing_properties = False
self.edit_properties_location = None
# Use this to initialize variables.
self.clearAreaSelection()
self.is_modified = False
self.widget = widget
self.subfunction_layout = subfunction_layout
self.image = image
self.image_size = image.overlay_image.size()
self.configuration_layout = configuration_layout
self.location_file = location_file
self.map_size = QSize(map.map.info.width, map.map.info.height)
self.readLocationsFromFile()
self.edit_area_button = {}
def readLocationsFromFile(self):
if os.path.isfile(self.location_file):
stream = open(self.location_file, 'r')
try:
contents = yaml.load(stream)
if "polygons" not in contents or "locations" not in contents:
rospy.logerr(
"YAML file found at " + self.location_file +
", but does not seem to have been written by this tool. I'm starting locations from scratch."
)
else:
location_keys = contents["locations"]
location_polygons = contents["polygons"]
for index, location in enumerate(location_keys):
self.locations[location] = QPolygon()
self.locations[location].setPoints(
location_polygons[index])
self.locations[location] = scalePolygon(
self.locations[location], self.map_size,
self.image_size)
(_, self.location_colors[location]
) = self.getUniqueNameAndColor()
self.draw_location[location] = True
except yaml.YAMLError:
rospy.logerr(
"File found at " + self.location_file +
", but cannot be parsed by YAML parser. I'm starting locations from scratch."
)
stream.close()
else:
rospy.logwarn(
"Location file not found at " + self.location_file +
". I'm starting locations from scratch and will attempt to write to this location before exiting."
)
def saveConfiguration(self):
self.writeLocationsToFile()
def writeLocationsToFile(self):
out_dict = {}
out_dict["locations"] = self.locations.keys()
out_dict["polygons"] = []
for index, location in enumerate(self.locations):
out_dict["polygons"].append([])
for i in range(self.locations[location].size()):
pt = self.locations[location].point(i)
scaled_pt = scalePoint(pt, self.image_size, self.map_size)
out_dict["polygons"][index].append(scaled_pt.x())
out_dict["polygons"][index].append(scaled_pt.y())
data_directory = os.path.dirname(os.path.realpath(self.location_file))
image_file = getLocationsImageFileLocationFromDataDirectory(
data_directory)
# Create an image with the location data, so that C++ programs don't need to rely on determining regions using polygons.
out_dict["data"] = 'locations.pgm'
location_image = QImage(self.map_size, QImage.Format_RGB32)
location_image.fill(Qt.white)
painter = QPainter(location_image)
for index, location in enumerate(self.locations):
if index > 254:
rospy.logerr(
"You have more than 254 locations, which is unsupported by the bwi_planning_common C++ code!"
)
painter.setPen(Qt.NoPen)
painter.setBrush(QColor(index, index, index))
scaled_polygon = scalePolygon(self.locations[location],
self.image_size, self.map_size)
painter.drawPolygon(scaled_polygon)
painter.end()
location_image.save(image_file)
stream = open(self.location_file, 'w')
yaml.dump(out_dict, stream)
stream.close()
self.is_modified = False
def deactivateFunction(self):
if self.editing_area:
self.endAreaEdit("Cancel")
elif self.editing_properties:
self.endPropertyEdit()
clearLayoutAndFixHeight(self.subfunction_layout)
self.edit_area_button.clear()
self.image.enableDefaultMouseHooks()
# Just in case we were editing a location, that location was not being drawn.
for location in self.draw_location:
self.draw_location[location] = True
def activateFunction(self):
# Add all the necessary buttons to the subfunction layout.
clearLayoutAndFixHeight(self.subfunction_layout)
for button_text in [
LocationFunction.ADD_LOCATION_AREA,
LocationFunction.EDIT_EXISTING_AREA
]:
button = QPushButton(button_text, self.widget)
button.clicked[bool].connect(
partial(self.startAreaEdit, button_text))
button.setCheckable(True)
self.subfunction_layout.addWidget(button)
self.edit_area_button[button_text] = button
self.edit_area_button[LocationFunction.EDIT_EXISTING_AREA].setEnabled(
False)
self.subfunction_layout.addStretch(1)
# ActivateMouseHooks.
self.image.mousePressEvent = self.mousePressEvent
self.image.mouseMoveEvent = self.mouseMoveEvent
self.image.mouseReleaseEvent = self.mouseReleaseEvent
self.updateOverlay()
def getLocationNameFromPoint(self, point):
for location in self.locations:
if self.locations[location].containsPoint(point, Qt.OddEvenFill):
return location
return None
def startAreaEdit(self, edit_type):
if self.editing_properties:
self.endPropertyEdit()
self.editing_area = True
if edit_type == LocationFunction.ADD_LOCATION_AREA:
self.edit_existing_location = None
# else edit_existing_location was set to the correct location by startPropertyEdit()
# Make sure all active selections have been cleared.
self.clearAreaSelection()
# If we're going to edit an existing area, stop drawing it and copy it to the active selection.
if self.edit_existing_location is not None:
self.draw_location[self.edit_existing_location] = False
self.current_selection = QPolygon(
self.locations[self.edit_existing_location])
self.edit_existing_location = self.edit_existing_location
# Setup the buttons in the configuration toolbar, and disable the original buttons to edit an area.
clearLayoutAndFixHeight(self.configuration_layout)
for button_text in ["Done", "Cancel"]:
button = QPushButton(button_text, self.widget)
button.clicked[bool].connect(partial(self.endAreaEdit,
button_text))
self.configuration_layout.addWidget(button)
self.configuration_layout.addStretch(1)
self.edit_area_button[LocationFunction.ADD_LOCATION_AREA].setEnabled(
False)
self.edit_area_button[LocationFunction.EDIT_EXISTING_AREA].setEnabled(
False)
self.updateOverlay()
def clearAreaSelection(self):
# Make sure all selections are clear.
self.new_selection_start_point = None
self.new_selection_end_point = None
# QPolygons to track current location.
self.current_selection = None
self.new_selection = None
self.subtract_new_selection = None
def endAreaEdit(self, button_text):
edit_properties_location = None
if (button_text
== "Done") and (self.current_selection is not None) and (
not self.current_selection.isEmpty()):
# If the current location being added completely wipes out an old location, make sure you remove it.
for location in self.locations.keys():
if location != self.edit_existing_location:
self.locations[location] = self.locations[
location].subtracted(self.current_selection)
if self.locations[location].isEmpty():
self.removeLocation(location)
if self.edit_existing_location == None:
# We're adding a new location. Generate a new location name and color.
(self.edit_existing_location,
new_location_color) = self.getUniqueNameAndColor()
self.location_colors[
self.edit_existing_location] = new_location_color
self.locations[
self.edit_existing_location] = self.current_selection
self.draw_location[self.edit_existing_location] = True
edit_properties_location = self.edit_existing_location
# Since a location was added or edited, set file modification to true.
self.is_modified = True
else:
# Cancel was pressed, draw the original location if we were editing as before.
if self.edit_existing_location is not None:
self.draw_location[self.edit_existing_location] = True
self.editing_area = False
self.edit_existing_location = None
self.clearAreaSelection()
# Update the entire image overlay.
self.updateOverlay()
self.edit_area_button[LocationFunction.ADD_LOCATION_AREA].setEnabled(
True)
self.edit_area_button[LocationFunction.ADD_LOCATION_AREA].setChecked(
False)
self.edit_area_button[LocationFunction.EDIT_EXISTING_AREA].setChecked(
False)
clearLayoutAndFixHeight(self.configuration_layout)
if edit_properties_location is not None:
self.edit_properties_location = edit_properties_location
self.startPropertyEdit()
def startPropertyEdit(self):
self.editing_properties = True
self.edit_existing_location = self.edit_properties_location
self.edit_area_button[LocationFunction.ADD_LOCATION_AREA].setEnabled(
True)
self.edit_area_button[LocationFunction.EDIT_EXISTING_AREA].setEnabled(
True)
# Construct the configuration layout.
clearLayoutAndFixHeight(self.configuration_layout)
self.update_name_label = QLabel(
"Location (" + self.edit_properties_location + ") New Name: ",
self.widget)
self.configuration_layout.addWidget(self.update_name_label)
self.update_name_textedit = QLineEdit(self.widget)
self.update_name_textedit.setText(self.edit_properties_location)
self.update_name_textedit.textEdited.connect(
self.locationNameTextEdited)
self.configuration_layout.addWidget(self.update_name_textedit)
self.update_name_button = QPushButton("Update location Name",
self.widget)
self.update_name_button.clicked[bool].connect(self.updateLocationName)
self.update_name_button.setEnabled(False)
self.configuration_layout.addWidget(self.update_name_button)
self.remove_location_button = QPushButton("Remove Location",
self.widget)
self.remove_location_button.clicked[bool].connect(
self.removeCurrentLocation)
self.configuration_layout.addWidget(self.remove_location_button)
self.configuration_layout.addStretch(1)
self.updateOverlay()
def endPropertyEdit(self):
self.edit_area_button[LocationFunction.ADD_LOCATION_AREA].setEnabled(
True)
self.edit_area_button[LocationFunction.EDIT_EXISTING_AREA].setEnabled(
False)
clearLayoutAndFixHeight(self.configuration_layout)
self.update_name_label = None
self.update_name_textedit = None
self.update_name_button = None
self.editing_properties = False
self.edit_properties_location = None
self.updateOverlay()
def locationNameTextEdited(self, text):
if str(text) != self.edit_properties_location:
self.update_name_button.setEnabled(True)
else:
self.update_name_button.setEnabled(False)
def updateLocationName(self):
old_loc_name = self.edit_properties_location
new_loc_name = str(self.update_name_textedit.text())
if new_loc_name in self.locations:
# This means that two locations need to be merged
self.locations[new_loc_name] = self.locations[new_loc_name].united(
self.locations.pop(old_loc_name))
else:
# This is a simple rename task.
self.locations[new_loc_name] = self.locations.pop(old_loc_name)
self.location_colors[new_loc_name] = self.location_colors.pop(
old_loc_name)
self.draw_location[new_loc_name] = self.draw_location.pop(
old_loc_name)
# Since a location name was modified, set file modification to true.
self.is_modified = True
# Restart property edit with the updated name.
self.endPropertyEdit()
self.edit_properties_location = new_loc_name
self.startPropertyEdit()
def removeCurrentLocation(self):
old_loc_name = self.edit_properties_location
self.removeLocation(old_loc_name)
self.endPropertyEdit()
self.updateOverlay()
# Since a location was removed, set file modification to true.
self.is_modified = True
def removeLocation(self, loc_name):
if loc_name in self.locations:
self.locations.pop(loc_name)
if loc_name in self.location_colors:
self.location_colors.pop(loc_name)
if loc_name in self.draw_location:
self.draw_location.pop(loc_name)
def isModified(self):
return self.is_modified
def mousePressEvent(self, event):
if self.editing_area:
self.subtract_new_selection = event.button() == Qt.RightButton
self.new_selection_start_point = event.pos()
self.new_selection_end_point = event.pos()
self.new_selection = None
else:
loc = self.getLocationNameFromPoint(event.pos())
if loc is not None:
self.edit_properties_location = loc
self.startPropertyEdit()
else:
self.endPropertyEdit()
def mouseReleaseEvent(self, event):
if self.editing_area:
self.mouseMoveEvent(event)
if self.new_selection is not None:
if self.current_selection is None and self.subtract_new_selection == False:
self.current_selection = self.new_selection
if self.subtract_new_selection:
self.current_selection = self.current_selection.subtracted(
self.new_selection)
else:
self.current_selection = self.current_selection.united(
self.new_selection)
self.new_selection = None
self.subtract_new_selection = None
def mouseMoveEvent(self, event):
if self.editing_area:
# First make sure we update the region corresponding to the old mark.
old_overlay_update_rect = self.get_rectangular_polygon(
self.new_selection_start_point, self.new_selection_end_point)
# Draw new mark, taking some care to reduce the size of the polygon's bottom right corner by (1,1).
self.new_selection_end_point = event.pos()
self.new_selection = self.get_rectangular_polygon(
self.new_selection_start_point, self.new_selection_end_point)
self.new_selection = self.new_selection.boundingRect()
self.new_selection.setHeight(self.new_selection.height() - 1)
self.new_selection.setWidth(self.new_selection.width() - 1)
self.new_selection = QPolygon(self.new_selection, True)
# Next determine the region that needs to be update because of the new mark.
new_overlay_update_rect = self.get_rectangular_polygon(
self.new_selection_start_point, self.new_selection_end_point)
overlay_update_region = (old_overlay_update_rect +
new_overlay_update_rect).boundingRect()
self.updateOverlay(overlay_update_region)
def updateOverlay(self, rect=None):
# Redraw the overlay image from scratch using the location image and current location.
self.image.overlay_image.fill(Qt.transparent)
painter = QPainter(self.image.overlay_image)
painter.setBackgroundMode(Qt.TransparentMode)
painter.setCompositionMode(QPainter.CompositionMode_Source)
for location in self.locations:
if self.draw_location[location]:
color = self.location_colors[location]
if self.edit_properties_location == location and self.editing_properties:
color = self.edit_area_selection_color
lineColor = QColor(color)
lineColor.setAlpha(255)
brushColor = QColor(color)
brushColor.setAlpha(128)
painter.setPen(lineColor)
painter.setBrush(brushColor)
painter.drawPolygon(self.locations[location])
if (self.current_selection is not None) or (self.new_selection
is not None):
lineColor = QColor(self.edit_area_selection_color)
lineColor.setAlpha(255)
brushColor = QColor(self.edit_area_selection_color)
brushColor.setAlpha(128)
painter.setPen(lineColor)
painter.setBrush(brushColor)
if self.new_selection is not None:
# Create a temporary polygon as the new selection is being drawn.
if self.current_selection is not None:
current_selection = QPolygon(self.current_selection)
if self.subtract_new_selection:
current_selection = current_selection.subtracted(
self.new_selection)
else:
current_selection = current_selection.united(
self.new_selection)
painter.drawPolygon(current_selection)
elif self.subtract_new_selection == False:
painter.drawPolygon(self.new_selection)
else:
painter.drawPolygon(self.current_selection)
painter.end()
if rect is None:
self.image.update()
else:
self.image.update(rect)
def getUniqueNameAndColor(self):
"""
Use golden ratio to generate unique colors.
http://martin.ankerl.com/2009/12/09/how-to-create-random-colors-programmatically/
"""
name = "new_loc" + str(self.unique_loc_counter)
h = int(359 * (self.unique_loc_counter * 0.618033988749895))
h = h % 359
self.unique_loc_counter += 1
return name, QColor.fromHsv(h, 255, 255)
def get_rectangular_polygon(self, pt1, pt2):
return QPolygon(
[pt1, QPoint(pt1.x(), pt2.y()), pt2,
QPoint(pt2.x(), pt1.y())])
class StepInterfaceWidget(QObject):
command_buttons = []
start_feet = Feet()
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 shutdown_plugin(self):
print "Shutting down ..."
print "Done!"
def add_command_button(self, parent, text, command):
button = QPushButton(text)
self.command_mapper.setMapping(button, command)
button.clicked.connect(self.command_mapper.map)
parent.addWidget(button)
self.command_buttons.append(button)
return button
def set_start_feet_callback(self, feet):
self.start_feet = feet
# message publisher
def _publish_step_plan_request(self, walk_command):
params = PatternParameters()
params.steps = self.step_number.value()
params.mode = walk_command
params.close_step = self.close_step_checkbox.isChecked()
params.extra_seperation = self.extra_seperation_checkbox.isChecked()
params.use_terrain_model = self.use_terrain_model_checkbox.isChecked()
params.override = self.override_checkbox.isChecked() and not self.use_terrain_model_checkbox.isChecked()
params.roll = math.radians(self.roll.value())
params.pitch = math.radians(self.pitch.value())
params.dz = self.dz.value()
params.step_distance_forward = self.step_distance.value()
params.step_distance_sideward = self.side_step.value()
params.turn_angle = math.radians(self.step_rotation.value())
request = StepPlanRequest()
request.header = std_msgs.msg.Header()
request.header.stamp = rospy.Time.now()
request.header.frame_id = self.frame_id_line_edit.text()
request.start = self.start_feet
request.start_step_index = self.start_step_index.value()
if (self.start_foot_selection_combo_box.currentText() == "AUTO"):
request.start_foot_selection = StepPlanRequest.AUTO
elif (self.start_foot_selection_combo_box.currentText() == "LEFT"):
request.start_foot_selection = StepPlanRequest.LEFT
elif (self.start_foot_selection_combo_box.currentText() == "RIGHT"):
request.start_foot_selection = StepPlanRequest.RIGHT
else:
self.logger.log_error("Unknown start foot selection mode ('" + self.start_foot_selection_combo_box.currentText() + "')!")
return;
if (walk_command == PatternParameters.FORWARD):
params.mode = PatternParameters.SAMPLING
elif (walk_command == PatternParameters.BACKWARD):
params.mode = PatternParameters.SAMPLING
params.step_distance_forward *= -1;
params.step_distance_sideward *= -1;
params.turn_angle *= -1;
request.pattern_parameters = params
request.planning_mode = StepPlanRequest.PLANNING_MODE_PATTERN
request.parameter_set_name.data = self.parameter_set_widget.current_parameter_set_name()
print "Send request = ", request
# send request
if (self.step_plan_request_client.wait_for_server(rospy.Duration(0.5))):
self.logger.log_info("Sending footstep plan request...")
goal = StepPlanRequestGoal()
goal.plan_request = request;
self.step_plan_request_client.send_goal(goal)
if (self.step_plan_request_client.wait_for_result(rospy.Duration(5.0))):
self.logger.log_info("Received footstep plan!")
self.logger.log(self.step_plan_request_client.get_result().status)
self.step_plan_pub.publish(self.step_plan_request_client.get_result().step_plan)
else:
self.logger.log_error("Didn't received any results. Check communcation!")
else:
self.logger.log_error("Can't connect to footstep planner action server!")
def commands_set_enabled(self, enable):
for button in self.command_buttons:
button.setEnabled(enable)
@Slot()
def param_cleared(self):
self.commands_set_enabled(False)
@Slot(str)
def param_selected(self, name):
self.commands_set_enabled(True)
@Slot(int)
def use_terrain_model_changed(self, state):
enable_override = True
if state == Qt.Checked:
enable_override = False
self.roll.setEnabled(enable_override)
self.pitch.setEnabled(enable_override)
self.override_checkbox.setEnabled(enable_override)
self.dz.setEnabled(enable_override)
class RoomDialogPlugin(Plugin):
def __init__(self, context):
super(RoomDialogPlugin, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('RoomDialogPlugin')
font_size = rospy.get_param("~font_size", 30)
# Create QWidget
self._widget = QWidget()
self._widget.setFont(QFont("Times", font_size, QFont.Bold))
self._layout = QVBoxLayout(self._widget)
self._text_browser = QTextBrowser(self._widget)
self._layout.addWidget(self._text_browser)
self._button_layout = QGridLayout()
self._layout.addLayout(self._button_layout)
# rospy.loginfo("Hello world")
# Add combobox
self._cb_layout = QHBoxLayout()
self._cb = QComboBox()
self._layout.addLayout(self._cb_layout)
#layout = QVBoxLayout(self._widget)
#layout.addWidget(self._button)
self._widget.setObjectName('RoomDialogPluginUI')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
context.add_widget(self._widget)
# Setup service provider
self.service = rospy.Service('room_dialog', RoomDialog,
self.service_callback)
self.response_ready = False
self.response = None
self.buttons = []
self.text_label = None
self.text_input = None
# Add combo options
self.connect(self._widget, SIGNAL("update"), self.update)
self.connect(self._widget, SIGNAL("timeout"), self.timeout)
def shutdown_plugin(self):
self.service.shutdown()
def service_callback(self, req):
self.response_ready = False
self.request = req
self._widget.emit(SIGNAL("update"))
# Start timer against wall clock here instead of the ros clock.
start_time = time.time()
while not self.response_ready:
if self.request != req:
# The request got preempted by a new request.
return RoomDialogResponse(RoomDialogRequest.PREEMPTED, "")
if req.timeout != RoomDialogRequest.NO_TIMEOUT:
current_time = time.time()
if current_time - start_time > req.timeout:
self._widget.emit(SIGNAL("timeout"))
return RoomDialogResponse(RoomDialogRequest.TIMED_OUT, "")
time.sleep(0.2)
return self.response
def update(self):
self.clean()
req = self.request
self._text_browser.setText(req.message)
if req.type == RoomDialogRequest.DISPLAY:
# All done, nothing more too see here.
self.response = RoomDialogResponse(RoomDialogRequest.NO_RESPONSE,
"")
self.response_ready = True
elif req.type == RoomDialogRequest.CHOICE_QUESTION:
for index, options in enumerate(req.options):
button = QPushButton(options, self._widget)
button.clicked.connect(partial(self.handle_button, index))
row = index / 3
col = index % 3
self._button_layout.addWidget(button, row, col)
self.buttons.append(button)
elif req.type == RoomDialogRequest.TEXT_QUESTION:
self.text_label = QLabel("Enter here: ", self._widget)
self._button_layout.addWidget(self.text_label, 0, 0)
self.text_input = QLineEdit(self._widget)
self.text_input.editingFinished.connect(self.handle_text)
self._button_layout.addWidget(self.text_input, 0, 1)
# add handling of combobox
elif req.type == RoomDialogRequest.COMBOBOX_QUESTION:
# self.clean()
rospy.loginfo("Combobox selected")
#self._cb.duplicatesEnabled = False
if self._cb.count() == 0:
for index, options in enumerate(req.options):
self._cb.addItem(options)
rospy.loginfo(options)
#self.buttons.append(options)
# NOTE COULD INTRODUCE BUG
self._cb.currentIndexChanged.connect(self.handle_cb)
self._cb_layout.addWidget(self._cb)
def timeout(self):
self._text_browser.setText("Oh no! The request timed out.")
self.clean()
def clean(self):
while self._button_layout.count():
item = self._button_layout.takeAt(0)
item.widget().deleteLater()
# while self._cb_layout.count():
# item = self._cb_layout.takeAt(0)
# item.widget().deleteLater()
self.buttons = []
self.text_input = None
self.text_label = None
def handle_button(self, index):
self.response = RoomDialogResponse(index, "")
self.clean()
self.response_ready = True
def handle_text(self):
self.response = RoomDialogResponse(RoomDialogRequest.TEXT_RESPONSE,
self.text_input.text())
self.clean()
self.response_ready = True
def handle_cb(self, index):
# This will be the sign format seen around building ex: 3.404
rospy.loginfo("handling cb")
roomHuman = self._cb.currentText()
# modify string into robot format ex: d3_404
splitHuman = roomHuman.split('.', 1)
roomRobot = 'd' + splitHuman[0] + '_' + splitHuman[1]
roomRobot = str(roomRobot)
self.response = RoomDialogResponse(RoomDialogRequest.CB_RESPONSE,
roomRobot)
self.clean()
self.response_ready = True
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
class Top(Plugin):
NODE_FIELDS = [
'pid', 'get_cpu_percent', 'get_memory_percent', 'get_num_threads'
]
OUT_FIELDS = [
'node_name', 'pid', 'cpu_percent', 'memory_percent', 'num_threads'
]
FORMAT_STRS = ['%s', '%s', '%0.2f', '%0.2f', '%s']
NODE_LABELS = ['Node', 'PID', 'CPU %', 'Mem %', 'Num Threads']
SORT_TYPE = [str, str, float, float, float]
TOOLTIPS = {
0: ('cmdline', lambda x: '\n'.join(textwrap.wrap(' '.join(x)))),
3: ('memory_info', lambda x:
('Resident: %0.2f MiB, Virtual: %0.2f MiB' %
(x[0] / 2**20, x[1] / 2**20)))
}
_node_info = NodeInfo()
name_filter = re.compile('')
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())
# if not args.quiet:
# print 'arguments: ', args
# print 'unknowns: ', unknowns
self._selected_node = ''
self._selected_node_lock = RLock()
# Setup the toolbar
self._toolbar = QToolBar()
self._filter_box = QLineEdit()
self._regex_box = QCheckBox()
self._regex_box.setText('regex')
self._toolbar.addWidget(QLabel('Filter'))
self._toolbar.addWidget(self._filter_box)
self._toolbar.addWidget(self._regex_box)
self._filter_box.returnPressed.connect(self.update_filter)
self._regex_box.stateChanged.connect(self.update_filter)
# Create a container widget and give it a layout
self._container = QWidget()
self._container.setWindowTitle('Process Monitor')
self._layout = QVBoxLayout()
self._container.setLayout(self._layout)
self._layout.addWidget(self._toolbar)
# Create the table widget
self._table_widget = QTreeWidget()
self._table_widget.setObjectName('TopTable')
self._table_widget.setColumnCount(len(self.NODE_LABELS))
self._table_widget.setHeaderLabels(self.NODE_LABELS)
self._table_widget.itemClicked.connect(self._tableItemClicked)
self._table_widget.setSortingEnabled(True)
self._table_widget.setAlternatingRowColors(True)
self._layout.addWidget(self._table_widget)
context.add_widget(self._container)
# Add a button for killing nodes
self._kill_button = QPushButton('Kill Node')
self._layout.addWidget(self._kill_button)
self._kill_button.clicked.connect(self._kill_node)
# Update twice since the first cpu% lookup will always return 0
self.update_table()
self.update_table()
self._table_widget.resizeColumnToContents(0)
# Start a timer to trigger updates
self._update_timer = QTimer()
self._update_timer.setInterval(1000)
self._update_timer.timeout.connect(self.update_table)
self._update_timer.start()
def _tableItemClicked(self, item, column):
with self._selected_node_lock:
self._selected_node = item.text(0)
def update_filter(self, *args):
if self._regex_box.isChecked():
expr = self._filter_box.text()
else:
expr = re.escape(self._filter_box.text())
self.name_filter = re.compile(expr)
self.update_table()
def _kill_node(self):
self._node_info.kill_node(self._selected_node)
def update_one_item(self, row, info):
twi = TopWidgetItem()
for col, field in enumerate(self.OUT_FIELDS):
val = info[field]
twi.setText(col, self.FORMAT_STRS[col] % val)
self._table_widget.insertTopLevelItem(row, twi)
for col, (key, func) in self.TOOLTIPS.iteritems():
twi.setToolTip(col, func(info[key]))
with self._selected_node_lock:
if twi.text(0) == self._selected_node:
twi.setSelected(True)
self._table_widget.setItemHidden(
twi,
len(self.name_filter.findall(info['node_name'])) == 0)
def update_table(self):
self._table_widget.clear()
infos = self._node_info.get_all_node_fields(self.NODE_FIELDS)
for nx, info in enumerate(infos):
self.update_one_item(nx, info)
def shutdown_plugin(self):
self._update_timer.stop()
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('filter_text', self._filter_box.text())
instance_settings.set_value('is_regex',
int(self._regex_box.checkState()))
def restore_settings(self, plugin_settings, instance_settings):
self._filter_box.setText(instance_settings.value('filter_text'))
is_regex_int = instance_settings.value('is_regex')
if is_regex_int:
self._regex_box.setCheckState(Qt.CheckState(is_regex_int))
else:
self._regex_box.setCheckState(Qt.CheckState(0))
self.update_filter()
class RunDialog(PackageDialog):
'''
A dialog to run a ROS node without configuration
'''
def __init__(self, host, masteruri=None, parent=None):
PackageDialog.__init__(self, masteruri, parent)
self.host = host
self.setWindowTitle('Run')
ns_name_label = QLabel("NS/Name:", self.content)
self.ns_field = QComboBox(self.content)
self.ns_field.setSizePolicy(
QSizePolicy(QSizePolicy.Expanding, QSizePolicy.Fixed))
self.ns_field.setEditable(True)
ns_history = nm.history().cachedParamValues('run_dialog/NS')
ns_history.insert(0, '/')
self.ns_field.addItems(ns_history)
self.name_field = QLineEdit(self.content)
self.name_field.setEnabled(False)
horizontalLayout = QHBoxLayout()
horizontalLayout.addWidget(self.ns_field)
horizontalLayout.addWidget(self.name_field)
self.contentLayout.addRow(ns_name_label, horizontalLayout)
args_label = QLabel("Args:", self.content)
self.args_field = QComboBox(self.content)
self.args_field.setSizeAdjustPolicy(
QComboBox.AdjustToMinimumContentsLength)
self.args_field.setSizePolicy(
QSizePolicy(QSizePolicy.Expanding, QSizePolicy.Fixed))
self.args_field.setEditable(True)
self.contentLayout.addRow(args_label, self.args_field)
args_history = nm.history().cachedParamValues('run_dialog/Args')
args_history.insert(0, '')
self.args_field.addItems(args_history)
host_label = QLabel("Host:", self.content)
self.host_field = QComboBox(self.content)
self.host_field.setSizePolicy(
QSizePolicy(QSizePolicy.Expanding, QSizePolicy.Fixed))
self.host_field.setEditable(True)
host_label.setBuddy(self.host_field)
self.contentLayout.addRow(host_label, self.host_field)
self.host_history = host_history = nm.history().cachedParamValues(
'/Host')
if self.host in host_history:
host_history.remove(self.host)
host_history.insert(0, self.host)
self.host_field.addItems(host_history)
master_label = QLabel("ROS Master URI:", self.content)
self.master_field = QComboBox(self.content)
self.master_field.setSizePolicy(
QSizePolicy(QSizePolicy.Expanding, QSizePolicy.Fixed))
self.master_field.setEditable(True)
master_label.setBuddy(self.host_field)
self.contentLayout.addRow(master_label, self.master_field)
self.master_history = master_history = nm.history().cachedParamValues(
'/Optional Parameter/ROS Master URI')
if self.masteruri in master_history:
master_history.remove(self.masteruri)
master_history.insert(0, self.masteruri)
self.master_field.addItems(master_history)
self.binary_field.activated[str].connect(self.on_binary_selected)
def run_params(self):
'''
Runs the selected node, or do nothing.
:return: a tuple with host, package, binary, name, args, maseruri or empty tuple on errors
'''
self.binary = self.binary_field.currentText()
self.host = self.host_field.currentText(
) if self.host_field.currentText() else self.host
self.masteruri = self.master_field.currentText(
) if self.master_field.currentText() else self.masteruri
if self.host not in self.host_history and self.host != 'localhost' and self.host != '127.0.0.1':
nm.history().add2HostHistory(self.host)
ns = self.ns_field.currentText()
if ns and ns != '/':
nm.history().addParamCache('run_dialog/NS', ns)
args = self.args_field.currentText()
if args:
nm.history().addParamCache('run_dialog/Args', args)
if self.package and self.binary:
nm.history().addParamCache('/Host', self.host)
return (self.host, self.package, self.binary,
self.name_field.text(),
('__ns:=%s %s' % (ns, args)).split(' '), None
if self.masteruri == 'ROS_MASTER_URI' else self.masteruri)
return ()
def on_package_selected(self, package):
PackageDialog.on_package_selected(self, package)
if self.packages and package in self.packages:
self.args_field.setEnabled(True)
self.ns_field.setEnabled(True)
self.name_field.setEnabled(True)
root, _ext = os.path.splitext(
os.path.basename(self.binary_field.currentText()))
self.name_field.setText(root)
def on_binary_selected(self, binary):
root, _ext = os.path.splitext(os.path.basename(binary))
self.name_field.setText(root)
class RunDialog(PackageDialog):
'''
A dialog to run a ROS node without configuration
'''
def __init__(self, host, masteruri=None, parent=None):
PackageDialog.__init__(self, parent)
self.host = host
self.setWindowTitle('Run')
ns_name_label = QLabel("NS/Name:", self.content)
self.ns_field = QComboBox(self.content)
self.ns_field.setSizePolicy(QSizePolicy(QSizePolicy.Expanding, QSizePolicy.Fixed))
self.ns_field.setEditable(True)
ns_history = nm.history().cachedParamValues('run_dialog/NS')
ns_history.insert(0, '/')
self.ns_field.addItems(ns_history)
self.name_field = QLineEdit(self.content)
self.name_field.setEnabled(False)
horizontalLayout = QHBoxLayout()
horizontalLayout.addWidget(self.ns_field)
horizontalLayout.addWidget(self.name_field)
self.contentLayout.addRow(ns_name_label, horizontalLayout)
args_label = QLabel("Args:", self.content)
self.args_field = QComboBox(self.content)
self.args_field.setSizeAdjustPolicy(QComboBox.AdjustToMinimumContentsLength)
self.args_field.setSizePolicy(QSizePolicy(QSizePolicy.Expanding, QSizePolicy.Fixed))
self.args_field.setEditable(True)
self.contentLayout.addRow(args_label, self.args_field)
args_history = nm.history().cachedParamValues('run_dialog/Args')
args_history.insert(0, '')
self.args_field.addItems(args_history)
host_label = QLabel("Host:", self.content)
self.host_field = QComboBox(self.content)
# self.host_field.setSizeAdjustPolicy(QComboBox.AdjustToContents)
self.host_field.setSizePolicy(QSizePolicy(QSizePolicy.Expanding, QSizePolicy.Fixed))
self.host_field.setEditable(True)
host_label.setBuddy(self.host_field)
self.contentLayout.addRow(host_label, self.host_field)
self.host_history = host_history = nm.history().cachedParamValues('/Host')
if self.host in host_history:
host_history.remove(self.host)
host_history.insert(0, self.host)
self.host_field.addItems(host_history)
master_label = QLabel("ROS Master URI:", self.content)
self.master_field = QComboBox(self.content)
self.master_field.setSizePolicy(QSizePolicy(QSizePolicy.Expanding, QSizePolicy.Fixed))
self.master_field.setEditable(True)
master_label.setBuddy(self.host_field)
self.contentLayout.addRow(master_label, self.master_field)
self.master_history = master_history = nm.history().cachedParamValues('/Optional Parameter/ROS Master URI')
self.masteruri = "ROS_MASTER_URI" if masteruri is None else masteruri
if self.masteruri in master_history:
master_history.remove(self.masteruri)
master_history.insert(0, self.masteruri)
self.master_field.addItems(master_history)
# self.package_field.setFocus(QtCore.Qt.TabFocusReason)
if hasattr(self.package_field, "textChanged"): # qt compatibility
self.package_field.textChanged.connect(self.on_package_selected)
else:
self.package_field.editTextChanged.connect(self.on_package_selected)
self.binary_field.activated[str].connect(self.on_binary_selected)
def run_params(self):
'''
Runs the selected node, or do nothing.
:return: a tuple with host, package, binary, name, args, maseruri or empty tuple on errors
'''
self.binary = self.binary_field.currentText()
self.host = self.host_field.currentText() if self.host_field.currentText() else self.host
self.masteruri = self.master_field.currentText() if self.master_field.currentText() else self.masteruri
if self.host not in self.host_history and self.host != 'localhost' and self.host != '127.0.0.1':
nm.history().add2HostHistory(self.host)
ns = self.ns_field.currentText()
if ns and ns != '/':
nm.history().addParamCache('run_dialog/NS', ns)
args = self.args_field.currentText()
if args:
nm.history().addParamCache('run_dialog/Args', args)
if self.package and self.binary:
nm.history().addParamCache('/Host', self.host)
return (self.host, self.package, self.binary, self.name_field.text(), ('__ns:=%s %s' % (ns, args)).split(' '), None if self.masteruri == 'ROS_MASTER_URI' else self.masteruri)
return ()
def on_package_selected(self, package):
PackageDialog.on_package_selected(self, package)
if self.packages and package in self.packages:
self.args_field.setEnabled(True)
self.ns_field.setEnabled(True)
self.name_field.setEnabled(True)
root, _ext = os.path.splitext(os.path.basename(self.binary_field.currentText()))
self.name_field.setText(root)
def on_binary_selected(self, binary):
root, _ext = os.path.splitext(os.path.basename(binary))
self.name_field.setText(root)
class SelectDialog(QDialog):
'''
This dialog creates an input mask for a string list and return selected entries.
'''
def __init__(self,
items=list(),
buttons=QDialogButtonBox.Cancel | QDialogButtonBox.Ok,
exclusive=False,
preselect_all=False,
title='',
description='',
icon='',
parent=None,
select_if_single=True,
checkitem1='',
checkitem2=''):
'''
Creates an input dialog.
@param items: a list with strings
@type items: C{list()}
'''
QDialog.__init__(self, parent=parent)
self.setObjectName(' - '.join(['SelectDialog', str(items)]))
self.verticalLayout = QVBoxLayout(self)
self.verticalLayout.setObjectName("verticalLayout")
self.verticalLayout.setContentsMargins(1, 1, 1, 1)
# add filter row
self.filter_frame = QFrame(self)
filterLayout = QHBoxLayout(self.filter_frame)
filterLayout.setContentsMargins(1, 1, 1, 1)
label = QLabel("Filter:", self.filter_frame)
self.filter_field = QLineEdit(self.filter_frame)
filterLayout.addWidget(label)
filterLayout.addWidget(self.filter_field)
self.filter_field.textChanged.connect(self._on_filter_changed)
self.verticalLayout.addWidget(self.filter_frame)
if description:
self.description_frame = QFrame(self)
descriptionLayout = QHBoxLayout(self.description_frame)
# descriptionLayout.setContentsMargins(1, 1, 1, 1)
if icon:
self.icon_label = QLabel(self.description_frame)
self.icon_label.setSizePolicy(QSizePolicy.Fixed,
QSizePolicy.Fixed)
self.icon_label.setPixmap(
QPixmap(icon).scaled(30, 30, Qt.KeepAspectRatio))
descriptionLayout.addWidget(self.icon_label)
self.description_label = QLabel(self.description_frame)
self.description_label.setWordWrap(True)
self.description_label.setText(description)
descriptionLayout.addWidget(self.description_label)
self.verticalLayout.addWidget(self.description_frame)
# create area for the parameter
self.content = MainBox(self)
if items:
self.scroll_area = QScrollArea(self)
self.scroll_area.setFocusPolicy(Qt.NoFocus)
self.scroll_area.setObjectName("scroll_area")
self.scroll_area.setWidgetResizable(True)
self.scroll_area.setWidget(self.content)
self.verticalLayout.addWidget(self.scroll_area)
self.checkitem1 = checkitem1
self.checkitem1_result = False
self.checkitem2 = checkitem2
self.checkitem2_result = False
# add select all option
if not exclusive and items:
self._ignore_next_toggle = False
self.select_all_checkbox = QCheckBox('all entries')
self.select_all_checkbox.setTristate(True)
self.select_all_checkbox.stateChanged.connect(
self._on_select_all_checkbox_stateChanged)
self.verticalLayout.addWidget(self.select_all_checkbox)
self.content.toggled.connect(self._on_main_toggle)
if self.checkitem1:
self.checkitem1_checkbox = QCheckBox(self.checkitem1)
self.checkitem1_checkbox.stateChanged.connect(
self._on_select_checkitem1_checkbox_stateChanged)
self.verticalLayout.addWidget(self.checkitem1_checkbox)
if self.checkitem2:
self.checkitem2_checkbox = QCheckBox(self.checkitem2)
self.checkitem2_checkbox.stateChanged.connect(
self._on_select_checkitem2_checkbox_stateChanged)
self.verticalLayout.addWidget(self.checkitem2_checkbox)
if not items:
spacerItem = QSpacerItem(1, 1, QSizePolicy.Expanding,
QSizePolicy.Expanding)
self.verticalLayout.addItem(spacerItem)
# create buttons
self.buttonBox = QDialogButtonBox(self)
self.buttonBox.setObjectName("buttonBox")
self.buttonBox.setOrientation(Qt.Horizontal)
self.buttonBox.setStandardButtons(buttons)
self.buttonBox.accepted.connect(self.accept)
self.buttonBox.rejected.connect(self.reject)
self.verticalLayout.addWidget(self.buttonBox)
# set the input fields
if items:
self.content.createFieldsFromValues(items, exclusive)
if (select_if_single and len(items) == 1) or preselect_all:
self.select_all_checkbox.setCheckState(Qt.Checked)
if not items or len(items) < 7:
self.filter_frame.setVisible(False)
# print '=============== create', self.objectName()
#
# def __del__(self):
# print "************ destroy", self.objectName()
def _on_main_toggle(self, state):
self._ignore_next_toggle = state != self.select_all_checkbox.checkState(
)
self.select_all_checkbox.setCheckState(state)
def _on_select_all_checkbox_stateChanged(self, state):
if not self._ignore_next_toggle:
self.content.setState(state)
self._ignore_next_toggle = False
def _on_select_checkitem1_checkbox_stateChanged(self, state):
if state == Qt.Checked:
self.checkitem1_result = True
elif state == Qt.Unchecked:
self.checkitem1_result = False
def _on_select_checkitem2_checkbox_stateChanged(self, state):
if state == Qt.Checked:
self.checkitem2_result = True
elif state == Qt.Unchecked:
self.checkitem2_result = False
def _on_filter_changed(self):
self.content.filter(self.filter_field.text())
def getSelected(self):
return self.content.getSelected()
@staticmethod
def getValue(title,
description='',
items=list(),
exclusive=False,
preselect_all=False,
icon='',
parent=None,
select_if_single=True,
checkitem1='',
checkitem2=''):
selectDia = SelectDialog(items,
exclusive=exclusive,
preselect_all=preselect_all,
description=description,
icon=icon,
parent=parent,
select_if_single=select_if_single,
checkitem1=checkitem1,
checkitem2=checkitem2)
selectDia.setWindowTitle(title)
selectDia.resize(480, 256)
if selectDia.exec_():
if selectDia.checkitem2:
return selectDia.getSelected(
), True, selectDia.checkitem1_result, selectDia.checkitem2_result
if selectDia.checkitem1:
return selectDia.getSelected(
), True, selectDia.checkitem1_result
return selectDia.getSelected(), True
if selectDia.checkitem2:
return list(), False, False, False
if selectDia.checkitem1:
return list(), False, False
return list(), False
# %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
# %%%%%%%%%%%%%%%%%% close handling %%%%%%%%%%%%%%%%%%%%%
# %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
def accept(self):
self.setResult(QDialog.Accepted)
self.hide()
def reject(self):
self.setResult(QDialog.Rejected)
self.hide()
def hideEvent(self, event):
self.close()
def closeEvent(self, event):
'''
Test the open files for changes and save this if needed.
'''
self.setAttribute(Qt.WA_DeleteOnClose, True)
QDialog.closeEvent(self, event)
class LocationFunction(object):
EDIT_LOCATION_PROPERITIES = 'Edit Location Properties'
ADD_LOCATION_AREA = 'Add Location'
EDIT_EXISTING_AREA = 'Edit Location'
def __init__(self,
location_file,
map,
widget,
subfunction_layout,
configuration_layout,
image):
self.edit_area_button = None
self.edit_area_selection_color = Qt.black
# Dictionary of polygons
self.locations = {}
# Dictionary that maps location names to their colors
self.location_colors = {}
self.draw_location = {}
self.unique_loc_counter = 1
self.editing_area = False
self.edit_existing_location = None
self.editing_properties = False
self.edit_properties_location = None
# Use this to initialize variables.
self.clearAreaSelection()
self.is_modified = False
self.widget = widget
self.subfunction_layout = subfunction_layout
self.image = image
self.image_size = image.overlay_image.size()
self.configuration_layout = configuration_layout
self.location_file = location_file
self.map_size = QSize(map.map.info.width, map.map.info.height)
self.readLocationsFromFile()
self.edit_area_button = {}
def readLocationsFromFile(self):
if os.path.isfile(self.location_file):
stream = open(self.location_file, 'r')
try:
contents = yaml.load(stream)
if "polygons" not in contents or "locations" not in contents:
rospy.logerr("YAML file found at " + self.location_file + ", but does not seem to have been written by this tool. I'm starting locations from scratch.")
else:
location_keys = contents["locations"]
location_polygons = contents["polygons"]
for index, location in enumerate(location_keys):
self.locations[location] = QPolygon()
self.locations[location].setPoints(location_polygons[index])
self.locations[location] = scalePolygon(self.locations[location],
self.map_size,
self.image_size)
(_,self.location_colors[location]) = self.getUniqueNameAndColor()
self.draw_location[location] = True
except yaml.YAMLError:
rospy.logerr("File found at " + self.location_file + ", but cannot be parsed by YAML parser. I'm starting locations from scratch.")
stream.close()
else:
rospy.logwarn("Location file not found at " + self.location_file + ". I'm starting locations from scratch and will attempt to write to this location before exiting.")
def saveConfiguration(self):
self.writeLocationsToFile()
def writeLocationsToFile(self):
out_dict = {}
out_dict["locations"] = self.locations.keys()
out_dict["polygons"] = []
for index, location in enumerate(self.locations):
out_dict["polygons"].append([])
for i in range(self.locations[location].size()):
pt = self.locations[location].point(i)
scaled_pt = scalePoint(pt, self.image_size, self.map_size)
out_dict["polygons"][index].append(scaled_pt.x())
out_dict["polygons"][index].append(scaled_pt.y())
data_directory = os.path.dirname(os.path.realpath(self.location_file))
image_file = getLocationsImageFileLocationFromDataDirectory(data_directory)
# Create an image with the location data, so that C++ programs don't need to rely on determining regions using polygons.
out_dict["data"] = 'locations.pgm'
location_image = QImage(self.map_size, QImage.Format_RGB32)
location_image.fill(Qt.white)
painter = QPainter(location_image)
for index, location in enumerate(self.locations):
if index > 254:
rospy.logerr("You have more than 254 locations, which is unsupported by the bwi_planning_common C++ code!")
painter.setPen(Qt.NoPen)
painter.setBrush(QColor(index, index, index))
scaled_polygon = scalePolygon(self.locations[location], self.image_size, self.map_size)
painter.drawPolygon(scaled_polygon)
painter.end()
location_image.save(image_file)
stream = open(self.location_file, 'w')
yaml.dump(out_dict, stream)
stream.close()
self.is_modified = False
def deactivateFunction(self):
if self.editing_area:
self.endAreaEdit("Cancel")
elif self.editing_properties:
self.endPropertyEdit()
clearLayoutAndFixHeight(self.subfunction_layout)
self.edit_area_button.clear()
self.image.enableDefaultMouseHooks()
# Just in case we were editing a location, that location was not being drawn.
for location in self.draw_location:
self.draw_location[location] = True
def activateFunction(self):
# Add all the necessary buttons to the subfunction layout.
clearLayoutAndFixHeight(self.subfunction_layout)
for button_text in [LocationFunction.ADD_LOCATION_AREA,
LocationFunction.EDIT_EXISTING_AREA]:
button = QPushButton(button_text, self.widget)
button.clicked[bool].connect(partial(self.startAreaEdit, button_text))
button.setCheckable(True)
self.subfunction_layout.addWidget(button)
self.edit_area_button[button_text] = button
self.edit_area_button[LocationFunction.EDIT_EXISTING_AREA].setEnabled(False)
self.subfunction_layout.addStretch(1)
# ActivateMouseHooks.
self.image.mousePressEvent = self.mousePressEvent
self.image.mouseMoveEvent = self.mouseMoveEvent
self.image.mouseReleaseEvent = self.mouseReleaseEvent
self.updateOverlay()
def getLocationNameFromPoint(self, point):
for location in self.locations:
if self.locations[location].containsPoint(point, Qt.OddEvenFill):
return location
return None
def startAreaEdit(self, edit_type):
if self.editing_properties:
self.endPropertyEdit()
self.editing_area = True
if edit_type == LocationFunction.ADD_LOCATION_AREA:
self.edit_existing_location = None
# else edit_existing_location was set to the correct location by startPropertyEdit()
# Make sure all active selections have been cleared.
self.clearAreaSelection()
# If we're going to edit an existing area, stop drawing it and copy it to the active selection.
if self.edit_existing_location is not None:
self.draw_location[self.edit_existing_location] = False
self.current_selection = QPolygon(self.locations[self.edit_existing_location])
self.edit_existing_location = self.edit_existing_location
# Setup the buttons in the configuration toolbar, and disable the original buttons to edit an area.
clearLayoutAndFixHeight(self.configuration_layout)
for button_text in ["Done", "Cancel"]:
button = QPushButton(button_text, self.widget)
button.clicked[bool].connect(partial(self.endAreaEdit, button_text))
self.configuration_layout.addWidget(button)
self.configuration_layout.addStretch(1)
self.edit_area_button[LocationFunction.ADD_LOCATION_AREA].setEnabled(False)
self.edit_area_button[LocationFunction.EDIT_EXISTING_AREA].setEnabled(False)
self.updateOverlay()
def clearAreaSelection(self):
# Make sure all selections are clear.
self.new_selection_start_point = None
self.new_selection_end_point = None
# QPolygons to track current location.
self.current_selection = None
self.new_selection = None
self.subtract_new_selection = None
def endAreaEdit(self, button_text):
edit_properties_location = None
if (button_text == "Done") and (self.current_selection is not None) and (not self.current_selection.isEmpty()):
# If the current location being added completely wipes out an old location, make sure you remove it.
for location in self.locations.keys():
if location != self.edit_existing_location:
self.locations[location] = self.locations[location].subtracted(self.current_selection)
if self.locations[location].isEmpty():
self.removeLocation(location)
if self.edit_existing_location == None:
# We're adding a new location. Generate a new location name and color.
(self.edit_existing_location, new_location_color) = self.getUniqueNameAndColor()
self.location_colors[self.edit_existing_location] = new_location_color
self.locations[self.edit_existing_location] = self.current_selection
self.draw_location[self.edit_existing_location] = True
edit_properties_location = self.edit_existing_location
# Since a location was added or edited, set file modification to true.
self.is_modified = True
else:
# Cancel was pressed, draw the original location if we were editing as before.
if self.edit_existing_location is not None:
self.draw_location[self.edit_existing_location] = True
self.editing_area = False
self.edit_existing_location = None
self.clearAreaSelection()
# Update the entire image overlay.
self.updateOverlay()
self.edit_area_button[LocationFunction.ADD_LOCATION_AREA].setEnabled(True)
self.edit_area_button[LocationFunction.ADD_LOCATION_AREA].setChecked(False)
self.edit_area_button[LocationFunction.EDIT_EXISTING_AREA].setChecked(False)
clearLayoutAndFixHeight(self.configuration_layout)
if edit_properties_location is not None:
self.edit_properties_location = edit_properties_location
self.startPropertyEdit()
def startPropertyEdit(self):
self.editing_properties = True
self.edit_existing_location = self.edit_properties_location
self.edit_area_button[LocationFunction.ADD_LOCATION_AREA].setEnabled(True)
self.edit_area_button[LocationFunction.EDIT_EXISTING_AREA].setEnabled(True)
# Construct the configuration layout.
clearLayoutAndFixHeight(self.configuration_layout)
self.update_name_label = QLabel("Location (" + self.edit_properties_location + ") New Name: ", self.widget)
self.configuration_layout.addWidget(self.update_name_label)
self.update_name_textedit = QLineEdit(self.widget)
self.update_name_textedit.setText(self.edit_properties_location)
self.update_name_textedit.textEdited.connect(self.locationNameTextEdited)
self.configuration_layout.addWidget(self.update_name_textedit)
self.update_name_button = QPushButton("Update location Name", self.widget)
self.update_name_button.clicked[bool].connect(self.updateLocationName)
self.update_name_button.setEnabled(False)
self.configuration_layout.addWidget(self.update_name_button)
self.remove_location_button = QPushButton("Remove Location", self.widget)
self.remove_location_button.clicked[bool].connect(self.removeCurrentLocation)
self.configuration_layout.addWidget(self.remove_location_button)
self.configuration_layout.addStretch(1)
self.updateOverlay()
def endPropertyEdit(self):
self.edit_area_button[LocationFunction.ADD_LOCATION_AREA].setEnabled(True)
self.edit_area_button[LocationFunction.EDIT_EXISTING_AREA].setEnabled(False)
clearLayoutAndFixHeight(self.configuration_layout)
self.update_name_label = None
self.update_name_textedit = None
self.update_name_button = None
self.editing_properties = False
self.edit_properties_location = None
self.updateOverlay()
def locationNameTextEdited(self, text):
if str(text) != self.edit_properties_location:
self.update_name_button.setEnabled(True)
else:
self.update_name_button.setEnabled(False)
def updateLocationName(self):
old_loc_name = self.edit_properties_location
new_loc_name = str(self.update_name_textedit.text())
if new_loc_name in self.locations:
# This means that two locations need to be merged
self.locations[new_loc_name] = self.locations[new_loc_name].united(self.locations.pop(old_loc_name))
else:
# This is a simple rename task.
self.locations[new_loc_name] = self.locations.pop(old_loc_name)
self.location_colors[new_loc_name] = self.location_colors.pop(old_loc_name)
self.draw_location[new_loc_name] = self.draw_location.pop(old_loc_name)
# Since a location name was modified, set file modification to true.
self.is_modified = True
# Restart property edit with the updated name.
self.endPropertyEdit()
self.edit_properties_location = new_loc_name
self.startPropertyEdit()
def removeCurrentLocation(self):
old_loc_name = self.edit_properties_location
self.removeLocation(old_loc_name)
self.endPropertyEdit()
self.updateOverlay()
# Since a location was removed, set file modification to true.
self.is_modified = True
def removeLocation(self, loc_name):
if loc_name in self.locations:
self.locations.pop(loc_name)
if loc_name in self.location_colors:
self.location_colors.pop(loc_name)
if loc_name in self.draw_location:
self.draw_location.pop(loc_name)
def isModified(self):
return self.is_modified
def mousePressEvent(self, event):
if self.editing_area:
self.subtract_new_selection = event.button() == Qt.RightButton
self.new_selection_start_point = event.pos()
self.new_selection_end_point = event.pos()
self.new_selection = None
else:
loc = self.getLocationNameFromPoint(event.pos())
if loc is not None:
self.edit_properties_location = loc
self.startPropertyEdit()
else:
self.endPropertyEdit()
def mouseReleaseEvent(self, event):
if self.editing_area:
self.mouseMoveEvent(event)
if self.new_selection is not None:
if self.current_selection is None and self.subtract_new_selection == False:
self.current_selection = self.new_selection
if self.subtract_new_selection:
self.current_selection = self.current_selection.subtracted(self.new_selection)
else:
self.current_selection = self.current_selection.united(self.new_selection)
self.new_selection = None
self.subtract_new_selection = None
def mouseMoveEvent(self, event):
if self.editing_area:
# First make sure we update the region corresponding to the old mark.
old_overlay_update_rect = self.get_rectangular_polygon(self.new_selection_start_point, self.new_selection_end_point)
# Draw new mark, taking some care to reduce the size of the polygon's bottom right corner by (1,1).
self.new_selection_end_point = event.pos()
self.new_selection = self.get_rectangular_polygon(self.new_selection_start_point, self.new_selection_end_point)
self.new_selection = self.new_selection.boundingRect()
self.new_selection.setHeight(self.new_selection.height() - 1)
self.new_selection.setWidth(self.new_selection.width() - 1)
self.new_selection = QPolygon(self.new_selection, True)
# Next determine the region that needs to be update because of the new mark.
new_overlay_update_rect = self.get_rectangular_polygon(self.new_selection_start_point, self.new_selection_end_point)
overlay_update_region = (old_overlay_update_rect + new_overlay_update_rect).boundingRect()
self.updateOverlay(overlay_update_region)
def updateOverlay(self, rect = None):
# Redraw the overlay image from scratch using the location image and current location.
self.image.overlay_image.fill(Qt.transparent)
painter = QPainter(self.image.overlay_image)
painter.setBackgroundMode(Qt.TransparentMode)
painter.setCompositionMode(QPainter.CompositionMode_Source)
for location in self.locations:
if self.draw_location[location]:
color = self.location_colors[location]
if self.edit_properties_location == location and self.editing_properties:
color = self.edit_area_selection_color
lineColor = QColor(color)
lineColor.setAlpha(255)
brushColor = QColor(color)
brushColor.setAlpha(128)
painter.setPen(lineColor)
painter.setBrush(brushColor)
painter.drawPolygon(self.locations[location])
if (self.current_selection is not None) or (self.new_selection is not None):
lineColor = QColor(self.edit_area_selection_color)
lineColor.setAlpha(255)
brushColor = QColor(self.edit_area_selection_color)
brushColor.setAlpha(128)
painter.setPen(lineColor)
painter.setBrush(brushColor)
if self.new_selection is not None:
# Create a temporary polygon as the new selection is being drawn.
if self.current_selection is not None:
current_selection = QPolygon(self.current_selection)
if self.subtract_new_selection:
current_selection = current_selection.subtracted(self.new_selection)
else:
current_selection = current_selection.united(self.new_selection)
painter.drawPolygon(current_selection)
elif self.subtract_new_selection == False:
painter.drawPolygon(self.new_selection)
else:
painter.drawPolygon(self.current_selection)
painter.end()
if rect is None:
self.image.update()
else:
self.image.update(rect)
def getUniqueNameAndColor(self):
"""
Use golden ratio to generate unique colors.
http://martin.ankerl.com/2009/12/09/how-to-create-random-colors-programmatically/
"""
name = "new_loc" + str(self.unique_loc_counter)
h = int(359 * (self.unique_loc_counter * 0.618033988749895))
h = h % 359
self.unique_loc_counter += 1
return name, QColor.fromHsv(h, 255, 255)
def get_rectangular_polygon(self, pt1, pt2):
return QPolygon([pt1, QPoint(pt1.x(), pt2.y()), pt2, QPoint(pt2.x(), pt1.y())])
