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 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 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 MultiRobotPatrollerPlugin(Plugin):
def __init__(self, context):
super(MultiRobotPatrollerPlugin, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('MultiRobotPatrollerPlugin')
# Create QWidget
self.widget = QWidget()
self.master_layout = QVBoxLayout(self.widget)
self.buttons = []
self.button_layout = QHBoxLayout()
self.master_layout.addLayout(self.button_layout)
for button_text in ["Play", "Pause"]:
button = QPushButton(button_text, self.widget)
button.clicked[bool].connect(self.handle_button)
button.setCheckable(True)
self.button_layout.addWidget(button)
if button_text == "Pause":
button.setChecked(True)
self.buttons.append(button)
self.text_labels = {}
self.widget.setObjectName('MultiRobotPatrollerPluginUI')
if context.serial_number() > 1:
self.widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
context.add_widget(self.widget)
self.points = rospy.get_param("points")
self.flip_direction = rospy.get_param("flip_direction")
self.available_robots = []
self.global_start_counter = 0
self.global_forward = True
self.available_robot_subscriber = rospy.Subscriber("/available_robots", AvailableRobotArray,
self.available_robot_callback)
self.robot_goals = {}
self.paused = True
self.connect(self.widget, SIGNAL("update"), self.update)
def navigate_robot(self, robot_name, start_counter, forward):
client = actionlib.SimpleActionClient('/' + robot_name + '/move_base_interruptable', MoveBaseAction)
resolved_frame = '/' + robot_name + '/level_mux/map'
# Waits until the action server has started up and started
# listening for goals.
while not rospy.is_shutdown():
if client.wait_for_server(rospy.Duration(1)):
break
counter = start_counter
failures = 0
while not rospy.is_shutdown():
# Creates a goal to send to the action server.
if not self.paused:
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = resolved_frame
goal.target_pose.header.stamp = rospy.get_rostime()
goal.target_pose.pose.position.x = self.points[counter][0]
goal.target_pose.pose.position.y = self.points[counter][1]
goal.target_pose.pose.position.z = 0.0
q = tf.transformations.quaternion_from_euler(0, 0, self.points[counter][2])
goal.target_pose.pose.orientation = Quaternion(*q)
# Sends the goal to the action server.
client.send_goal(goal)
self.robot_goals[robot_name] = counter
self.widget.emit(SIGNAL("update"))
# Waits for the server to finish performing the action.
while not rospy.is_shutdown():
if client.wait_for_result(rospy.Duration(1)):
break
elif self.paused:
client.cancel_goal()
break
if client.get_state() == GoalStatus.SUCCEEDED or failures >= 3:
failures = 0
if forward:
counter = (counter + 1) % len(self.points)
else:
counter = counter - 1
if counter < 0:
counter = len(self.points) - 1
else:
# Try the goal again after given seconds
time.sleep(3.0)
failures += 1
else:
time.sleep(1.0)
def available_robot_callback(self, msg):
for robot in msg.robots:
robot_name = robot.name
if robot_name not in self.available_robots:
self.available_robots.append(robot_name)
thread.start_new_thread(self.navigate_robot, (robot_name, self.global_start_counter,
self.global_forward))
if self.flip_direction:
self.global_forward = not self.global_forward
self.global_start_counter = (self.global_start_counter + 1) % len(self.points)
def update(self):
for robot in self.robot_goals:
point = self.points[self.robot_goals[robot]]
text = robot + " -> (" + str(point[0]) + "," + str(point[1]) + ")"
if robot not in self.text_labels:
self.text_labels[robot] = QLabel(robot, self.widget)
self.master_layout.addWidget(self.text_labels[robot])
self.text_labels[robot].setText(text)
def handle_button(self):
source = self.sender()
if ((source.text() == "Pause" and self.paused) or
(source.text() == "Play" and not self.paused)):
source.setChecked(True)
return
for button in self.buttons:
if button != source:
button.setChecked(False)
self.paused = (source.text() == "Pause")
def save_settings(self, plugin_settings, instance_settings):
pass
def restore_settings(self, plugin_settings, instance_settings):
pass
class MultiRobotPasserGUIPlugin(Plugin):
def __init__(self, context):
super(MultiRobotPasserGUIPlugin, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('MultiRobotPasserGUIPlugin')
# Create QWidget
self.widget = QWidget()
self.master_layout = QVBoxLayout(self.widget)
self.buttons = {}
self.button_layout = QHBoxLayout()
self.master_layout.addLayout(self.button_layout)
for button_text in ["Enable", "Disable"]:
button = QPushButton(button_text, self.widget)
button.clicked[bool].connect(self.handle_button)
button.setCheckable(True)
self.button_layout.addWidget(button)
self.buttons[button_text] = button
self.widget.setObjectName('MultiRobotPasserGUIPluginUI')
if context.serial_number() > 1:
self.widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
context.add_widget(self.widget)
self.status_subscriber = rospy.Subscriber("status", Bool, self.status_callback)
self.enable = rospy.ServiceProxy("enable", Empty)
self.disable = rospy.ServiceProxy("disable", Empty)
self.enabled = False
self.connect(self.widget, SIGNAL("update"), self.update)
def status_callback(self, msg):
self.enabled = msg.data
self.widget.emit(SIGNAL("update"))
def update(self):
if self.enabled:
self.buttons["Enable"].setChecked(True)
self.buttons["Disable"].setChecked(False)
else:
self.buttons["Enable"].setChecked(False)
self.buttons["Disable"].setChecked(True)
def handle_button(self):
source = self.sender()
if ((source.text() == "Enable" and self.enabled) or
(source.text() == "Disable" and not self.enabled)):
source.setChecked(True)
return
for _,button in self.buttons.iteritems():
if button != source:
button.setChecked(False)
# Call appropriate service here.
if source.text() == "Enable":
self.enable()
else:
self.disable()
def save_settings(self, plugin_settings, instance_settings):
pass
def restore_settings(self, plugin_settings, instance_settings):
pass
class RqtSrVisualServoing(Plugin):
def __init__(self, context):
super(RqtSrVisualServoing, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('RqtSrVisualServoing')
# # 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
# Create QWidget
self.ui = QWidget()
# Get path to UI file which is in our packages resource directory
rp = rospkg.RosPack()
self.ui_file = os.path.join(rp.get_path(PKG), 'resource', 'gui.ui')
# Extend the widget with all attributes and children from UI file
loadUi(self.ui_file, self.ui)
# Give QObjects reasonable names
self.ui.setObjectName('RqtSrVisualServoingUi')
# Show ui.windowTitle on left-top of each plugin (when it's set in ui).
# This is useful when you open multiple plugins at once. Also if you
# open multiple instances of your plugin at once, these lines add
# number to make it easy to tell from pane to pane.
if context.serial_number() > 1:
self.ui.setWindowTitle(self.ui.windowTitle() + (' (%d)' % context.serial_number()))
# Wire up the buttons
self.ui.startBtn.clicked.connect( self.start_clicked )
self.ui.stopBtn.clicked.connect( self.stop_clicked )
# Annoyingly setting the icon theme in designer only works in designer,
# we have to set it again here for it to work at run time
self.ui.startBtn.setIcon(QIcon.fromTheme('media-playback-start'))
self.ui.stopBtn.setIcon(QIcon.fromTheme('media-playback-stop'))
# Add widget to the user interface
context.add_widget(self.ui)
# Setup a model for the feedback and link to the view.
self.feedback_model = QStandardItemModel(0,2)
self.feedback_model.setHorizontalHeaderLabels(['Name','Value'])
self.ui.feedbackView.setModel(self.feedback_model)
self.ui.connect( self.ui, SIGNAL('feedback(QString)'), self.update_feedback )
# ROS setup
self.last_feedback = None
self.client = actionlib.SimpleActionClient('visual_servo', VisualServoingAction)
msg = ""
if self.client.wait_for_server(rospy.Duration(2.0)):
msg = "Found action server, servoing appears to be running"
rospy.loginfo(msg)
else:
msg = "Can't find action server, servoing not running"
rospy.logerr(msg)
self.ui.statusValue.setText(msg)
def shutdown_plugin(self):
# TODO unregister all publishers here
pass
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
#def trigger_configuration(self):
# Comment in to signal that the plugin has a way to configure it
# Usually used to open a configuration dialog
def start_clicked(self):
goal = VisualServoingActionGoal()
self.client.send_goal(goal, feedback_cb = self._feedback_cb)
self.ui.statusValue.setText("Starting")
def stop_clicked(self):
self.client.cancel_all_goals()
self.ui.statusValue.setText("Stopped")
self.feedback_model.clear()
def _feedback_cb(self, feedback):
# We can't update the UI in this thread so stash the data and emit a
# signal that can be traped by the main thread and update the ui.
self.last_feedback = feedback
self.ui.emit( SIGNAL('feedback(QString)'), "" )
def update_feedback(self, data):
"""Listen for feedback signals and update the interface."""
fb = self.last_feedback
self.ui.statusValue.setText(str(self.client.get_goal_status_text()))
# Update the feedback model, which triggers the view to update
m = self.feedback_model
m.setHorizontalHeaderLabels(['Name','Value'])
m.setItem(0,0,QStandardItem('distance'))
m.setItem(0,1,QStandardItem(str(fb.distance)))
m.setItem(1,0,QStandardItem('object_pose.position.x'))
m.setItem(1,1,QStandardItem(str(fb.object_pose.position.x)))
m.setItem(2,0,QStandardItem('object_pose.position.y'))
m.setItem(2,1,QStandardItem(str(fb.object_pose.position.y)))
m.setItem(3,0,QStandardItem('object_pose.position.z'))
m.setItem(3,1,QStandardItem(str(fb.object_pose.position.z)))
m.setItem(4,0,QStandardItem('object_pose.orientation.x'))
m.setItem(4,1,QStandardItem(str(fb.object_pose.orientation.x)))
m.setItem(5,0,QStandardItem('object_pose.orientation.y'))
m.setItem(5,1,QStandardItem(str(fb.object_pose.orientation.y)))
m.setItem(6,0,QStandardItem('object_pose.orientation.z'))
m.setItem(6,1,QStandardItem(str(fb.object_pose.orientation.z)))
m.setItem(7,0,QStandardItem('object_pose.orientation.w'))
m.setItem(7,1,QStandardItem(str(fb.object_pose.orientation.w)))
m.setItem(8,0,QStandardItem('grasp_pose.position.x'))
m.setItem(8,1,QStandardItem(str(fb.grasp_pose.position.x)))
m.setItem(9,0,QStandardItem('grasp_pose.position.y'))
m.setItem(9,1,QStandardItem(str(fb.grasp_pose.position.y)))
m.setItem(10,0,QStandardItem('grasp_pose.position.z'))
m.setItem(10,1,QStandardItem(str(fb.grasp_pose.position.z)))
m.setItem(11,0,QStandardItem('grasp_pose.orientation.x'))
m.setItem(11,1,QStandardItem(str(fb.grasp_pose.orientation.x)))
m.setItem(12,0,QStandardItem('grasp_pose.orientation.y'))
m.setItem(12,1,QStandardItem(str(fb.grasp_pose.orientation.y)))
m.setItem(13,0,QStandardItem('grasp_pose.orientation.z'))
m.setItem(13,1,QStandardItem(str(fb.grasp_pose.orientation.z)))
m.setItem(14,0,QStandardItem('grasp_pose.orientation.w'))
m.setItem(14,1,QStandardItem(str(fb.grasp_pose.orientation.w)))
self.ui.feedbackView.resizeColumnsToContents()
class LevelSelectorPlugin(Plugin):
def __init__(self, context):
super(LevelSelectorPlugin, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('LevelSelectorPlugin')
# Create QWidget
self._widget = QWidget()
# self._widget.setFont(QFont("Times", 15, QFont.Bold))
self._button_layout = QVBoxLayout(self._widget)
self.buttons = []
self.text_label = QLabel("Waiting for MultiLevelMapData...",
self._widget)
self._button_layout.addWidget(self.text_label)
self._widget.setObjectName('LevelSelectorPluginUI')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
context.add_widget(self._widget)
self.connect(self._widget, SIGNAL("update_buttons"),
self.update_buttons)
self.connect(self._widget, SIGNAL("update_button_status"),
self.update_button_status)
# Subcribe to the multi level map data to get information about all the maps.
self.multimap_subscriber = rospy.Subscriber("map_metadata",
MultiLevelMapData,
self.process_multimap)
self.levels = []
self.current_level = None
# Subscribe to the current level we are on.
self.status_subscriber = None
# Create a service proxy to change the current level.
self.level_selector_proxy = rospy.ServiceProxy(
"level_mux/change_current_level", ChangeCurrentLevel)
self.level_selector_proxy.wait_for_service()
def process_multimap(self, msg):
self.levels = msg.levels
self._widget.emit(SIGNAL("update_buttons"))
def update_buttons(self):
self.clean()
for index, level in enumerate(self.levels):
self.text_label.setText("Choose Level: ")
button = QPushButton(level.level_id, self._widget)
button.clicked[bool].connect(self.handle_button)
button.setCheckable(True)
self._button_layout.addWidget(button)
self.buttons.append(button)
# Subscribe to the current level we are on.
if self.status_subscriber is None:
self.status_subscriber = rospy.Subscriber(
"level_mux/current_level", LevelMetaData,
self.process_level_status)
def update_button_status(self):
for index, level in enumerate(self.levels):
if self.current_level == level.level_id:
self.buttons[index].setChecked(True)
else:
self.buttons[index].setChecked(False)
def process_level_status(self, msg):
level_found = False
for level in self.levels:
if msg.level_id == level.level_id:
self.current_level = level.level_id
level_found = True
break
if not level_found:
self.current_level = None
self._widget.emit(SIGNAL("update_button_status"))
def handle_button(self):
source = self.sender()
if source.text() == self.current_level:
source.setChecked(True)
return
# Construct a identity pose. The level selector cannot be used to choose the initialpose, as it does not have
# the interface for specifying the position. The position should be specified via rviz.
origin_pose = PoseWithCovarianceStamped()
origin_pose.header.frame_id = frameIdFromLevelId(source.text())
origin_pose.pose.pose.orientation.w = 1 # Makes the origin quaternion valid.
origin_pose.pose.covariance[0] = 1.0
origin_pose.pose.covariance[7] = 1.0
origin_pose.pose.covariance[14] = 1.0
origin_pose.pose.covariance[21] = 1.0
origin_pose.pose.covariance[28] = 1.0
origin_pose.pose.covariance[35] = 1.0
# Don't actually publish the initial pose via the level selector. It doesn't know any better.
self.level_selector_proxy(source.text(), False, origin_pose)
def clean(self):
while self._button_layout.count():
item = self._button_layout.takeAt(0)
item.widget().deleteLater()
def save_settings(self, plugin_settings, instance_settings):
pass
def restore_settings(self, plugin_settings, instance_settings):
pass
class LevelSelectorPlugin(Plugin):
def __init__(self, context):
super(LevelSelectorPlugin, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('LevelSelectorPlugin')
# Create QWidget
self._widget = QWidget()
# self._widget.setFont(QFont("Times", 15, QFont.Bold))
self._button_layout = QVBoxLayout(self._widget)
self.buttons = []
self.text_label = QLabel("Waiting for MultiLevelMapData...", self._widget)
self._button_layout.addWidget(self.text_label)
self._widget.setObjectName('LevelSelectorPluginUI')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
context.add_widget(self._widget)
self.connect(self._widget, SIGNAL("update_buttons"), self.update_buttons)
self.connect(self._widget, SIGNAL("update_button_status"), self.update_button_status)
# Subcribe to the multi level map data to get information about all the maps.
self.multimap_subscriber = rospy.Subscriber("map_metadata", MultiLevelMapData, self.process_multimap)
self.levels = []
self.current_level = None
# Subscribe to the current level we are on.
self.status_subscriber = None
# Create a service proxy to change the current level.
self.level_selector_proxy = rospy.ServiceProxy("level_mux/change_current_level", ChangeCurrentLevel)
self.level_selector_proxy.wait_for_service()
def process_multimap(self, msg):
self.levels = msg.levels
self._widget.emit(SIGNAL("update_buttons"))
def update_buttons(self):
self.clean()
for index, level in enumerate(self.levels):
self.text_label.setText("Choose Level: ")
button = QPushButton(level.level_id, self._widget)
button.clicked[bool].connect(self.handle_button)
button.setCheckable(True)
self._button_layout.addWidget(button)
self.buttons.append(button)
# Subscribe to the current level we are on.
if self.status_subscriber is None:
self.status_subscriber = rospy.Subscriber("level_mux/current_level", LevelMetaData, self.process_level_status)
def update_button_status(self):
for index, level in enumerate(self.levels):
if self.current_level == level.level_id:
self.buttons[index].setChecked(True)
else:
self.buttons[index].setChecked(False)
def process_level_status(self, msg):
level_found = False
for level in self.levels:
if msg.level_id == level.level_id:
self.current_level = level.level_id
level_found = True
break
if not level_found:
self.current_level = None
self._widget.emit(SIGNAL("update_button_status"))
def handle_button(self):
source = self.sender()
if source.text() == self.current_level:
source.setChecked(True)
return
# Construct a identity pose. The level selector cannot be used to choose the initialpose, as it does not have
# the interface for specifying the position. The position should be specified via rviz.
origin_pose = PoseWithCovarianceStamped()
origin_pose.header.frame_id = frameIdFromLevelId(source.text())
origin_pose.pose.pose.orientation.w = 1 # Makes the origin quaternion valid.
origin_pose.pose.covariance[0] = 1.0
origin_pose.pose.covariance[7] = 1.0
origin_pose.pose.covariance[14] = 1.0
origin_pose.pose.covariance[21] = 1.0
origin_pose.pose.covariance[28] = 1.0
origin_pose.pose.covariance[35] = 1.0
# Don't actually publish the initial pose via the level selector. It doesn't know any better.
self.level_selector_proxy(source.text(), False, origin_pose)
def clean(self):
while self._button_layout.count():
item = self._button_layout.takeAt(0)
item.widget().deleteLater()
def save_settings(self, plugin_settings, instance_settings):
pass
def restore_settings(self, plugin_settings, instance_settings):
pass