class PyConsole(Plugin):
"""
Plugin providing an interactive Python console
"""
def __init__(self, context):
super(PyConsole, self).__init__(context)
self.setObjectName('PyConsole')
self._context = context
self._use_spyderlib = _has_spyderlib
self._console_widget = None
self._widget = QWidget()
self._widget.setLayout(QVBoxLayout())
self._widget.layout().setContentsMargins(0, 0, 0, 0)
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
self._context.add_widget(self._widget)
def _switch_console_widget(self):
self._widget.layout().removeWidget(self._console_widget)
self.shutdown_console_widget()
if _has_spyderlib and self._use_spyderlib:
self._console_widget = SpyderConsoleWidget(self._context)
self._widget.setWindowTitle('SpyderConsole')
else:
self._console_widget = PyConsoleWidget(self._context)
self._widget.setWindowTitle('PyConsole')
if self._context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % self._context.serial_number()))
self._widget.layout().addWidget(self._console_widget)
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('use_spyderlib', self._use_spyderlib)
def restore_settings(self, plugin_settings, instance_settings):
self._use_spyderlib = _has_spyderlib and (instance_settings.value('use_spyderlib', True) in [True, 'true'])
self._switch_console_widget()
def trigger_configuration(self):
options = [
{'title': 'SpyderConsole', 'description': 'Advanced Python console with tab-completion (needs spyderlib to be installed).', 'enabled': _has_spyderlib},
{'title': 'PyConsole', 'description': 'Simple Python console.'},
]
dialog = SimpleSettingsDialog(title='PyConsole Options')
dialog.add_exclusive_option_group(title='Console Type', options=options, selected_index=int(not self._use_spyderlib))
console_type = dialog.get_settings()[0]
new_use_spyderlib = {0: True, 1: False}.get(console_type['selected_index'], self._use_spyderlib)
if self._use_spyderlib != new_use_spyderlib:
self._use_spyderlib = new_use_spyderlib
self._switch_console_widget()
def shutdown_console_widget(self):
if self._console_widget is not None and hasattr(self._console_widget, 'shutdown'):
self._console_widget.shutdown()
def shutdown_plugin(self):
self.shutdown_console_widget()
class MyPlugin(Plugin):
def __init__(self, context):
super(MyPlugin, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('MyPlugin')
rospy.loginfo("Remora: Basic Controller Online")
# 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._widget = QWidget()
# Get path to UI file which is a sibling of this file
# in this example the .ui and .py file are in the same folder
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'remora_gui.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('MyPluginUi')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). 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.
self._widget.setWindowTitle('Basic Console')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle()+(' (%d)'% context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
# Register all subsribers here
return
def shutdown_plugin(self):
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
class Interface(Plugin):
def __init__(self, context):
super(Interface, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('Interface')
# 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._widget = QWidget()
# Get path to UI file which should be in the "resource" folder of this package
ui_file = os.path.join(rospkg.RosPack().get_path('dsi_human_gui'),
'resource', 'Interface.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('MyPluginUi')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). 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._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
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
class Visualizer(Plugin):
def __init__(self, context):
super(Visualizer, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('Visualizer')
# 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._widget = QWidget()
# Get path to UI file which should be in the "resource" folder of this package
ui_file = os.path.join(rospkg.RosPack().get_path('consai2_gui'),
'resource', 'Visualizer.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget, {"PaintWidget": PaintWidget})
# Give QObjects reasonable names
self._widget.setObjectName('VisualizerUi')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). 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._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
self._redraw_timer = QTimer()
self._redraw_timer.setInterval(16)
self._redraw_timer.timeout.connect(self._widget.update)
self._redraw_timer.start()
def shutdown_plugin(self):
pass
def save_settings(self, plugin_settings, instance_settings):
pass
def restore_settings(self, plugin_settings, instance_settings):
pass
class MissionModePlugin(Plugin):
def __init__(self, context):
super(MissionModePlugin, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('MissionModePlugin')
# Create QWidget
self._widget = QWidget()
rp = rospkg.RosPack()
# Get path to UI file which should be in the "resource" folder of this package
ui_file = os.path.join(rp.get_path('behavior_selector'), 'resource',
'MissionModePlugin.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('MissionModePluginUi')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). 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._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
self._widget.start_push_button.pressed.connect(self._on_start_pressed)
self._widget.end_push_button.pressed.connect(self._on_end_pressed)
self._widget.stop_push_button.pressed.connect(self._on_stop_pressed)
def _on_start_pressed(self):
mode = START
self._change_mode(mode)
def _on_end_pressed(self):
mode = END
self._change_mode(mode)
def _on_stop_pressed(self):
mode = KILL
self._change_mode(mode)
def _change_mode(self, mode):
rospy.wait_for_service('change_mode', 1)
try:
mm = rospy.ServiceProxy('change_mode', MissionModeChange)
mm(mode)
except rospy.ServiceException as e:
print("Service call failed: %s" % e)
class JoyVis(Plugin):
def __init__(self, context):
super(JoyVis, self).__init__(context)
self.setObjectName('Joy')
from argparse import ArgumentParser
parser = ArgumentParser()
parser.add_argument("-q",
"--quiet",
action="store_true",
dest="quiet",
help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
self._widget = QWidget()
ui_file = os.path.join(rospkg.RosPack().get_path('rqt_joy'),
'resource', 'Joy.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('JoyUi')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
context.add_widget(self._widget)
self._widget.joy_1_x.setValue(0)
self._widget.joy_1_y.setValue(0)
rospy.Subscriber("joy", Joy, self.joyMessageCallback)
def shutdown_plugin(self):
pass
def save_settings(self, plugin_settings, instance_settings):
pass
def restore_settings(self, plugin_settings, instance_settings):
pass
def joyMessageCallback(self, joy):
self._widget.joy_1_x.setValue(joy.axes[0] * 100)
self._widget.joy_1_y.setValue(joy.axes[1] * 100)
class FakeMotionRendererPlugin(Plugin):
def __init__(self, context):
super(FakeMotionRendererPlugin, self).__init__(context)
self.setObjectName('FakeMotionRendererPlugin')
self._widget = QWidget()
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
ui_file = os.path.join(rospkg.RosPack().get_path('motion_renderer'),
'resource', 'fake_motion_renderer.ui')
loadUi(ui_file, self._widget)
context.add_widget(self._widget)
self.color = 0.0
self._widget.glWidget.paintGL = self.paintGL
self._widget.glWidget.timerEvent = self.timerEvent
self._widget.glWidget.startTimer(40)
self.gl = self._widget.glWidget.context().versionFunctions(
QOpenGLVersionProfile((QSurfaceFormat())))
self.gl.initializeOpenGLFunctions()
def timerEvent(self, event):
self._widget.glWidget.update()
def paintGL(self):
self.color = self.color + 0.01
if self.color >= 1.0:
self.color = 0.0
self.gl.glClearColor(1.0, self.color, 1.0, 1.0)
self.gl.glClear(self.gl.GL_COLOR_BUFFER_BIT
| self.gl.GL_DEPTH_BUFFER_BIT)
def shutdown_plugin(self):
pass
def save_settings(self, plugin_settings, instance_settings):
pass
def restore_settings(self, plugin_settings, instance_settings):
pass
class Smach(Plugin):
update_graph_sig = Signal(str)
def __init__(self, context):
super(Smach, self).__init__(context)
self.initialized = 0
self._dotcode_sub = None
self._topic_dict = {}
self._update_thread = WorkerThread(self._update_thread_run,
self._update_finished)
# Give QObjects reasonable names
self.setObjectName('Smach')
# 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._widget = QWidget()
# Get path to UI file which is a sibling of this file
# in this example the .ui and .py file are in the same folder
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'rqt_smach.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.ns_refresh_button.setIcon(QIcon.fromTheme('view-refresh'))
self._widget.setObjectName('SmachPluginUi')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). 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._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
palette = QPalette()
palette.setColor(QPalette.Background, Qt.white)
self._widget.setPalette(palette)
#Connect widgets with corresponding methods
self._widget.namespace_input.currentIndexChanged.connect(
self._handle_ns_changed)
self._widget.ns_refresh_button.clicked.connect(self.refresh_combo_box)
self._widget.restrict_ns.clicked.connect(self.refresh_combo_box)
self._widget.ud_path_input.currentIndexChanged.connect(
self._handle_ud_path)
self._widget.ud_set_initial.clicked.connect(self._handle_ud_set_path)
self._widget.ud_text_browser.setReadOnly(1)
self._widget.show_implicit_button.clicked.connect(
self._handle_show_implicit)
self._widget.help_button.setIcon(QIcon.fromTheme('help-contents'))
self._widget.help_button.clicked.connect(self._handle_help)
self._widget.tree.clicked.connect(self._handle_tree_clicked)
#Depth and width spinners:
self._widget.depth_input.setRange(-1, 1337)
self._widget.depth_input.setValue(-1)
self._widget.depth_input.valueChanged.connect(self._set_depth)
self._widget.label_width_input.setRange(1, 1337)
self._widget.label_width_input.setValue(40)
self._widget.label_width_input.valueChanged.connect(self._set_width)
self._widget.tree.setColumnCount(1)
self._widget.tree.setHeaderLabels(["Containers"])
self._widget.tree.show()
self._ns = ""
self.refresh_combo_box()
# Bind path list
self._widget.path_input.currentIndexChanged.connect(
self._handle_path_changed)
#Keep Combo Boxes sorted
self._widget.namespace_input.setInsertPolicy(6)
self._widget.path_input.setInsertPolicy(6)
self._widget.ud_path_input.setInsertPolicy(6)
#Set up mouse actions for xdot widget
self._widget.xdot_widget.register_select_callback(self.select_cb)
# Create graph data structures
# Containers is a map of (container path) -> container proxy
self._containers = {}
self._top_containers = {}
# smach introspection client
self._client = smach_ros.IntrospectionClient()
self._selected_paths = []
# Message subscribers
self._structure_subs = {}
self._status_subs = {}
# Initialize xdot display state
self._path = '/'
self._needs_zoom = True
self._structure_changed = True
self._max_depth = -1
self._show_all_transitions = False
self._label_wrapper = textwrap.TextWrapper(40, break_long_words=True)
self._graph_needs_refresh = True
self._tree_needs_refresh = True
self._keep_running = True
self._update_server_list()
self._update_graph()
self._update_tree()
# Start a timer to update the server list
self._server_timer = QTimer(self)
self._server_timer.timeout.connect(self._update_server_list)
self._server_timer.start(1000)
# Start a timer to update the graph display
self._graph_timer = QTimer(self)
self._graph_timer.timeout.connect(self._update_graph)
self._graph_timer.start(1093)
# Start a timer to update the._widget.tree display
self._tree_timer = QTimer(self)
self._tree_timer.timeout.connect(self._update_tree)
self._tree_timer.start(1217)
def _handle_tree_clicked(self):
selected = self._widget.tree.selectedItems()[0]
path = "/" + str(selected.text(0))
parent = selected.parent()
while parent:
path = "/" + str(parent.text(0)) + path
parent = parent.parent()
self._widget.ud_path_input.setCurrentIndex(
self._widget.ud_path_input.findText(path))
def _handle_help(self):
"""Event: Help button pressed"""
helpMsg = QMessageBox()
helpMsg.setWindowTitle("Keyboard Controls")
helpMsg.setIcon(QMessageBox.Information)
helpMsg.setText(
"Pan: Arrow Keys\nZoom: PageUp / PageDown\nZoom To Fit: F\nRefresh: R"
)
ret = helpMsg.exec_()
def select_cb(self, event):
"""Event: Click to select a graph node to display user data and update the graph."""
# Only set string status
x = event.x()
y = event.y()
url = self._widget.xdot_widget.get_url(x, y)
if url is None:
return
item = self._widget.xdot_widget.items_by_url.get(url.url, None)
if item:
self._widget.status_bar.showMessage(item.url)
# Left button-up
if item.url not in self._containers:
if ':' in item.url:
container_url = '/'.join(item.url.split(':')[:-1])
else:
container_url = '/'.join(item.url.split('/')[:-1])
else:
container_url = item.url
if event.button() == Qt.LeftButton:
self._selected_paths = [item.url]
self._widget.ud_path_input.setCurrentIndex(
self._widget.ud_path_input.findText(item.url))
self._graph_needs_refresh = True
def _handle_show_implicit(self):
"""Event: Show Implicit button checked"""
self._show_all_transitions = not self._show_all_transitions
self._graph_needs_refresh = True
self._structure_changed = True
if self._widget.zoom_checkbox.isChecked():
self._needs_zoom = True
def _set_depth(self):
"""Event: Depth value changed"""
self._max_depth = self._widget.depth_input.value()
self._graph_needs_refresh = True
self._structure_changed = True
if self._widget.zoom_checkbox.isChecked():
self._needs_zoom = True
def _set_width(self):
"""Event: Label Width value changed"""
self._label_wrapper.width = self._widget.label_width_input.value()
self._graph_needs_refresh = True
def _handle_ud_set_path(self):
"""Event: Set as Initial State button pressed"""
state_path = self._widget.ud_path_input.currentText()
parent_path = get_parent_path(state_path)
if len(parent_path) > 0:
state = get_label(state_path)
server_name = self._containers[parent_path]._server_name
self._client.set_initial_state(server_name,
parent_path, [state],
timeout=rospy.Duration(60.0))
self._structure_changed = True
self._graph_needs_refresh = True
if self._widget.zoom_checkbox.isChecked():
self._needs_zoom = True
def _handle_ud_path(self):
"""Event: User Data selection combo box changed"""
path = self._widget.ud_path_input.currentText()
#Check the path is non-zero length
if len(path) > 0:
# Get the container corresponding to this path, since userdata is
# stored in the containers
if path not in self._containers:
parent_path = get_parent_path(path)
else:
parent_path = path
if parent_path not in self._containers:
parent_path = get_parent_path(parent_path)
if parent_path in self._containers:
# Get the container
container = self._containers[parent_path]
# Generate the userdata string
ud_str = ''
for (k, v) in container._local_data._data.iteritems():
ud_str += str(k) + ": "
vstr = str(v)
# Add a line break if this is a multiline value
if vstr.find('\n') != -1:
ud_str += '\n'
ud_str += vstr + '\n\n'
#Display the user data
self._widget.ud_text_browser.setPlainText(ud_str)
self._widget.ud_text_browser.show()
def _update_server_list(self):
"""Update the list of known SMACH introspection servers."""
# Discover new servers
if self._widget.restrict_ns.isChecked():
server_names = [self._widget.namespace_input.currentText()[0:-1]]
#self._status_subs = {}
else:
server_names = self._client.get_servers()
new_server_names = [
sn for sn in server_names if sn not in self._status_subs
]
# Create subscribers for newly discovered servers
for server_name in new_server_names:
# Create a subscriber for the plan structure (topology) published by this server
self._structure_subs[server_name] = rospy.Subscriber(
server_name + smach_ros.introspection.STRUCTURE_TOPIC,
SmachContainerStructure,
callback=self._structure_msg_update,
callback_args=server_name,
queue_size=50)
# Create a subscriber for the active states in the plan published by this server
self._status_subs[server_name] = rospy.Subscriber(
server_name + smach_ros.introspection.STATUS_TOPIC,
SmachContainerStatus,
callback=self._status_msg_update,
queue_size=50)
def _structure_msg_update(self, msg, server_name):
"""Update the structure of the SMACH plan (re-generate the dotcode)."""
# Just return if we're shutting down
if not self._keep_running:
return
# Get the node path
path = msg.path
pathsplit = path.split('/')
parent_path = '/'.join(pathsplit[0:-1])
rospy.logdebug("RECEIVED: " + path)
rospy.logdebug("CONTAINERS: " + str(self._containers.keys()))
# Initialize redraw flag
needs_redraw = False
# Determine if we need to update one of the proxies or create a new one
if path in self._containers:
rospy.logdebug("UPDATING: " + path)
# Update the structure of this known container
needs_redraw = self._containers[path].update_structure(msg)
else:
rospy.logdebug("CONSTRUCTING: " + path)
# Create a new container
container = ContainerNode(server_name, msg)
self._containers[path] = container
#Add items to ud path combo box
if self._widget.ud_path_input.findText(path) == -1:
self._widget.ud_path_input.addItem(path)
for item in container._children:
if self._widget.ud_path_input.findText(path + "/" +
item) == -1:
self._widget.ud_path_input.addItem(path + "/" + item)
# Store this as a top container if it has no parent
if parent_path == '':
self._top_containers[path] = container
# Append the path to the appropriate widgets
self._append_new_path(path)
# We need to redraw the graph if this container's parent is being viewed
if path.find(self._widget.path_input.currentText()) == 0:
needs_redraw = True
if needs_redraw:
self._structure_changed = True
if self._widget.zoom_checkbox.isChecked():
self._needs_zoom = True
self._tree_needs_refresh = True
self._graph_needs_refresh = True
def _status_msg_update(self, msg):
"""Process status messages."""
# Check if we're in the process of shutting down
if not self._keep_running:
return
# Get the path to the updating conainer
path = msg.path
rospy.logdebug("STATUS MSG: " + path)
# Check if this is a known container
if path in self._containers:
# Get the container and check if the status update requires regeneration
container = self._containers[path]
if container.update_status(msg):
self._graph_needs_refresh = True
self._tree_needs_refresh = True
def _append_new_path(self, path):
"""Append a new path to the path selection widgets"""
if ((not self._widget.restrict_ns.isChecked())
or ((self._widget.restrict_ns.isChecked()) and
(self._widget.namespace_input.currentText() in path))
or (path == self._widget.namespace_input.currentText()[0:-1])):
self._widget.path_input.addItem(path)
def _update_graph(self):
"""This thread continuously updates the graph when it changes.
The graph gets updated in one of two ways:
1: The structure of the SMACH plans has changed, or the display
settings have been changed. In this case, the dotcode needs to be
regenerated.
2: The status of the SMACH plans has changed. In this case, we only
need to change the styles of the graph.
"""
if self._keep_running and self._graph_needs_refresh and not rospy.is_shutdown(
):
# Get the containers to update
containers_to_update = {}
# Check if the path that's currently being viewed is in the
# list of existing containers
if self._path in self._containers:
# Some non-root path
containers_to_update = {
self._path: self._containers[self._path]
}
elif self._path == '/':
# Root path
containers_to_update = self._top_containers
# Check if we need to re-generate the dotcode (if the structure changed)
# TODO: needs_zoom is a misnomer
if self._structure_changed or self._needs_zoom:
dotstr = "digraph {\n\t"
dotstr += ';'.join([
"compound=true",
"outputmode=nodesfirst",
"labeljust=l",
"nodesep=0.5",
"minlen=2",
"mclimit=5",
"clusterrank=local",
"ranksep=0.75",
# "remincross=true",
# "rank=sink",
"ordering=\"\"",
])
dotstr += ";\n"
# Generate the rest of the graph
# TODO: Only re-generate dotcode for containers that have changed
for path, container in containers_to_update.items():
dotstr += container.get_dotcode(self._selected_paths, [],
0, self._max_depth,
self._containers,
self._show_all_transitions,
self._label_wrapper)
# The given path isn't available
if len(containers_to_update) == 0:
dotstr += '"__empty__" [label="Path not available.", shape="plaintext"]'
dotstr += '\n}\n'
# Set the dotcode to the new dotcode, reset the flags
self.set_dotcode(dotstr, zoom=False)
self._structure_changed = False
self._graph_needs_refresh = False
# Update the styles for the graph if there are any updates
for path, container in containers_to_update.items():
container.set_styles(self._selected_paths, 0, self._max_depth,
self._widget.xdot_widget.items_by_url,
self._widget.xdot_widget.subgraph_shapes,
self._containers)
self._widget.xdot_widget.repaint()
def set_dotcode(self, dotcode, zoom=True):
"""Set the xdot view's dotcode and refresh the display."""
# Set the new dotcode
if self._widget.xdot_widget.set_dotcode(dotcode, False):
# Re-zoom if necessary
if zoom or self._needs_zoom:
self._widget.xdot_widget.zoom_to_fit()
self._needs_zoom = False
def _update_tree(self):
"""Update the tree view."""
if self._keep_running and self._tree_needs_refresh and not rospy.is_shutdown(
):
self._tree_nodes = {}
self._widget.tree.clear()
for path, tc in self._top_containers.iteritems():
if ((not self._widget.restrict_ns.isChecked()) or
((self._widget.restrict_ns.isChecked()) and
(self._widget.namespace_input.currentText() in path)) or
(path
== self._widget.namespace_input.currentText()[0:-1])):
self.add_to_tree(path, None)
self._tree_needs_refresh = False
self._widget.tree.sortItems(0, 0)
def add_to_tree(self, path, parent):
"""Add a path to the tree view."""
if parent is None:
container = QTreeWidgetItem()
container.setText(0, self._containers[path]._label)
self._widget.tree.addTopLevelItem(container)
else:
container = QTreeWidgetItem(parent)
container.setText(0, self._containers[path]._label)
# Add children to_tree
for label in self._containers[path]._children:
child_path = '/'.join([path, label])
if child_path in self._containers.keys():
self.add_to_tree(child_path, container)
else:
child = QTreeWidgetItem(container)
child.setText(0, label)
def append_tree(self, container, parent=None):
"""Append an item to the tree view."""
if not parent:
node = QTreeWidgetItem()
node.setText(0, container._label)
self._widget.tree.addTopLevelItem(node)
for child_label in container._children:
child = QTreeWidgetItem(node)
child.setText(0, child_label)
def _update_thread_run(self):
"""Update the list of namespaces."""
_, _, topic_types = rospy.get_master().getTopicTypes()
self._topic_dict = dict(topic_types)
keys = list(self._topic_dict.keys())
namespaces = list()
for i in keys:
print i
if i.endswith("smach/container_status"):
namespaces.append(i[0:i.index("smach/container_status")])
self._widget.namespace_input.setItems.emit(namespaces)
@Slot()
def _update_finished(self):
"""Enable namespace combo box."""
self._widget.namespace_input.setEnabled(True)
def _handle_ns_changed(self):
"""If namespace selection is changed then reinitialize everything."""
ns = self._widget.namespace_input.currentText()
if len(ns) > 0:
if self._ns != ns:
self._actions_connected = False
self._ns = ns
self.enable_widgets(False)
rospy.loginfo("Creating action clients on namespace '%s'..." %
ns)
rospy.loginfo("Action clients created.")
self._actions_connected = True
self.enable_widgets(True)
self._containers = {}
self._top_containers = {}
self._selected_paths = []
self._structure_subs = {}
self._status_subs = {}
self._needs_zoom = True
self._structure_changed = True
self._max_depth = -1
self._show_all_transitions = False
self._graph_needs_refresh = True
self._tree_needs_refresh = True
#self._widget.namespace_input.clear()
self._widget.path_input.clear()
self._widget.ud_path_input.clear()
self._widget.tree.clear()
@Slot()
def refresh_combo_box(self):
"""Refresh namespace combo box."""
self._update_thread.kill()
self._containers = {}
self._top_containers = {}
self._selected_paths = []
self._structure_subs = {}
self._status_subs = {}
self._structure_changed = True
self._tree_needs_refresh = True
#self._graph_needs_refresh = True
#self._widget.namespace_input.clear()
self._widget.path_input.clear()
self._widget.ud_path_input.clear()
self._widget.tree.clear()
if self._widget.restrict_ns.isChecked():
self._widget.namespace_input.setEnabled(False)
self._widget.namespace_input.setEditText('updating...')
self._widget.ns_refresh_button.setEnabled(True)
self._update_thread.start()
else:
self._widget.namespace_input.setEnabled(False)
self._widget.namespace_input.setEditText('Unrestricted')
self._widget.ns_refresh_button.setEnabled(False)
self._graph_needs_refresh = True
self._tree_needs_refresh = True
self._widget.path_input.addItem('/')
def _handle_path_changed(self, checked):
"""If the path input is changed, update graph."""
self._path = self._widget.path_input.currentText()
self._graph_needs_refresh = True
self._structure_changed = True
if self._widget.zoom_checkbox.isChecked():
self._needs_zoom = True
def enable_widgets(self, enable):
"""Enable all widgets."""
self._widget.xdot_widget.setEnabled(enable)
self._widget.path_input.setEnabled(enable)
self._widget.depth_input.setEnabled(enable)
self._widget.label_width_input.setEnabled(enable)
self._widget.ud_path_input.setEnabled(enable)
self._widget.ud_text_browser.setEnabled(enable)
class MessageGUI(Plugin):
def reply_msg_callback(self, msg_in):
self._widget.reply.setText(msg_in.message)
def arithmetic_reply_msg_callback(self, msg_in):
display_text = 'Operation Type: '+msg_in.oper_type+'\n'+ \
'Answer: '+str(msg_in.answer)+'\n'+ \
'Time Received: '+str(msg_in.time_received)+'\n'+ \
'Time Answered: '+str(msg_in.time_answered)+'\n'+ \
'Process Time: '+str(msg_in.process_time)
self._widget.reply.setText(display_text)
def message_count_display(self, counter_val):
display_text = ''
if self.counter_req_id == 0:
display_text = display_text + 'Total messages:'
elif self.counter_req_id == 1:
display_text = display_text + 'Total replied messages:'
elif self.counter_req_id == 2:
display_text = display_text + 'Total sent messages:'
elif self.counter_req_id == 3:
display_text = display_text + 'Time since last replied message:'
elif self.counter_req_id == 4:
display_text = display_text + 'Time since last sent message:'
print(display_text)
self._widget.counter_reply.setText(display_text +
str(counter_val.reply))
def __init__(self, context):
super(MessageGUI, self).__init__(context)
self.setObjectName('MessageGUI')
self.message_pub = rospy.Publisher("sent_msg",
sent_msg,
queue_size=1000)
rospy.Subscriber("reply_msg", reply_msg, self.reply_msg_callback)
rospy.Subscriber("arithmetic_reply", arithmetic_reply,
self.arithmetic_reply_msg_callback)
self.msg_to_send = sent_msg()
self.counter_req_id = -1
self._widget = QWidget()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('message_ui'), 'resource',
'MessageGUI.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('MessageGUIui')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
context.add_widget(self._widget)
self._widget.message_to_send.textChanged.connect(
self._on_msg_to_send_changed)
self._widget.counter_val_to_get.textChanged.connect(
self._on_counter_val_to_get_changed)
self._widget.counter_val_to_get.setInputMask('9')
self._widget.send_message.pressed.connect(
self._on_send_message_pressed)
self._widget.send_request.pressed.connect(
self._on_send_request_pressed)
def _on_msg_to_send_changed(self, msg):
msg = str(msg)
self.msg_to_send.message = msg
def _on_send_message_pressed(self):
self.msg_to_send.header.stamp = rospy.Time.now()
self.message_pub.publish(self.msg_to_send)
def _on_counter_val_to_get_changed(self, id):
try:
self.counter_req_id = int(id)
except ValueError:
print('String input is not an integer')
def _on_send_request_pressed(self):
rospy.wait_for_service('message_counter')
try:
counter_serv = rospy.ServiceProxy('message_counter', counter)
response = counter_serv(self.counter_req_id)
self.message_count_display(response)
return response
except rospy.ServiceException, ex:
print "Service call to get message counter failed. %s" % e
class PlexilPlanSelectionGUI(Plugin):
#sets up our signal for the sub callback
monitor_signal = pyqtSignal(['QString'])
def __init__(self, context):
'''init initializes the widget and sets up our subscriber, publisher and event handlers'''
super(PlexilPlanSelectionGUI, self).__init__(context)
self.setObjectName('PlexilPlanSelectionGUI')
# Process standalone plugin command-line arguments
parser = ArgumentParser()
parser.add_argument("-q", "--quiet", action="store_true", dest="quiet", help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
# Find resources and Create QWidget
self._widget = QWidget()
ui_file = os.path.join(rospkg.RosPack().get_path('ow_plexil'), 'rqt_plexil_plan_selection', 'resource', 'plexil_plan_selection.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('PlexilPlanSelectionUI')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
context.add_widget(self._widget)
#set window icon
icon = os.path.join(rospkg.RosPack().get_path('ow_plexil'), 'rqt_plexil_plan_selection', 'resource', 'icon.png')
self._widget.setWindowIcon(QIcon(icon))
#pub and sub setup
self.status_publisher = rospy.Publisher('plexil_plan_selection_status', String, queue_size=20)
self.status_subscriber = rospy.Subscriber('plexil_plan_selection_status', String, self.status_callback)
#client placeholder
self.plan_select_client = None
#Qt signal to modify GUI from callback
self.monitor_signal[str].connect(self.monitor_status)
#populates the plan list, shows different plans based off of what launch file is running
if rospy.get_param('owlat_flag', False):
owlat_plan_dir = os.path.join(rospkg.RosPack().get_path('ow_plexil'), 'src', 'plans', 'owlat_plans')
self.populate_plan_list(owlat_plan_dir)
else:
ow_plan_dir = os.path.join(rospkg.RosPack().get_path('ow_plexil'), 'src', 'plans')
self.populate_plan_list(ow_plan_dir)
#sets up tables
self._widget.sentPlansTable.setSizeAdjustPolicy(QAbstractScrollArea.AdjustToContents)
self._widget.planQueueTable.setSizeAdjustPolicy(QAbstractScrollArea.AdjustToContents)
self._widget.sentPlansTable.insertColumn(0)
self._widget.sentPlansTable.insertColumn(1)
self._widget.planQueueTable.insertColumn(0)
#sets up event listeners
self._widget.addButton.clicked[bool].connect(self.handle_add_button_clicked)
self._widget.removeButton.clicked[bool].connect(self.handle_remove_button_clicked)
self._widget.upButton.clicked[bool].connect(self.handle_up_button_clicked)
self._widget.downButton.clicked[bool].connect(self.handle_down_button_clicked)
self._widget.sendButton.clicked[bool].connect(self.handle_send_button_clicked)
self._widget.resetButton.clicked[bool].connect(self.handle_reset_button_clicked)
def populate_plan_list(self, plan_dir):
'''Finds all .ple plexil plans in the plans directory and stores them in the widget list.'''
plan_list = []
#get all plans with extension .ple
for p in os.listdir(plan_dir):
if p.endswith(".ple"):
plan_list.append(p.rsplit(".")[0])
#add to list
self._widget.planList.addItems(plan_list)
self._widget.planList.sortItems()
def monitor_status(self, feedback):
'''Signal from callback calls this function to do the work to avoid threading issued with the GUI,
changes the status on the sent plans table to match the current plan status. Plan can be Pending,
Running, Finished or Failed.'''
num_rows = self._widget.sentPlansTable.rowCount()
#if completed and previously running we set status as finished
if feedback == "COMPLETE":
for i in range(num_rows):
current_status = self._widget.sentPlansTable.item(i,1).text()
if current_status == "Running...":
self._widget.sentPlansTable.item(i, 1).setText("Finished")
self._widget.sentPlansTable.item(i, 1).setBackground(QColor(0,128,0))
break
else:
#splitting into plan name and status
status = feedback.rsplit(":")[0]
running_plan = feedback.rsplit(":")[1].rsplit(".")[0]
#we set status to running or Failed depending on status
for i in range(num_rows):
plan_name = self._widget.sentPlansTable.item(i,0).text()
current_status = self._widget.sentPlansTable.item(i,1).text()
if plan_name == running_plan and current_status == "Pending...":
if status == "SUCCESS":
self._widget.sentPlansTable.item(i, 1).setText("Running...")
break
else:
self._widget.sentPlansTable.item(i, 1).setText("Failed")
self._widget.sentPlansTable.item(i, 1).setBackground(QColor(230,38,0))
break
return
def status_callback(self, msg):
'''Callback from status subscriber. Sends the msg to the function monitor_signal for further
processing in order to prevent threading issues in the GUI.'''
#have to use a signal here or else GUI wont update
feedback_string = str(msg.data)
self.monitor_signal.emit(feedback_string)
def handle_add_button_clicked(self, checked):
'''When the add button is clicked this function moves any selected items to the plan queue table.'''
#get selected items
selected_plans = self._widget.planList.selectedItems()
#create a new row in the queue table for each selection and insert it
for i in selected_plans:
row_position = self._widget.planQueueTable.rowCount()
self._widget.planQueueTable.insertRow(row_position)
self._widget.planQueueTable.setItem(row_position-1, 1, QTableWidgetItem(i.text()))
self._widget.planList.selectionModel().clear()
self._widget.planQueueTable.resizeColumnsToContents()
return
def handle_remove_button_clicked(self, checked):
'''When the remove button is clicked this function deletes any selected items from the plan queue table.'''
selected_rows = self._widget.planQueueTable.selectedItems()
for i in selected_rows:
self._widget.planQueueTable.removeRow(self._widget.planQueueTable.row(i))
self._widget.planQueueTable.selectionModel().clear()
return
def handle_up_button_clicked(self, checked):
'''When up button is clicked this function moves the selected item up in the queue table.'''
selected_row = self._widget.planQueueTable.currentRow()
#checks we are not at top of list
if selected_row <= 0:
return
#switches the two rows and puts selection on previously selected row
else:
belowTxt = self._widget.planQueueTable.item(selected_row,0).text()
aboveTxt = self._widget.planQueueTable.item(selected_row-1,0).text()
self._widget.planQueueTable.item(selected_row, 0).setText(aboveTxt)
self._widget.planQueueTable.item(selected_row-1, 0).setText(belowTxt)
self._widget.planQueueTable.selectionModel().clear()
self._widget.planQueueTable.setCurrentItem(self._widget.planQueueTable.item(selected_row-1,0))
return
def handle_down_button_clicked(self, checked):
'''When down button is clicked this function moves the selected item down in the queue table.'''
selected_row = self._widget.planQueueTable.currentRow()
num_rows = self._widget.planQueueTable.rowCount()
#checks we are not at bottom of list
if selected_row >= num_rows-1 or selected_row < 0:
return
#switches the two rows and puts selection on previously selected row
else:
belowTxt = self._widget.planQueueTable.item(selected_row+1,0).text()
aboveTxt = self._widget.planQueueTable.item(selected_row,0).text()
self._widget.planQueueTable.item(selected_row+1, 0).setText(aboveTxt)
self._widget.planQueueTable.item(selected_row, 0).setText(belowTxt)
self._widget.planQueueTable.selectionModel().clear()
self._widget.planQueueTable.setCurrentItem(self._widget.planQueueTable.item(selected_row+1,0))
return
def handle_send_button_clicked(self, checked):
'''When send button is clicked any items in the plan queue table are sent (in order) to the sent plans table.
It then publishes a string array of these plans so that the ow_plexil_plan_selection node can run them
sequentially. If the subscriber is not connected a popup box reminding the user to run the ow_plexil node will show up.'''
if self.check_client_set_up() == False:
return
num_rows = self._widget.planQueueTable.rowCount()
plans_sent = []
#creates a new row in the sent table for each item in the queue table and inserts it
for i in range(num_rows):
plan_name = self._widget.planQueueTable.item(0,0).text()
plans_sent.append(str(plan_name + ".plx"))
self._widget.planQueueTable.removeRow(0)
row_position = self._widget.sentPlansTable.rowCount()
self._widget.sentPlansTable.insertRow(row_position)
self._widget.sentPlansTable.setItem(row_position, 0, QTableWidgetItem(plan_name))
self._widget.sentPlansTable.setItem(row_position, 1, QTableWidgetItem("Pending..."))
self._widget.sentPlansTable.resizeColumnsToContents()
# Create msg and send to subscriber for plan execution
self.plan_select_client("ADD", plans_sent)
return
def handle_reset_button_clicked(self, checked):
'''When reset button is clicked all plans in the sent plans table are deleted. It also publishes a string command
RESET letting the ow_plexil_plan_selection node know that any plans in its queue should also be deleted.'''
if self.check_client_set_up() == False:
return
num_rows = self._widget.sentPlansTable.rowCount()
#deletes each row pressent in sentplanstable
for i in range(num_rows):
self._widget.sentPlansTable.removeRow(0)
self.plan_select_client("RESET", [])
return
def check_client_set_up(self):
'''Checks to see if the client is initialized, if not we check if the server is running before initializing the client.
If server is not yet running we send a popup informing the user and return False.'''
#checks to see if we have connected to service yet
if self.plan_select_client == None:
#we get list of services and see if any match plexil_plan_selection
services = rosservice.get_service_list()
service_running = [i for i in services if "plexil_plan_selection" in i]
#if none exist we send a popup and return
if len(service_running) == 0:
popup = QMessageBox()
popup.setWindowTitle("ow_plexil service not yet connected")
popup.setText("ow_plexil plan selection service not connected yet, please make sure the ow_plexil launch file is running.")
popup.exec_()
return False
else:
#client setup
rospy.wait_for_service('plexil_plan_selection')
self.plan_select_client = rospy.ServiceProxy('plexil_plan_selection', PlanSelection)
return True
else:
return True
def shutdown_plugin(self):
'''handles shutdown procedures for the plugin, unregisters the publisher and subscriber.'''
self.status_subscriber.unregister()
pass
class JointTrajectoryController(Plugin):
"""
Graphical frontend for a C{JointTrajectoryController}.
There are two modes for interacting with a controller:
1. B{Monitor mode} Joint displays are updated with the state reported
by the controller. This is a read-only mode and does I{not} send
control commands. Every time a new controller is selected, it starts
in monitor mode for safety reasons.
2. B{Control mode} Joint displays update the control command that is
sent to the controller. Commands are sent periodically evan if the
displays are not being updated by the user.
To control the aggressiveness of the motions, the maximum speed of the
sent commands can be scaled down using the C{Speed scaling control}
This plugin is able to detect and keep track of all active controller
managers, as well as the JointTrajectoryControllers that are I{running}
in each controller manager.
For a controller to be compatible with this plugin, it must comply with
the following requisites:
- The controller type contains the C{JointTrajectoryController}
substring, e.g., C{position_controllers/JointTrajectoryController}
- The controller exposes the C{command} and C{state} topics in its
ROS interface.
Additionally, there must be a URDF loaded with a valid joint limit
specification, namely position (when applicable) and velocity limits.
A reference implementation of the C{JointTrajectoryController} is
available in the C{joint_trajectory_controller} ROS package.
"""
_cmd_pub_freq = 10.0 # Hz
_widget_update_freq = 30.0 # Hz
_ctrlrs_update_freq = 1 # Hz
_min_traj_dur = 5.0 / _cmd_pub_freq # Minimum trajectory duration
jointStateChanged = Signal([dict])
def __init__(self, context):
super(JointTrajectoryController, self).__init__(context)
self.setObjectName('JointTrajectoryController')
# Create QWidget and extend it with all the attributes and children
# from the UI file
self._widget = QWidget()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_joint_trajectory_controller'),
'resource',
'joint_trajectory_controller.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('JointTrajectoryControllerUi')
# Setup speed scaler
speed_scaling = DoubleEditor(1.0, 100.0)
speed_scaling.spin_box.setSuffix('%')
speed_scaling.spin_box.setValue(50.0)
speed_scaling.spin_box.setDecimals(0)
speed_scaling.setEnabled(False)
self._widget.speed_scaling_layout.addWidget(speed_scaling)
self._speed_scaling_widget = speed_scaling
speed_scaling.valueChanged.connect(self._on_speed_scaling_change)
self._on_speed_scaling_change(speed_scaling.value())
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). 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._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
# Initialize members
self._jtc_name = [] # Name of selected joint trajectory controller
self._cm_ns = [] # Namespace of the selected controller manager
self._joint_pos = {} # name->pos map for joints of selected controller
self._joint_names = [] # Ordered list of selected controller joints
self._robot_joint_limits = {} # Lazily evaluated on first use
# Timer for sending commands to active controller
self._update_cmd_timer = QTimer(self)
self._update_cmd_timer.setInterval(1000.0 / self._cmd_pub_freq)
self._update_cmd_timer.timeout.connect(self._update_cmd_cb)
# Timer for updating the joint widgets from the controller state
self._update_act_pos_timer = QTimer(self)
self._update_act_pos_timer.setInterval(1000.0 /
self._widget_update_freq)
self._update_act_pos_timer.timeout.connect(self._update_joint_widgets)
# Timer for controller manager updates
self._list_cm = ControllerManagerLister()
self._update_cm_list_timer = QTimer(self)
self._update_cm_list_timer.setInterval(1000.0 /
self._ctrlrs_update_freq)
self._update_cm_list_timer.timeout.connect(self._update_cm_list)
self._update_cm_list_timer.start()
# Timer for running controller updates
self._update_jtc_list_timer = QTimer(self)
self._update_jtc_list_timer.setInterval(1000.0 /
self._ctrlrs_update_freq)
self._update_jtc_list_timer.timeout.connect(self._update_jtc_list)
self._update_jtc_list_timer.start()
# Signal connections
w = self._widget
w.enable_button.toggled.connect(self._on_jtc_enabled)
w.jtc_combo.currentIndexChanged[str].connect(self._on_jtc_change)
w.cm_combo.currentIndexChanged[str].connect(self._on_cm_change)
self._cmd_pub = None # Controller command publisher
self._state_sub = None # Controller state subscriber
self._list_controllers = None
def shutdown_plugin(self):
self._update_cmd_timer.stop()
self._update_act_pos_timer.stop()
self._update_cm_list_timer.stop()
self._update_jtc_list_timer.stop()
self._unregister_state_sub()
self._unregister_cmd_pub()
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('cm_ns', self._cm_ns)
instance_settings.set_value('jtc_name', self._jtc_name)
def restore_settings(self, plugin_settings, instance_settings):
# Restore last session's controller_manager, if present
self._update_cm_list()
cm_ns = instance_settings.value('cm_ns')
cm_combo = self._widget.cm_combo
cm_list = [cm_combo.itemText(i) for i in range(cm_combo.count())]
try:
idx = cm_list.index(cm_ns)
cm_combo.setCurrentIndex(idx)
# Resore last session's controller, if running
self._update_jtc_list()
jtc_name = instance_settings.value('jtc_name')
jtc_combo = self._widget.jtc_combo
jtc_list = [jtc_combo.itemText(i) for i in range(jtc_combo.count())]
try:
idx = jtc_list.index(jtc_name)
jtc_combo.setCurrentIndex(idx)
except (ValueError):
pass
except (ValueError):
pass
# def trigger_configuration(self):
# Comment in to signal that the plugin has a way to configure
# This will enable a setting button (gear icon) in each dock widget
# title bar
# Usually used to open a modal configuration dialog
def _update_cm_list(self):
update_combo(self._widget.cm_combo, self._list_cm())
def _update_jtc_list(self):
# Clear controller list if no controller information is available
if not self._list_controllers:
self._widget.jtc_combo.clear()
return
# List of running controllers with a valid joint limits specification
# for _all_ their joints
running_jtc = self._running_jtc_info()
if running_jtc and not self._robot_joint_limits:
self._robot_joint_limits = get_joint_limits() # Lazy evaluation
valid_jtc = []
for jtc_info in running_jtc:
has_limits = all(name in self._robot_joint_limits
for name in _jtc_joint_names(jtc_info))
if has_limits:
valid_jtc.append(jtc_info);
valid_jtc_names = [data.name for data in valid_jtc]
# Update widget
update_combo(self._widget.jtc_combo, sorted(valid_jtc_names))
def _on_speed_scaling_change(self, val):
self._speed_scale = val / self._speed_scaling_widget.slider.maximum()
def _on_joint_state_change(self, actual_pos):
assert(len(actual_pos) == len(self._joint_pos))
for name in actual_pos.keys():
self._joint_pos[name]['position'] = actual_pos[name]
def _on_cm_change(self, cm_ns):
self._cm_ns = cm_ns
if cm_ns:
self._list_controllers = ControllerLister(cm_ns)
# NOTE: Clear below is important, as different controller managers
# might have controllers with the same name but different
# configurations. Clearing forces controller re-discovery
self._widget.jtc_combo.clear()
self._update_jtc_list()
else:
self._list_controllers = None
def _on_jtc_change(self, jtc_name):
self._unload_jtc()
self._jtc_name = jtc_name
if self._jtc_name:
self._load_jtc()
def _on_jtc_enabled(self, val):
# Don't allow enabling if there are no controllers selected
if not self._jtc_name:
self._widget.enable_button.setChecked(False)
return
# Enable/disable joint displays
for joint_widget in self._joint_widgets():
joint_widget.setEnabled(val)
# Enable/disable speed scaling
self._speed_scaling_widget.setEnabled(val)
if val:
# Widgets send desired position commands to controller
self._update_act_pos_timer.stop()
self._update_cmd_timer.start()
else:
# Controller updates widgets with actual position
self._update_cmd_timer.stop()
self._update_act_pos_timer.start()
def _load_jtc(self):
# Initialize joint data corresponding to selected controller
running_jtc = self._running_jtc_info()
self._joint_names = next(_jtc_joint_names(x) for x in running_jtc
if x.name == self._jtc_name)
for name in self._joint_names:
self._joint_pos[name] = {}
# Update joint display
try:
layout = self._widget.joint_group.layout()
for name in self._joint_names:
limits = self._robot_joint_limits[name]
joint_widget = DoubleEditor(limits['min_position'],
limits['max_position'])
layout.addRow(name, joint_widget)
# NOTE: Using partial instead of a lambda because lambdas
# "will not evaluate/look up the argument values before it is
# effectively called, breaking situations like using a loop
# variable inside it"
from functools import partial
par = partial(self._update_single_cmd_cb, name=name)
joint_widget.valueChanged.connect(par)
except:
# TODO: Can we do better than swallow the exception?
from sys import exc_info
print 'Unexpected error:', exc_info()[0]
# Enter monitor mode (sending commands disabled)
self._on_jtc_enabled(False)
# Setup ROS interfaces
jtc_ns = _resolve_controller_ns(self._cm_ns, self._jtc_name)
state_topic = jtc_ns + '/state'
cmd_topic = jtc_ns + '/command'
self._state_sub = rospy.Subscriber(state_topic,
JointTrajectoryControllerState,
self._state_cb,
queue_size=1)
self._cmd_pub = rospy.Publisher(cmd_topic,
JointTrajectory,
queue_size=1)
# Start updating the joint positions
self.jointStateChanged.connect(self._on_joint_state_change)
def _unload_jtc(self):
# Stop updating the joint positions
try:
self.jointStateChanged.disconnect(self._on_joint_state_change)
except:
pass
# Reset ROS interfaces
self._unregister_state_sub()
self._unregister_cmd_pub()
# Clear joint widgets
# NOTE: Implementation is a workaround for:
# https://bugreports.qt-project.org/browse/QTBUG-15990 :(
layout = self._widget.joint_group.layout()
if layout is not None:
while layout.count():
layout.takeAt(0).widget().deleteLater()
# Delete existing layout by reparenting to temporary
QWidget().setLayout(layout)
self._widget.joint_group.setLayout(QFormLayout())
# Reset joint data
self._joint_names = []
self._joint_pos = {}
# Enforce monitor mode (sending commands disabled)
self._widget.enable_button.setChecked(False)
def _running_jtc_info(self):
from controller_manager_msgs.utils\
import filter_by_type, filter_by_state
controller_list = self._list_controllers()
jtc_list = filter_by_type(controller_list,
'JointTrajectoryController',
match_substring=True)
running_jtc_list = filter_by_state(jtc_list, 'running')
return running_jtc_list
def _unregister_cmd_pub(self):
if self._cmd_pub is not None:
self._cmd_pub.unregister()
self._cmd_pub = None
def _unregister_state_sub(self):
if self._state_sub is not None:
self._state_sub.unregister()
self._state_sub = None
def _state_cb(self, msg):
actual_pos = {}
for i in range(len(msg.joint_names)):
joint_name = msg.joint_names[i]
joint_pos = msg.actual.positions[i]
actual_pos[joint_name] = joint_pos
self.jointStateChanged.emit(actual_pos)
def _update_single_cmd_cb(self, val, name):
self._joint_pos[name]['command'] = val
def _update_cmd_cb(self):
dur = []
traj = JointTrajectory()
traj.joint_names = self._joint_names
point = JointTrajectoryPoint()
for name in traj.joint_names:
pos = self._joint_pos[name]['position']
cmd = pos
try:
cmd = self._joint_pos[name]['command']
except (KeyError):
pass
max_vel = self._robot_joint_limits[name]['max_velocity']
dur.append(max(abs(cmd - pos) / max_vel, self._min_traj_dur))
point.positions.append(cmd)
point.time_from_start = rospy.Duration(max(dur) / self._speed_scale)
traj.points.append(point)
self._cmd_pub.publish(traj)
def _update_joint_widgets(self):
joint_widgets = self._joint_widgets()
for id in range(len(joint_widgets)):
joint_name = self._joint_names[id]
try:
joint_pos = self._joint_pos[joint_name]['position']
joint_widgets[id].setValue(joint_pos)
except (KeyError):
pass # Can happen when first connected to controller
def _joint_widgets(self): # TODO: Cache instead of compute every time?
widgets = []
layout = self._widget.joint_group.layout()
for row_id in range(layout.rowCount()):
widgets.append(layout.itemAt(row_id,
QFormLayout.FieldRole).widget())
return widgets
class MavManagerUi(Plugin):
def __init__(self, context):
super(MavManagerUi, self).__init__(context)
self.setObjectName('MavManagerUi')
self._publisher = None
self.robot_name = 'quadrotor'
self.mav_node_name = 'mav_services'
self._widget = QWidget()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_mav_manager'), 'resource',
'MavManager.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('MavManagerWidget')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
context.add_widget(self._widget)
self._widget.robot_name_line_edit.textChanged.connect(
self._on_robot_name_changed)
self._widget.node_name_line_edit.textChanged.connect(
self._on_node_name_changed)
self._widget.motors_on_push_button.pressed.connect(
self._on_motors_on_pressed)
self._widget.motors_off_push_button.pressed.connect(
self._on_motors_off_pressed)
self._widget.hover_push_button.pressed.connect(self._on_hover_pressed)
self._widget.ehover_push_button.pressed.connect(
self._on_ehover_pressed)
self._widget.land_push_button.pressed.connect(self._on_land_pressed)
self._widget.eland_push_button.pressed.connect(self._on_eland_pressed)
self._widget.estop_push_button.pressed.connect(self._on_estop_pressed)
self._widget.goto_push_button.pressed.connect(self._on_goto_pressed)
self._widget.takeoff_push_button.pressed.connect(
self._on_takeoff_pressed)
self._widget.gohome_push_button.pressed.connect(
self._on_gohome_pressed)
def _on_robot_name_changed(self, robot_name):
self.robot_name = str(robot_name)
def _on_node_name_changed(self, node_name):
self.mav_node_name = str(node_name)
def _on_motors_on_pressed(self):
try:
motors_topic = '/' + self.robot_name + '/' + self.mav_node_name + '/motors'
rospy.wait_for_service(motors_topic, timeout=1.0)
motors_on = rospy.ServiceProxy(motors_topic, std_srvs.srv.SetBool)
resp = motors_on(True)
print('Motors on ', resp.success)
except rospy.ServiceException as e:
print("Service call failed: %s" % e)
except (rospy.ROSException) as e:
print("Service call failed: %s" % e)
def _on_motors_off_pressed(self):
try:
motors_topic = '/' + self.robot_name + '/' + self.mav_node_name + '/motors'
rospy.wait_for_service(motors_topic, timeout=1.0)
motors_off = rospy.ServiceProxy(motors_topic, std_srvs.srv.SetBool)
resp = motors_off(False)
print(resp.success)
except rospy.ServiceException as e:
print("Service call failed: %s" % e)
except (rospy.ROSException) as e:
print("Service call failed: %s" % e)
def _on_hover_pressed(self):
try:
hover_topic = '/' + self.robot_name + '/' + self.mav_node_name + '/hover'
rospy.wait_for_service(hover_topic, timeout=1.0)
hover = rospy.ServiceProxy(hover_topic, std_srvs.srv.Trigger)
resp = hover()
print('Hover ', resp.success)
except rospy.ServiceException as e:
print("Service call failed: %s" % e)
except (rospy.ROSException) as e:
print("Service call failed: %s" % e)
def _on_ehover_pressed(self):
try:
ehover_topic = '/' + self.robot_name + '/' + self.mav_node_name + '/ehover'
rospy.wait_for_service(ehover_topic, timeout=1.0)
ehover = rospy.ServiceProxy(ehover_topic, std_srvs.srv.Trigger)
resp = ehover()
print(resp.success)
except rospy.ServiceException as e:
print("Service call failed: %s" % e)
except (rospy.ROSException) as e:
print("Service call failed: %s" % e)
def _on_land_pressed(self):
try:
land_topic = '/' + self.robot_name + '/' + self.mav_node_name + '/land'
rospy.wait_for_service(land_topic, timeout=1.0)
land = rospy.ServiceProxy(land_topic, std_srvs.srv.Trigger)
resp = land()
print(resp.success)
except rospy.ServiceException as e:
print("Service call failed: %s" % e)
except (rospy.ROSException) as e:
print("Service call failed: %s" % e)
def _on_eland_pressed(self):
try:
eland_topic = '/' + self.robot_name + '/' + self.mav_node_name + '/eland'
rospy.wait_for_service(eland_topic, timeout=1.0)
eland = rospy.ServiceProxy(eland_topic, std_srvs.srv.Trigger)
resp = eland()
print(resp.success)
except rospy.ServiceException as e:
print("Service call failed: %s" % e)
except (rospy.ROSException) as e:
print("Service call failed: %s" % e)
def _on_estop_pressed(self):
try:
estop_topic = '/' + self.robot_name + '/' + self.mav_node_name + '/estop'
rospy.wait_for_service(estop_topic, timeout=1.0)
estop = rospy.ServiceProxy(estop_topic, std_srvs.srv.Trigger)
resp = estop()
print(resp.success)
except rospy.ServiceException as e:
print("Service call failed: %s" % e)
except (rospy.ROSException) as e:
print("Service call failed: %s" % e)
def _on_takeoff_pressed(self):
try:
takeoff_topic = '/' + self.robot_name + '/' + self.mav_node_name + '/takeoff'
rospy.wait_for_service(takeoff_topic, timeout=1.0)
takeoff = rospy.ServiceProxy(takeoff_topic, std_srvs.srv.Trigger)
resp = takeoff()
print(resp.success)
except rospy.ServiceException as e:
print("Service call failed: %s" % e)
except (rospy.ROSException) as e:
print("Service call failed: %s" % e)
def _on_gohome_pressed(self):
try:
gohome_topic = '/' + self.robot_name + '/' + self.mav_node_name + '/goHome'
rospy.wait_for_service(gohome_topic, timeout=1.0)
gohome = rospy.ServiceProxy(gohome_topic, std_srvs.srv.Trigger)
resp = gohome()
print(resp.success)
except rospy.ServiceException as e:
print("Service call failed: %s" % e)
except (rospy.ROSException) as e:
print("Service call failed: %s" % e)
def _on_goto_pressed(self):
req = kr_mav_manager.srv.Vec4Request()
req.goal[0] = self._widget.x_doubleSpinBox.value()
req.goal[1] = self._widget.y_doubleSpinBox.value()
req.goal[2] = self._widget.z_doubleSpinBox.value()
req.goal[3] = self._widget.yaw_doubleSpinBox.value()
print(req.goal)
if (self._widget.relative_checkbox.isChecked()):
try:
goto = rospy.ServiceProxy(
'/' + self.robot_name + '/' + self.mav_node_name +
'/goToRelative', kr_mav_manager.srv.Vec4)
resp = goto(req)
print(resp.success)
except rospy.ServiceException as e:
print("Service call failed: %s" % e)
else:
try:
goto_relative = rospy.ServiceProxy(
'/' + self.robot_name + '/' + self.mav_node_name + '/goTo',
kr_mav_manager.srv.Vec4)
resp = goto_relative(req)
print(resp.success)
except rospy.ServiceException as e:
print("Service call failed: %s" % e)
def _unregister_publisher(self):
if self._publisher is not None:
self._publisher.unregister()
self._publisher = None
def shutdown_plugin(self):
self._unregister_publisher()
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('robot_name',
self._widget.robot_name_line_edit.text())
instance_settings.set_value('node_name',
self._widget.node_name_line_edit.text())
def restore_settings(self, plugin_settings, instance_settings):
#Override saved value with param value if set
value = instance_settings.value('robot_name', "quadrotor")
param_value = rospy.get_param("robot_name", value)
self.robot_name = param_value
self._widget.robot_name_line_edit.setText(param_value)
value = instance_settings.value('node_name', "mav_services")
self.node_name = value
self._widget.node_name_line_edit.setText(value)
class MyPlugin(Plugin):
sig_sysmsg = Signal(str)
l_location = pyqtSignal(float, float)
r_location = pyqtSignal(float, float)
def __init__(self, context):
super(MyPlugin, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('MyPlugin')
rp = rospkg.RosPack()
# 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
## def goCoords():
## def resetError():
## # coordsEdit.setStyleSheet('')
##
## try:
## print("work")
## #latitude, longitude = coordsEdit.text().split(",")
##
## except ValueError:
## #coordsEdit.setStyleSheet("color: red;")
## QTimer.singleShot(500, resetError)
## else:
## map.centerAt(latitude, longitude)
## # map.moveMarker("MyDragableMark", latitude, longitude)
def onMarkerMoved(key, latitude, longitude):
print("Moved!!", key, latitude, longitude)
#coordsEdit.setText("{}, {}".format(latitude, longitude))
#self.map.moveMarker("local GPS",latitude+0.01, longitude+0.01)
def onMarkerRClick(key):
print("RClick on ", key)
# map.setMarkerOptions(key, draggable=False)
def onMarkerLClick(key):
print("LClick on ", key)
def onMarkerDClick(key):
print("DClick on ", key)
# map.setMarkerOptions(key, draggable=True)
def onMapMoved(latitude, longitude):
print("Moved to ", latitude, longitude)
def onMapRClick(latitude, longitude):
print("RClick on ", latitude, longitude)
def onMapLClick(latitude, longitude):
print("LClick on ", latitude, longitude)
def onMapDClick(latitude, longitude):
print("DClick on ", latitude, longitude)
def move_l_mark(lat_l, lon_l):
#onMarkerMoved("local gps",lat_,lon_)
#print("get location",lat_,lon_)
self.map.moveMarker("local GPS", lat_l, lon_l)
l_coordsEdit.setText("{0:.8}, {1:.8}".format(lat_l, lon_l))
def local_callback(llocation):
llat = llocation.data[0]
llon = llocation.data[1]
#move_mark(self,lat,lon)
self.l_location.emit(llat, llon)
def move_r_mark(lat_r, lon_r):
#onMarkerMoved("local gps",lat_,lon_)
#print("get location",lat_,lon_)
self.map.moveMarker("remote GPS", lat_r, lon_r)
r_coordsEdit.setText("{0:.8}, {1:.8}".format(lat_r, lon_r))
def remote_callback(rlocation):
rlat = rlocation.data[0]
rlon = rlocation.data[1]
#move_mark(self,lat,lon)
self.r_location.emit(rlat, rlon)
# Create QWidget
self._widget = QWidget()
h = QVBoxLayout(self._widget)
#h = QVBoxLayout(w)
v = QHBoxLayout()
# l = QFormLayout()
# v.addLayout(l)
l_coordsEdit = QLineEdit()
r_coordsEdit = QLineEdit()
lgps_label = QLabel('LGPS:')
rgps_label = QLabel('RGPS:')
v.addWidget(lgps_label)
v.addWidget(l_coordsEdit)
v.addWidget(rgps_label)
v.addWidget(r_coordsEdit)
h.addLayout(v)
# l.addRow('Coords:', coordsEdit)
# l.addRow('lcoords:',r_coordsEdit)
#h.addLayout(v)
#l = QFormLayout()
#h.addLayout(l)
#coordsEdit = QLineEdit()
#l.addRow('Coords:', coordsEdit)
#coordsEdit.editingFinished.connect(goCoords)
self.map = QOSM(self._widget)
self.map.mapMoved.connect(onMapMoved)
#self.map.markerMoved.connect(onMarkerMoved)
self.map.mapClicked.connect(onMapLClick)
self.map.mapDoubleClicked.connect(onMapDClick)
self.map.mapRightClicked.connect(onMapRClick)
self.map.markerClicked.connect(onMarkerLClick)
self.map.markerDoubleClicked.connect(onMarkerDClick)
self.map.markerRightClicked.connect(onMarkerRClick)
self.l_location.connect(move_l_mark)
self.r_location.connect(move_r_mark)
h.addWidget(self.map)
self.map.setSizePolicy(QSizePolicy.MinimumExpanding,
QSizePolicy.MinimumExpanding)
# Get path to UI file which is a sibling of this file
# in this example the .ui and .py file are in the same folder
ui_file = os.path.join(rp.get_path('osm_maps_node'), 'resource',
'MyPlugin.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('MyPluginUi')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). 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._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
self.map.waitUntilReady()
self.map.centerAt(32.7471012, -96.883642)
self.map.setZoom(12)
# Many icons at: https://sites.google.com/site/gmapsdevelopment/
coords = self.map.center()
self.map.addMarker(
"local GPS", *coords,
**dict(
icon=
"http://maps.gstatic.com/mapfiles/ridefinder-images/mm_20_gray.png",
draggable=True,
title="locat GPS marker!"))
coords = coords[0] + 0.1, coords[1] + 0.1
self.map.addMarker(
"remote GPS", *coords,
**dict(
icon=
"http://maps.gstatic.com/mapfiles/ridefinder-images/mm_20_red.png",
draggable=True,
title="remote GPS marker"))
sub1 = rospy.Subscriber("/local_gps", Float64MultiArray,
local_callback)
sub2 = rospy.Subscriber("/remote_gps", Float64MultiArray,
remote_callback)
#time.sleep(10)
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
class RosPackGraph(Plugin):
_deferred_fit_in_view = Signal()
def __init__(self, context):
super(RosPackGraph, self).__init__(context)
self.initialized = False
self._current_dotcode = None
self._update_thread = WorkerThread(self._update_thread_run, self._update_finished)
self._nodes = {}
self._edges = {}
self._options = {}
self._options_serialized = ''
self.setObjectName('RosPackGraph')
rospack = rospkg.RosPack()
rosstack = rospkg.RosStack()
# factory builds generic dotcode items
self.dotcode_factory = PydotFactory()
# self.dotcode_factory = PygraphvizFactory()
# generator builds rosgraph
self.dotcode_generator = RosPackageGraphDotcodeGenerator(rospack, rosstack)
# dot_to_qt transforms into Qt elements using dot layout
self.dot_to_qt = DotToQtGenerator()
self._widget = QWidget()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_dep'), 'resource', 'RosPackGraph.ui')
loadUi(ui_file, self._widget, {'InteractiveGraphicsView': InteractiveGraphicsView})
self._widget.setObjectName('RosPackGraphUi')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
self._scene = QGraphicsScene()
self._scene.setBackgroundBrush(Qt.white)
self._widget.graphics_view.setScene(self._scene)
self._widget.depth_combo_box.insertItem(0, self.tr('infinite'), -1)
self._widget.depth_combo_box.insertItem(1, self.tr('1'), 2)
self._widget.depth_combo_box.insertItem(2, self.tr('2'), 3)
self._widget.depth_combo_box.insertItem(3, self.tr('3'), 4)
self._widget.depth_combo_box.insertItem(4, self.tr('4'), 5)
self._widget.depth_combo_box.currentIndexChanged.connect(self._refresh_rospackgraph)
self._widget.directions_combo_box.insertItem(0, self.tr('depends'), 0)
self._widget.directions_combo_box.insertItem(1, self.tr('depends_on'), 1)
self._widget.directions_combo_box.insertItem(2, self.tr('both'), 2)
self._widget.directions_combo_box.currentIndexChanged.connect(self._refresh_rospackgraph)
self._widget.package_type_combo_box.insertItem(0, self.tr('wet & dry'), 3)
self._widget.package_type_combo_box.insertItem(1, self.tr('wet only'), 2)
self._widget.package_type_combo_box.insertItem(2, self.tr('dry only'), 1)
self._widget.package_type_combo_box.currentIndexChanged.connect(self._refresh_rospackgraph)
completionmodel = StackageCompletionModel(self._widget.filter_line_edit, rospack, rosstack)
completer = RepeatedWordCompleter(completionmodel, self)
completer.setCompletionMode(QCompleter.PopupCompletion)
completer.setWrapAround(True)
completer.setCaseSensitivity(Qt.CaseInsensitive)
self._widget.filter_line_edit.editingFinished.connect(self._refresh_rospackgraph)
self._widget.filter_line_edit.setCompleter(completer)
self._widget.filter_line_edit.selectionChanged.connect(self._clear_filter)
self._widget.with_stacks_check_box.clicked.connect(self._refresh_rospackgraph)
self._widget.mark_check_box.clicked.connect(self._refresh_rospackgraph)
self._widget.colorize_check_box.clicked.connect(self._refresh_rospackgraph)
self._widget.hide_transitives_check_box.clicked.connect(self._refresh_rospackgraph)
self._widget.refresh_graph_push_button.setIcon(QIcon.fromTheme('view-refresh'))
self._widget.refresh_graph_push_button.pressed.connect(self._update_rospackgraph)
self._widget.highlight_connections_check_box.toggled.connect(self._refresh_rospackgraph)
self._widget.auto_fit_graph_check_box.toggled.connect(self._refresh_rospackgraph)
self._widget.fit_in_view_push_button.setIcon(QIcon.fromTheme('zoom-original'))
self._widget.fit_in_view_push_button.pressed.connect(self._fit_in_view)
self._widget.load_dot_push_button.setIcon(QIcon.fromTheme('document-open'))
self._widget.load_dot_push_button.pressed.connect(self._load_dot)
self._widget.save_dot_push_button.setIcon(QIcon.fromTheme('document-save-as'))
self._widget.save_dot_push_button.pressed.connect(self._save_dot)
self._widget.save_as_svg_push_button.setIcon(QIcon.fromTheme('document-save-as'))
self._widget.save_as_svg_push_button.pressed.connect(self._save_svg)
self._widget.save_as_image_push_button.setIcon(QIcon.fromTheme('image'))
self._widget.save_as_image_push_button.pressed.connect(self._save_image)
self._deferred_fit_in_view.connect(self._fit_in_view, Qt.QueuedConnection)
self._deferred_fit_in_view.emit()
context.add_widget(self._widget)
# If in either of following case, this turnes True
# - 1st filtering key is already input by user
# - filtering key is restored
self._filtering_started = False
def shutdown_plugin(self):
self._update_thread.kill()
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('depth_combo_box_index', self._widget.depth_combo_box.currentIndex())
instance_settings.set_value('directions_combo_box_index', self._widget.directions_combo_box.currentIndex())
instance_settings.set_value('package_type_combo_box', self._widget.package_type_combo_box.currentIndex())
instance_settings.set_value('filter_line_edit_text', self._widget.filter_line_edit.text())
instance_settings.set_value('with_stacks_state', self._widget.with_stacks_check_box.isChecked())
instance_settings.set_value('hide_transitives_state', self._widget.hide_transitives_check_box.isChecked())
instance_settings.set_value('mark_state', self._widget.mark_check_box.isChecked())
instance_settings.set_value('colorize_state', self._widget.colorize_check_box.isChecked())
instance_settings.set_value('auto_fit_graph_check_box_state', self._widget.auto_fit_graph_check_box.isChecked())
instance_settings.set_value('highlight_connections_check_box_state', self._widget.highlight_connections_check_box.isChecked())
def restore_settings(self, plugin_settings, instance_settings):
_str_filter = instance_settings.value('filter_line_edit_text', '')
if (_str_filter == None or _str_filter == '') and \
not self._filtering_started:
_str_filter = '(Separate pkgs by comma)'
else:
self._filtering_started = True
self._widget.depth_combo_box.setCurrentIndex(int(instance_settings.value('depth_combo_box_index', 0)))
self._widget.directions_combo_box.setCurrentIndex(int(instance_settings.value('directions_combo_box_index', 0)))
self._widget.package_type_combo_box.setCurrentIndex(int(instance_settings.value('package_type_combo_box', 0)))
self._widget.filter_line_edit.setText(_str_filter)
self._widget.with_stacks_check_box.setChecked(instance_settings.value('with_stacks_state', True) in [True, 'true'])
self._widget.mark_check_box.setChecked(instance_settings.value('mark_state', True) in [True, 'true'])
self._widget.colorize_check_box.setChecked(instance_settings.value('colorize_state', False) in [True, 'true'])
self._widget.hide_transitives_check_box.setChecked(instance_settings.value('hide_transitives_state', False) in [True, 'true'])
self._widget.auto_fit_graph_check_box.setChecked(instance_settings.value('auto_fit_graph_check_box_state', True) in [True, 'true'])
self._widget.highlight_connections_check_box.setChecked(instance_settings.value('highlight_connections_check_box_state', True) in [True, 'true'])
self.initialized = True
self._refresh_rospackgraph()
def _update_rospackgraph(self):
# re-enable controls customizing fetched ROS graph
self._widget.depth_combo_box.setEnabled(True)
self._widget.directions_combo_box.setEnabled(True)
self._widget.package_type_combo_box.setEnabled(True)
self._widget.filter_line_edit.setEnabled(True)
self._widget.with_stacks_check_box.setEnabled(True)
self._widget.mark_check_box.setEnabled(True)
self._widget.colorize_check_box.setEnabled(True)
self._widget.hide_transitives_check_box.setEnabled(True)
self._refresh_rospackgraph(force_update=True)
def _update_options(self):
self._options['depth'] = self._widget.depth_combo_box.itemData(self._widget.depth_combo_box.currentIndex())
self._options['directions'] = self._widget.directions_combo_box.itemData(self._widget.directions_combo_box.currentIndex())
self._options['package_types'] = self._widget.package_type_combo_box.itemData(self._widget.package_type_combo_box.currentIndex())
self._options['with_stacks'] = self._widget.with_stacks_check_box.isChecked()
self._options['mark_selected'] = self._widget.mark_check_box.isChecked()
self._options['hide_transitives'] = self._widget.hide_transitives_check_box.isChecked()
# TODO: Allow different color themes
self._options['colortheme'] = True if self._widget.colorize_check_box.isChecked() else None
self._options['names'] = self._widget.filter_line_edit.text().split(',')
if self._options['names'] == [u'None']:
self._options['names'] = []
self._options['highlight_level'] = 3 if self._widget.highlight_connections_check_box.isChecked() else 1
self._options['auto_fit'] = self._widget.auto_fit_graph_check_box.isChecked()
def _refresh_rospackgraph(self, force_update=False):
if not self.initialized:
return
self._update_thread.kill()
self._update_options()
# avoid update if options did not change and force_update is not set
new_options_serialized = pickle.dumps(self._options)
if new_options_serialized == self._options_serialized and not force_update:
return
self._options_serialized = pickle.dumps(self._options)
self._scene.setBackgroundBrush(Qt.lightGray)
self._update_thread.start()
# this runs in a non-gui thread, so don't access widgets here directly
def _update_thread_run(self):
self._update_graph(self._generate_dotcode())
@Slot()
def _update_finished(self):
self._scene.setBackgroundBrush(Qt.white)
self._redraw_graph_scene()
# this runs in a non-gui thread, so don't access widgets here directly
def _generate_dotcode(self):
includes = []
excludes = []
for name in self._options['names']:
if name.strip().startswith('-'):
excludes.append(name.strip()[1:])
else:
includes.append(name.strip())
# orientation = 'LR'
descendants = True
ancestors = True
if self._options['directions'] == 1:
descendants = False
if self._options['directions'] == 0:
ancestors = False
return self.dotcode_generator.generate_dotcode(dotcode_factory=self.dotcode_factory,
selected_names=includes,
excludes=excludes,
depth=self._options['depth'],
with_stacks=self._options['with_stacks'],
descendants=descendants,
ancestors=ancestors,
mark_selected=self._options['mark_selected'],
colortheme=self._options['colortheme'],
hide_transitives=self._options['hide_transitives'],
hide_wet=self._options['package_types'] == 1,
hide_dry=self._options['package_types'] == 2)
# this runs in a non-gui thread, so don't access widgets here directly
def _update_graph(self, dotcode):
self._current_dotcode = dotcode
self._nodes, self._edges = self.dot_to_qt.dotcode_to_qt_items(self._current_dotcode, self._options['highlight_level'])
def _generate_tool_tip(self, url):
if url is not None and ':' in url:
item_type, item_path = url.split(':', 1)
if item_type == 'node':
tool_tip = 'Node:\n %s' % (item_path)
service_names = rosservice.get_service_list(node=item_path)
if service_names:
tool_tip += '\nServices:'
for service_name in service_names:
try:
service_type = rosservice.get_service_type(service_name)
tool_tip += '\n %s [%s]' % (service_name, service_type)
except rosservice.ROSServiceIOException as e:
tool_tip += '\n %s' % (e)
return tool_tip
elif item_type == 'topic':
topic_type, topic_name, _ = rostopic.get_topic_type(item_path)
return 'Topic:\n %s\nType:\n %s' % (topic_name, topic_type)
return url
def _redraw_graph_scene(self):
# remove items in order to not garbage nodes which will be continued to be used
for item in self._scene.items():
self._scene.removeItem(item)
self._scene.clear()
for node_item in self._nodes.values():
self._scene.addItem(node_item)
for edge_items in self._edges.values():
for edge_item in edge_items:
edge_item.add_to_scene(self._scene)
self._scene.setSceneRect(self._scene.itemsBoundingRect())
if self._options['auto_fit']:
self._fit_in_view()
def _load_dot(self, file_name=None):
if file_name is None:
file_name, _ = QFileDialog.getOpenFileName(self._widget, self.tr('Open graph from file'), None, self.tr('DOT graph (*.dot)'))
if file_name is None or file_name == '':
return
try:
fh = open(file_name, 'rb')
dotcode = fh.read()
fh.close()
except IOError:
return
# disable controls customizing fetched ROS graph
self._widget.depth_combo_box.setEnabled(False)
self._widget.directions_combo_box.setEnabled(False)
self._widget.package_type_combo_box.setEnabled(False)
self._widget.filter_line_edit.setEnabled(False)
self._widget.with_stacks_check_box.setEnabled(False)
self._widget.mark_check_box.setEnabled(False)
self._widget.colorize_check_box.setEnabled(False)
self._widget.hide_transitives_check_box.setEnabled(False)
self._update_graph(dotcode)
self._redraw_graph_scene()
@Slot()
def _fit_in_view(self):
self._widget.graphics_view.fitInView(self._scene.itemsBoundingRect(), Qt.KeepAspectRatio)
def _save_dot(self):
file_name, _ = QFileDialog.getSaveFileName(self._widget, self.tr('Save as DOT'), 'rospackgraph.dot', self.tr('DOT graph (*.dot)'))
if file_name is None or file_name == '':
return
handle = QFile(file_name)
if not handle.open(QIODevice.WriteOnly | QIODevice.Text):
return
handle.write(self._current_dotcode)
handle.close()
def _save_svg(self):
file_name, _ = QFileDialog.getSaveFileName(self._widget, self.tr('Save as SVG'), 'rospackgraph.svg', self.tr('Scalable Vector Graphic (*.svg)'))
if file_name is None or file_name == '':
return
generator = QSvgGenerator()
generator.setFileName(file_name)
generator.setSize((self._scene.sceneRect().size() * 2.0).toSize())
painter = QPainter(generator)
painter.setRenderHint(QPainter.Antialiasing)
self._scene.render(painter)
painter.end()
def _save_image(self):
file_name, _ = QFileDialog.getSaveFileName(self._widget, self.tr('Save as image'), 'rospackgraph.png', self.tr('Image (*.bmp *.jpg *.png *.tiff)'))
if file_name is None or file_name == '':
return
img = QImage((self._scene.sceneRect().size() * 2.0).toSize(), QImage.Format_ARGB32_Premultiplied)
painter = QPainter(img)
painter.setRenderHint(QPainter.Antialiasing)
self._scene.render(painter)
painter.end()
img.save(file_name)
def _clear_filter(self):
if not self._filtering_started:
self._widget.filter_line_edit.setText('')
self._filtering_started = True
class RqtLgsvlSimulatorConfiguratorPlugin(Plugin):
def __init__(self, context):
super(RqtLgsvlSimulatorConfiguratorPlugin, self).__init__(context)
rospack = rospkg.RosPack()
self.settings = QSettings(rospack.get_path('lgsvl_simulator_bridge')+"/config/setting.dat",QSettings.IniFormat)
# Give QObjects reasonable names
self.setObjectName('RqtLgsvlSimulatorConfiguratorPlugin')
# 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._widget = QWidget()
# Get path to UI file which should be in the "resource" folder of this package
ui_file = os.path.join(rospkg.RosPack().get_path('lgsvl_simulator_bridge'), 'resource', 'LgsvlSimulatorConfigratorPlugin.ui')
# Extend the widget with all atrributes and children from UI File
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('RqtLgsvlSimulatorConfiguratorPluginUi')
# Show _widget.windowTitle on left-top of each plugin(when it's set in _widget).
# This is useful when you open multiple plugins aat 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._widget.setWindowTitle(self._widget.windowTitle() + (' %d' % context.serial_number()))
# Add widget to the user interface
self.settings.beginGroup("simulator_params")
context.add_widget(self._widget)
ip = self.settings.value('ip')
if ip != None:
self._widget.ip_box.setText(ip)
port = self.settings.value('port')
if port != None:
self._widget.port_box.setText(port)
config_path = self.settings.value('config_path')
if config_path != None:
self._widget.configpath_box.setText(config_path)
self.settings.endGroup()
self.json_dict = {}
self.instance_id = ""
self.is_remote = False
self._widget.button_config_ref.clicked[bool].connect(self._handle_config_ref_button_clicked)
self._widget.button_config.clicked[bool].connect(self._handle_config_button_clicked)
self._widget.launch_button.clicked[bool].connect(self._handle_launch_button_clicked)
self._widget.terminate_button.clicked[bool].connect(self._handle_terminate_button_clicked)
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.get_value(k, v)
pass
def restore_settings(self, pluign_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
def _handle_launch_button_clicked(self):
if str(self._widget.ip_box.text()) == "localhost" or str(self._widget.ip_box.text()) == "0.0.0.0" or str(self._widget.ip_box.text()) == "127.0.0.1":
cmd = ["roslaunch","lgsvl_simulator_bridge","lgsvl_simulator.launch"]
self.proc = subprocess.Popen(cmd, stdout=subprocess.PIPE)
self._widget.launch_button.setStyleSheet("background-color: #8fb8e0")
self._widget.terminate_button.setStyleSheet("background-color: #FFFFFF")
self.is_remote = False
return
else:
cmd = ["roslaunch","lgsvl_simulator_bridge","bridge.launch"]
self.proc = subprocess.Popen(cmd, stdout=subprocess.PIPE)
address = "http://" + self._widget.ip_box.text() + ":" + self._widget.port_box.text()+"/simulator/launch"
self.settings.beginGroup("simulator_params")
self.settings.setValue("ip", self._widget.ip_box.text())
self.settings.setValue("port", self._widget.port_box.text())
self.settings.endGroup()
try:
response = requests.post(address,json=self.json_dict)
self.instance_id = response.json()[u'instance_id']
except:
self._widget.launch_button.setStyleSheet("background-color: #F5A9A9")
return
self.is_remote = False
self._widget.launch_button.setStyleSheet("background-color: #8fb8e0")
self._widget.terminate_button.setStyleSheet("background-color: #FFFFFF")
def _handle_config_ref_button_clicked(self):
rospack = rospkg.RosPack()
fname = QFileDialog.getOpenFileName(self._widget, 'Open file', rospack.get_path('lgsvl_simulator_bridge')+'/config')
self._widget.configpath_box.setText(fname[0])
def _handle_config_button_clicked(self):
path = self._widget.configpath_box.text()
try:
f = open(path, "r+")
self.json_dict = json.load(f)
self.settings.beginGroup("simulator_params")
self.settings.setValue("config_path", path)
self.settings.endGroup()
except:
self._widget.button_config.setStyleSheet("background-color: #F5A9A9")
return
self._widget.button_config.setStyleSheet("background-color: #8fb8e0")
def _handle_terminate_button_clicked(self):
address = "http://" + self._widget.ip_box.text() + ":" + self._widget.port_box.text()+"/simulator/terminate"
self.settings.beginGroup("simulator_params")
self.settings.setValue("ip", self._widget.ip_box.text())
self.settings.setValue("port", self._widget.port_box.text())
self.settings.endGroup()
if self.instance_id == "" and self.is_remote == True:
return
if self.is_remote == True:
try:
response = requests.post(address,json={u'instance_id':self.instance_id})
except:
self._widget.terminate_button.setStyleSheet("background-color: #F5A9A9")
return
try:
self.proc.terminate()
except:
self._widget.terminate_button.setStyleSheet("background-color: #F5A9A9")
return
self._widget.terminate_button.setStyleSheet("background-color: #8fb8e0")
self._widget.launch_button.setStyleSheet("background-color: #FFFFFF")
class RobotSteering(Plugin):
slider_factor = 1000.0
def __init__(self, context):
super(RobotSteering, self).__init__(context)
self.setObjectName('RobotSteering')
self._publisher = None
self._widget = QWidget()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_robot_steering'), 'resource', 'RobotSteering.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('RobotSteeringUi')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
context.add_widget(self._widget)
self._widget.topic_line_edit.textChanged.connect(self._on_topic_changed)
self._widget.stop_push_button.pressed.connect(self._on_stop_pressed)
self._widget.x_linear_slider.valueChanged.connect(self._on_x_linear_slider_changed)
self._widget.z_angular_slider.valueChanged.connect(self._on_z_angular_slider_changed)
self._widget.increase_x_linear_push_button.pressed.connect(self._on_strong_increase_x_linear_pressed)
self._widget.reset_x_linear_push_button.pressed.connect(self._on_reset_x_linear_pressed)
self._widget.decrease_x_linear_push_button.pressed.connect(self._on_strong_decrease_x_linear_pressed)
self._widget.increase_z_angular_push_button.pressed.connect(self._on_strong_increase_z_angular_pressed)
self._widget.reset_z_angular_push_button.pressed.connect(self._on_reset_z_angular_pressed)
self._widget.decrease_z_angular_push_button.pressed.connect(self._on_strong_decrease_z_angular_pressed)
self._widget.max_x_linear_double_spin_box.valueChanged.connect(self._on_max_x_linear_changed)
self._widget.min_x_linear_double_spin_box.valueChanged.connect(self._on_min_x_linear_changed)
self._widget.max_z_angular_double_spin_box.valueChanged.connect(self._on_max_z_angular_changed)
self._widget.min_z_angular_double_spin_box.valueChanged.connect(self._on_min_z_angular_changed)
self.shortcut_w = QShortcut(QKeySequence(Qt.Key_W), self._widget)
self.shortcut_w.setContext(Qt.ApplicationShortcut)
self.shortcut_w.activated.connect(self._on_increase_x_linear_pressed)
self.shortcut_x = QShortcut(QKeySequence(Qt.Key_X), self._widget)
self.shortcut_x.setContext(Qt.ApplicationShortcut)
self.shortcut_x.activated.connect(self._on_reset_x_linear_pressed)
self.shortcut_s = QShortcut(QKeySequence(Qt.Key_S), self._widget)
self.shortcut_s.setContext(Qt.ApplicationShortcut)
self.shortcut_s.activated.connect(self._on_decrease_x_linear_pressed)
self.shortcut_a = QShortcut(QKeySequence(Qt.Key_A), self._widget)
self.shortcut_a.setContext(Qt.ApplicationShortcut)
self.shortcut_a.activated.connect(self._on_increase_z_angular_pressed)
self.shortcut_z = QShortcut(QKeySequence(Qt.Key_Z), self._widget)
self.shortcut_z.setContext(Qt.ApplicationShortcut)
self.shortcut_z.activated.connect(self._on_reset_z_angular_pressed)
self.shortcut_d = QShortcut(QKeySequence(Qt.Key_D), self._widget)
self.shortcut_d.setContext(Qt.ApplicationShortcut)
self.shortcut_d.activated.connect(self._on_decrease_z_angular_pressed)
self.shortcut_shift_w = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_W), self._widget)
self.shortcut_shift_w.setContext(Qt.ApplicationShortcut)
self.shortcut_shift_w.activated.connect(self._on_strong_increase_x_linear_pressed)
self.shortcut_shift_x = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_X), self._widget)
self.shortcut_shift_x.setContext(Qt.ApplicationShortcut)
self.shortcut_shift_x.activated.connect(self._on_reset_x_linear_pressed)
self.shortcut_shift_s = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_S), self._widget)
self.shortcut_shift_s.setContext(Qt.ApplicationShortcut)
self.shortcut_shift_s.activated.connect(self._on_strong_decrease_x_linear_pressed)
self.shortcut_shift_a = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_A), self._widget)
self.shortcut_shift_a.setContext(Qt.ApplicationShortcut)
self.shortcut_shift_a.activated.connect(self._on_strong_increase_z_angular_pressed)
self.shortcut_shift_z = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_Z), self._widget)
self.shortcut_shift_z.setContext(Qt.ApplicationShortcut)
self.shortcut_shift_z.activated.connect(self._on_reset_z_angular_pressed)
self.shortcut_shift_d = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_D), self._widget)
self.shortcut_shift_d.setContext(Qt.ApplicationShortcut)
self.shortcut_shift_d.activated.connect(self._on_strong_decrease_z_angular_pressed)
self.shortcut_space = QShortcut(QKeySequence(Qt.Key_Space), self._widget)
self.shortcut_space.setContext(Qt.ApplicationShortcut)
self.shortcut_space.activated.connect(self._on_stop_pressed)
self.shortcut_space = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_Space), self._widget)
self.shortcut_space.setContext(Qt.ApplicationShortcut)
self.shortcut_space.activated.connect(self._on_stop_pressed)
self._widget.stop_push_button.setToolTip(self._widget.stop_push_button.toolTip() + ' ' + self.tr('([Shift +] Space)'))
self._widget.increase_x_linear_push_button.setToolTip(self._widget.increase_x_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] W)'))
self._widget.reset_x_linear_push_button.setToolTip(self._widget.reset_x_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] X)'))
self._widget.decrease_x_linear_push_button.setToolTip(self._widget.decrease_x_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] S)'))
self._widget.increase_z_angular_push_button.setToolTip(self._widget.increase_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] A)'))
self._widget.reset_z_angular_push_button.setToolTip(self._widget.reset_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] Z)'))
self._widget.decrease_z_angular_push_button.setToolTip(self._widget.decrease_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] D)'))
# timer to consecutively send twist messages
self._update_parameter_timer = QTimer(self)
self._update_parameter_timer.timeout.connect(self._on_parameter_changed)
self._update_parameter_timer.start(100)
self.zero_cmd_sent = False
@Slot(str)
def _on_topic_changed(self, topic):
topic = str(topic)
self._unregister_publisher()
try:
self._publisher = rospy.Publisher(topic, Twist, queue_size=10)
except TypeError:
self._publisher = rospy.Publisher(topic, Twist)
def _on_stop_pressed(self):
self._widget.x_linear_slider.setValue(0)
self._widget.z_angular_slider.setValue(0)
def _on_x_linear_slider_changed(self):
self._widget.current_x_linear_label.setText('%0.2f m/s' % (self._widget.x_linear_slider.value() / RobotSteering.slider_factor))
self._on_parameter_changed()
def _on_z_angular_slider_changed(self):
self._widget.current_z_angular_label.setText('%0.2f rad/s' % (self._widget.z_angular_slider.value() / RobotSteering.slider_factor))
self._on_parameter_changed()
def _on_increase_x_linear_pressed(self):
self._widget.x_linear_slider.setValue(self._widget.x_linear_slider.value() + self._widget.x_linear_slider.singleStep())
def _on_reset_x_linear_pressed(self):
self._widget.x_linear_slider.setValue(0)
def _on_decrease_x_linear_pressed(self):
self._widget.x_linear_slider.setValue(self._widget.x_linear_slider.value() - self._widget.x_linear_slider.singleStep())
def _on_increase_z_angular_pressed(self):
self._widget.z_angular_slider.setValue(self._widget.z_angular_slider.value() + self._widget.z_angular_slider.singleStep())
def _on_reset_z_angular_pressed(self):
self._widget.z_angular_slider.setValue(0)
def _on_decrease_z_angular_pressed(self):
self._widget.z_angular_slider.setValue(self._widget.z_angular_slider.value() - self._widget.z_angular_slider.singleStep())
def _on_max_x_linear_changed(self, value):
self._widget.x_linear_slider.setMaximum(value * RobotSteering.slider_factor)
def _on_min_x_linear_changed(self, value):
self._widget.x_linear_slider.setMinimum(value * RobotSteering.slider_factor)
def _on_max_z_angular_changed(self, value):
self._widget.z_angular_slider.setMaximum(value * RobotSteering.slider_factor)
def _on_min_z_angular_changed(self, value):
self._widget.z_angular_slider.setMinimum(value * RobotSteering.slider_factor)
def _on_strong_increase_x_linear_pressed(self):
self._widget.x_linear_slider.setValue(self._widget.x_linear_slider.value() + self._widget.x_linear_slider.pageStep())
def _on_strong_decrease_x_linear_pressed(self):
self._widget.x_linear_slider.setValue(self._widget.x_linear_slider.value() - self._widget.x_linear_slider.pageStep())
def _on_strong_increase_z_angular_pressed(self):
self._widget.z_angular_slider.setValue(self._widget.z_angular_slider.value() + self._widget.z_angular_slider.pageStep())
def _on_strong_decrease_z_angular_pressed(self):
self._widget.z_angular_slider.setValue(self._widget.z_angular_slider.value() - self._widget.z_angular_slider.pageStep())
def _on_parameter_changed(self):
self._send_twist(self._widget.x_linear_slider.value() / RobotSteering.slider_factor, self._widget.z_angular_slider.value() / RobotSteering.slider_factor)
def _send_twist(self, x_linear, z_angular):
if self._publisher is None:
return
twist = Twist()
twist.linear.x = x_linear
twist.linear.y = 0
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = z_angular
# Only send the zero command once so other devices can take control
if x_linear == 0 and z_angular == 0:
if not self.zero_cmd_sent:
self.zero_cmd_sent = True
self._publisher.publish(twist)
else:
self.zero_cmd_sent = False
self._publisher.publish(twist)
def _unregister_publisher(self):
if self._publisher is not None:
self._publisher.unregister()
self._publisher = None
def shutdown_plugin(self):
self._update_parameter_timer.stop()
self._unregister_publisher()
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('topic' , self._widget.topic_line_edit.text())
instance_settings.set_value('vx_max', self._widget.max_x_linear_double_spin_box.value())
instance_settings.set_value('vx_min', self._widget.min_x_linear_double_spin_box.value())
instance_settings.set_value('vw_max', self._widget.max_z_angular_double_spin_box.value())
instance_settings.set_value('vw_min', self._widget.min_z_angular_double_spin_box.value())
def restore_settings(self, plugin_settings, instance_settings):
value = instance_settings.value('topic', "/cmd_vel")
value = rospy.get_param("~default_topic", value)
self._widget.topic_line_edit.setText(value)
value = self._widget.max_x_linear_double_spin_box.value()
value = instance_settings.value( 'vx_max', value)
value = rospy.get_param("~default_vx_max", value)
self._widget.max_x_linear_double_spin_box.setValue(float(value))
value = self._widget.min_x_linear_double_spin_box.value()
value = instance_settings.value( 'vx_min', value)
value = rospy.get_param("~default_vx_min", value)
self._widget.min_x_linear_double_spin_box.setValue(float(value))
value = self._widget.max_z_angular_double_spin_box.value()
value = instance_settings.value( 'vw_max', value)
value = rospy.get_param("~default_vw_max", value)
self._widget.max_z_angular_double_spin_box.setValue(float(value))
value = self._widget.min_z_angular_double_spin_box.value()
value = instance_settings.value( 'vw_min', value)
value = rospy.get_param("~default_vw_min", value)
self._widget.min_z_angular_double_spin_box.setValue(float(value))
class RosGraph(Plugin):
_deferred_fit_in_view = Signal()
def __init__(self, context):
super(RosGraph, self).__init__(context)
self.initialized = False
self.setObjectName('RosGraph')
self._graph = None
self._current_dotcode = None
self._widget = QWidget()
# factory builds generic dotcode items
self.dotcode_factory = PydotFactory()
# self.dotcode_factory = PygraphvizFactory()
# generator builds rosgraph
self.dotcode_generator = RosGraphDotcodeGenerator()
# dot_to_qt transforms into Qt elements using dot layout
self.dot_to_qt = DotToQtGenerator()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_graph'), 'resource', 'RosGraph.ui')
loadUi(ui_file, self._widget, {'InteractiveGraphicsView': InteractiveGraphicsView})
self._widget.setObjectName('RosGraphUi')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
self._scene = QGraphicsScene()
self._scene.setBackgroundBrush(Qt.white)
self._widget.graphics_view.setScene(self._scene)
self._widget.graph_type_combo_box.insertItem(0, self.tr('Nodes only'), NODE_NODE_GRAPH)
self._widget.graph_type_combo_box.insertItem(1, self.tr('Nodes/Topics (active)'), NODE_TOPIC_GRAPH)
self._widget.graph_type_combo_box.insertItem(2, self.tr('Nodes/Topics (all)'), NODE_TOPIC_ALL_GRAPH)
self._widget.graph_type_combo_box.setCurrentIndex(0)
self._widget.graph_type_combo_box.currentIndexChanged.connect(self._refresh_rosgraph)
self.node_completionmodel = NamespaceCompletionModel(self._widget.filter_line_edit, False)
completer = RepeatedWordCompleter(self.node_completionmodel, self)
completer.setCompletionMode(QCompleter.PopupCompletion)
completer.setWrapAround(True)
completer.setCaseSensitivity(Qt.CaseInsensitive)
self._widget.filter_line_edit.editingFinished.connect(self._refresh_rosgraph)
self._widget.filter_line_edit.setCompleter(completer)
self.topic_completionmodel = NamespaceCompletionModel(self._widget.topic_filter_line_edit, False)
topic_completer = RepeatedWordCompleter(self.topic_completionmodel, self)
topic_completer.setCompletionMode(QCompleter.PopupCompletion)
topic_completer.setWrapAround(True)
topic_completer.setCaseSensitivity(Qt.CaseInsensitive)
self._widget.topic_filter_line_edit.editingFinished.connect(self._refresh_rosgraph)
self._widget.topic_filter_line_edit.setCompleter(topic_completer)
self._widget.namespace_cluster_check_box.clicked.connect(self._refresh_rosgraph)
self._widget.actionlib_check_box.clicked.connect(self._refresh_rosgraph)
self._widget.dead_sinks_check_box.clicked.connect(self._refresh_rosgraph)
self._widget.leaf_topics_check_box.clicked.connect(self._refresh_rosgraph)
self._widget.quiet_check_box.clicked.connect(self._refresh_rosgraph)
self._widget.refresh_graph_push_button.setIcon(QIcon.fromTheme('view-refresh'))
self._widget.refresh_graph_push_button.pressed.connect(self._update_rosgraph)
self._widget.highlight_connections_check_box.toggled.connect(self._redraw_graph_view)
self._widget.auto_fit_graph_check_box.toggled.connect(self._redraw_graph_view)
self._widget.fit_in_view_push_button.setIcon(QIcon.fromTheme('zoom-original'))
self._widget.fit_in_view_push_button.pressed.connect(self._fit_in_view)
self._widget.load_dot_push_button.setIcon(QIcon.fromTheme('document-open'))
self._widget.load_dot_push_button.pressed.connect(self._load_dot)
self._widget.save_dot_push_button.setIcon(QIcon.fromTheme('document-save-as'))
self._widget.save_dot_push_button.pressed.connect(self._save_dot)
self._widget.save_as_svg_push_button.setIcon(QIcon.fromTheme('document-save-as'))
self._widget.save_as_svg_push_button.pressed.connect(self._save_svg)
self._widget.save_as_image_push_button.setIcon(QIcon.fromTheme('image'))
self._widget.save_as_image_push_button.pressed.connect(self._save_image)
self._update_rosgraph()
self._deferred_fit_in_view.connect(self._fit_in_view, Qt.QueuedConnection)
self._deferred_fit_in_view.emit()
context.add_widget(self._widget)
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('graph_type_combo_box_index', self._widget.graph_type_combo_box.currentIndex())
instance_settings.set_value('filter_line_edit_text', self._widget.filter_line_edit.text())
instance_settings.set_value('topic_filter_line_edit_text', self._widget.topic_filter_line_edit.text())
instance_settings.set_value('namespace_cluster_check_box_state', self._widget.namespace_cluster_check_box.isChecked())
instance_settings.set_value('actionlib_check_box_state', self._widget.actionlib_check_box.isChecked())
instance_settings.set_value('dead_sinks_check_box_state', self._widget.dead_sinks_check_box.isChecked())
instance_settings.set_value('leaf_topics_check_box_state', self._widget.leaf_topics_check_box.isChecked())
instance_settings.set_value('quiet_check_box_state', self._widget.quiet_check_box.isChecked())
instance_settings.set_value('auto_fit_graph_check_box_state', self._widget.auto_fit_graph_check_box.isChecked())
instance_settings.set_value('highlight_connections_check_box_state', self._widget.highlight_connections_check_box.isChecked())
def restore_settings(self, plugin_settings, instance_settings):
self._widget.graph_type_combo_box.setCurrentIndex(int(instance_settings.value('graph_type_combo_box_index', 0)))
self._widget.filter_line_edit.setText(instance_settings.value('filter_line_edit_text', '/'))
self._widget.topic_filter_line_edit.setText(instance_settings.value('topic_filter_line_edit_text', '/'))
self._widget.namespace_cluster_check_box.setChecked(instance_settings.value('namespace_cluster_check_box_state', True) in [True, 'true'])
self._widget.actionlib_check_box.setChecked(instance_settings.value('actionlib_check_box_state', True) in [True, 'true'])
self._widget.dead_sinks_check_box.setChecked(instance_settings.value('dead_sinks_check_box_state', True) in [True, 'true'])
self._widget.leaf_topics_check_box.setChecked(instance_settings.value('leaf_topics_check_box_state', True) in [True, 'true'])
self._widget.quiet_check_box.setChecked(instance_settings.value('quiet_check_box_state', True) in [True, 'true'])
self._widget.auto_fit_graph_check_box.setChecked(instance_settings.value('auto_fit_graph_check_box_state', True) in [True, 'true'])
self._widget.highlight_connections_check_box.setChecked(instance_settings.value('highlight_connections_check_box_state', True) in [True, 'true'])
self.initialized = True
self._refresh_rosgraph()
def _update_rosgraph(self):
# re-enable controls customizing fetched ROS graph
self._widget.graph_type_combo_box.setEnabled(True)
self._widget.filter_line_edit.setEnabled(True)
self._widget.topic_filter_line_edit.setEnabled(True)
self._widget.namespace_cluster_check_box.setEnabled(True)
self._widget.actionlib_check_box.setEnabled(True)
self._widget.dead_sinks_check_box.setEnabled(True)
self._widget.leaf_topics_check_box.setEnabled(True)
self._widget.quiet_check_box.setEnabled(True)
self._graph = rosgraph.impl.graph.Graph()
self._graph.set_master_stale(5.0)
self._graph.set_node_stale(5.0)
self._graph.update()
self.node_completionmodel.refresh(self._graph.nn_nodes)
self.topic_completionmodel.refresh(self._graph.nt_nodes)
self._refresh_rosgraph()
def _refresh_rosgraph(self):
if not self.initialized:
return
self._update_graph_view(self._generate_dotcode())
def _generate_dotcode(self):
ns_filter = self._widget.filter_line_edit.text()
topic_filter = self._widget.topic_filter_line_edit.text()
graph_mode = self._widget.graph_type_combo_box.itemData(self._widget.graph_type_combo_box.currentIndex())
orientation = 'LR'
if self._widget.namespace_cluster_check_box.isChecked():
namespace_cluster = 1
else:
namespace_cluster = 0
accumulate_actions = self._widget.actionlib_check_box.isChecked()
hide_dead_end_topics = self._widget.dead_sinks_check_box.isChecked()
hide_single_connection_topics = self._widget.leaf_topics_check_box.isChecked()
quiet = self._widget.quiet_check_box.isChecked()
return self.dotcode_generator.generate_dotcode(
rosgraphinst=self._graph,
ns_filter=ns_filter,
topic_filter=topic_filter,
graph_mode=graph_mode,
hide_single_connection_topics=hide_single_connection_topics,
hide_dead_end_topics=hide_dead_end_topics,
cluster_namespaces_level=namespace_cluster,
accumulate_actions=accumulate_actions,
dotcode_factory=self.dotcode_factory,
orientation=orientation,
quiet=quiet)
def _update_graph_view(self, dotcode):
if dotcode == self._current_dotcode:
return
self._current_dotcode = dotcode
self._redraw_graph_view()
def _generate_tool_tip(self, url):
if url is not None and ':' in url:
item_type, item_path = url.split(':', 1)
if item_type == 'node':
tool_tip = 'Node:\n %s' % (item_path)
service_names = rosservice.get_service_list(node=item_path)
if service_names:
tool_tip += '\nServices:'
for service_name in service_names:
try:
service_type = rosservice.get_service_type(service_name)
tool_tip += '\n %s [%s]' % (service_name, service_type)
except rosservice.ROSServiceIOException as e:
tool_tip += '\n %s' % (e)
return tool_tip
elif item_type == 'topic':
topic_type, topic_name, _ = rostopic.get_topic_type(item_path)
return 'Topic:\n %s\nType:\n %s' % (topic_name, topic_type)
return url
def _redraw_graph_view(self):
self._scene.clear()
if self._widget.highlight_connections_check_box.isChecked():
highlight_level = 3
else:
highlight_level = 1
# layout graph and create qt items
(nodes, edges) = self.dot_to_qt.dotcode_to_qt_items(self._current_dotcode,
highlight_level=highlight_level,
same_label_siblings=True)
for node_item in nodes.itervalues():
self._scene.addItem(node_item)
for edge_items in edges.itervalues():
for edge_item in edge_items:
edge_item.add_to_scene(self._scene)
self._scene.setSceneRect(self._scene.itemsBoundingRect())
if self._widget.auto_fit_graph_check_box.isChecked():
self._fit_in_view()
def _load_dot(self, file_name=None):
if file_name is None:
file_name, _ = QFileDialog.getOpenFileName(self._widget, self.tr('Open graph from file'), None, self.tr('DOT graph (*.dot)'))
if file_name is None or file_name == '':
return
try:
fh = open(file_name, 'rb')
dotcode = fh.read()
fh.close()
except IOError:
return
# disable controls customizing fetched ROS graph
self._widget.graph_type_combo_box.setEnabled(False)
self._widget.filter_line_edit.setEnabled(False)
self._widget.topic_filter_line_edit.setEnabled(False)
self._widget.namespace_cluster_check_box.setEnabled(False)
self._widget.actionlib_check_box.setEnabled(False)
self._widget.dead_sinks_check_box.setEnabled(False)
self._widget.leaf_topics_check_box.setEnabled(False)
self._widget.quiet_check_box.setEnabled(False)
self._update_graph_view(dotcode)
def _fit_in_view(self):
self._widget.graphics_view.fitInView(self._scene.itemsBoundingRect(), Qt.KeepAspectRatio)
def _save_dot(self):
file_name, _ = QFileDialog.getSaveFileName(self._widget, self.tr('Save as DOT'), 'rosgraph.dot', self.tr('DOT graph (*.dot)'))
if file_name is None or file_name == '':
return
handle = QFile(file_name)
if not handle.open(QIODevice.WriteOnly | QIODevice.Text):
return
handle.write(self._current_dotcode)
handle.close()
def _save_svg(self):
file_name, _ = QFileDialog.getSaveFileName(self._widget, self.tr('Save as SVG'), 'rosgraph.svg', self.tr('Scalable Vector Graphic (*.svg)'))
if file_name is None or file_name == '':
return
generator = QSvgGenerator()
generator.setFileName(file_name)
generator.setSize((self._scene.sceneRect().size() * 2.0).toSize())
painter = QPainter(generator)
painter.setRenderHint(QPainter.Antialiasing)
self._scene.render(painter)
painter.end()
def _save_image(self):
file_name, _ = QFileDialog.getSaveFileName(self._widget, self.tr('Save as image'), 'rosgraph.png', self.tr('Image (*.bmp *.jpg *.png *.tiff)'))
if file_name is None or file_name == '':
return
img = QImage((self._scene.sceneRect().size() * 2.0).toSize(), QImage.Format_ARGB32_Premultiplied)
painter = QPainter(img)
painter.setRenderHint(QPainter.Antialiasing)
self._scene.render(painter)
painter.end()
img.save(file_name)
class Overview(Plugin):
def __init__(self, context):
super(Overview, self).__init__(context)
self.__tfListener = tf.TransformListener()
self.__copterFrame = "base_link"
self.__worldFrame = "world"
# Give QObjects reasonable names
self.setObjectName('simple_position_control_gui')
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
parser.add_argument("-c", "--copterFrame", dest="copterFrame", help="Specify the copter frame")
parser.add_argument("-w", "--worldFrame", dest="worldFrame", help="Specify the world frame")
args, unknowns = parser.parse_known_args(context.argv())
if args.copterFrame:
self.__copterFrame = args.copterFrame
rospy.loginfo("using %s as copter frame", self.__copterFrame)
if args.worldFrame:
self.__worldFrame = args.worldFrame
rospy.loginfo("using %s as world frame", self.__worldFrame)
# Create QWidget
self._widget = QWidget()
# Get path to UI file which should be in the "resource" folder of this node
ui_file = os.path.join(rospkg.RosPack().get_path('position_controller'), 'src', 'rqt_control_gui', 'resource', 'overview.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('OverviewUI')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). 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._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
self._widget.forwardButton.clicked.connect(self.goForward)
self._widget.backwardButton.clicked.connect(self.goBackward)
self._widget.leftButton.clicked.connect(self.goLeft)
self._widget.rightButton.clicked.connect(self.goRight)
self._widget.rotateButton.clicked.connect(self.rotate)
self._widget.landButton.clicked.connect(self.land)
self._widget.startButton.clicked.connect(self.start)
self._widget.absoluteControlButton.clicked.connect(self.absoluteControl)
self._widget.tfButton.clicked.connect(self.goToTF)
self.__statusLabel = self._widget.statusLabel
def setRelativePose(self, x_delta, y_delta, yaw_delta):
"""
set new target position relative to current position
:param: x_delta: change in x
:param: y_delta: change in y
:param: yaw_delta: change in yaw
"""
try:
(trans, rot) = self.__tfListener.lookupTransform(self.__worldFrame, self.__copterFrame, rospy.Time(0))
quad_delta = tf.transformations.quaternion_from_euler(0, 0, yaw_delta)
translation_delta = tf.transformations.translation_matrix((x_delta, y_delta, 0))
m_current = tf.transformations.translation_matrix(trans).dot(tf.transformations.quaternion_matrix(rot))
m_delta = translation_delta.dot(tf.transformations.quaternion_matrix(quad_delta))
m_target = m_current.dot(m_delta)
_, _, target_yaw = tf.transformations.euler_from_matrix(m_target)
target_x, target_y, _ = tf.transformations.translation_from_matrix(m_target)
self.__statusLabel.setText("going to x: {0} y: {1} yaw: {2}".format(target_x, target_y, target_yaw))
rospy.wait_for_service('/position_controller/set_target_pos', 1)
setPose = rospy.ServiceProxy('/position_controller/set_target_pos', SetPos)
setPose(target_x, target_y, target_yaw)
except Exception as e:
self.__statusLabel.setText(str(e))
def goForward(self):
distance = float(self._widget.distanceEdit.text())
self.setRelativePose(distance, 0, 0)
def goBackward(self):
distance = float(self._widget.distanceEdit.text())
self.setRelativePose(-distance, 0, 0)
def goLeft(self):
distance = float(self._widget.distanceEdit.text())
self.setRelativePose(0, distance, 0)
def goRight(self):
distance = float(self._widget.distanceEdit.text())
self.setRelativePose(0, -distance, 0)
def rotate(self):
angle = math.pi * float(self._widget.rotationEdit.text()) / 180.0
self.setRelativePose(0, 0, angle)
def land(self):
try:
self.setAltitude(0.0)
except ValueError:
pass
def start(self):
try:
altitude = float(self._widget.zEdit.text())
self.setAltitude(altitude)
self.setAltitude(altitude)
statusMessage = "starting to altitude {0}".format(altitude)
self.__statusLabel.setText(statusMessage)
except ValueError:
pass
def setAltitude(self, altitude):
rospy.wait_for_service('/position_controller/set_altitude', 1)
setAltitudeService = rospy.ServiceProxy('/position_controller/set_altitude', SetAltitude)
setAltitudeService(altitude)
def absoluteControl(self):
statusMessage = ""
try:
((x, y, _z), quaternion) = self.__tfListener.lookupTransform(self.__worldFrame, self.__copterFrame, rospy.Time(0))
(_roll, _pitch, yaw) = euler_from_quaternion(quaternion)
try:
yaw = math.pi * float(self._widget.angleEdit.text()) / 180.0
except ValueError:
pass
try:
x = float(self._widget.xEdit.text())
except ValueError:
pass
try:
y = float(self._widget.yEdit.text())
except ValueError:
pass
statusMessage += "going to x: {0} y: {1} yaw: {2} ".format(x, y, yaw)
rospy.wait_for_service('/position_controller/set_target_pos', 1)
setPose = rospy.ServiceProxy('/position_controller/set_target_pos', SetPos)
setPose(x, y, yaw)
self.__statusLabel.setText(statusMessage)
try:
altitude = float(self._widget.zEdit.text())
statusMessage += "setting altitude to {0}".format(altitude)
self.setAltitude(altitude)
self.__statusLabel.setText(statusMessage)
except ValueError:
pass
except Exception as e:
self.__statusLabel.setText(str(e))
def goToTF(self):
pass
def shutdown_plugin(self):
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
rospy.loginfo("saving simple position controller gui setting")
instance_settings.set_value("worldFrame", self.__worldFrame)
instance_settings.set_value("copterFrame", self.__copterFrame)
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
rospy.loginfo("restoring simple position controller gui setting")
storedWorldFrame = instance_settings.value("worldFrame")
if type(storedWorldFrame) == unicode:
storedWorldFrame = storedWorldFrame.encode('ascii', 'ignore')
if storedWorldFrame:
self.__worldFrame = storedWorldFrame
storedCopterFrame = instance_settings.value("copterFrame")
if type(storedCopterFrame) == unicode:
storedCopterFrame = storedCopterFrame.encode('ascii', 'ignore')
if storedCopterFrame:
self.__copterFrame = storedCopterFrame
class JointTrajectoryController(Plugin):
"""
Graphical frontend for a C{JointTrajectoryController}.
There are two modes for interacting with a controller:
1. B{Monitor mode} Joint displays are updated with the state reported
by the controller. This is a read-only mode and does I{not} send
control commands. Every time a new controller is selected, it starts
in monitor mode for safety reasons.
2. B{Control mode} Joint displays update the control command that is
sent to the controller. Commands are sent periodically evan if the
displays are not being updated by the user.
To control the aggressiveness of the motions, the maximum speed of the
sent commands can be scaled down using the C{Speed scaling control}
This plugin is able to detect and keep track of all active controller
managers, as well as the JointTrajectoryControllers that are I{running}
in each controller manager.
For a controller to be compatible with this plugin, it must comply with
the following requisites:
- The controller type contains the C{JointTrajectoryController}
substring, e.g., C{position_controllers/JointTrajectoryController}
- The controller exposes the C{command} and C{state} topics in its
ROS interface.
Additionally, there must be a URDF loaded with a valid joint limit
specification, namely position (when applicable) and velocity limits.
A reference implementation of the C{JointTrajectoryController} is
available in the C{joint_trajectory_controller} ROS package.
"""
_cmd_pub_freq = 10.0 # Hz
_widget_update_freq = 30.0 # Hz
_ctrlrs_update_freq = 1 # Hz
_min_traj_dur = 5.0 / _cmd_pub_freq # Minimum trajectory duration
jointStateChanged = Signal([dict])
def __init__(self, context):
super(JointTrajectoryController, self).__init__(context)
self.setObjectName('JointTrajectoryController')
self._node = context.node
# Get the robot_description via a topic
qos_profile = QoSProfile(depth=1,
history=HistoryPolicy.KEEP_LAST,
durability=DurabilityPolicy.TRANSIENT_LOCAL)
self._robot_description_sub = self._node.create_subscription(
String, 'robot_description',
self._robot_description_cb, qos_profile)
self._robot_description = None
self._publisher = None
self._widget = QWidget()
_, package_path = get_resource('packages', 'rqt_joint_trajectory_controller')
ui_file = os.path.join(package_path, 'share', 'rqt_joint_trajectory_controller', 'resource', 'joint_trajectory_controller.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('JointTrajectoryControllerUi')
if context.serial_number() > 1:
self._widget.setWindowTitle(
self._widget.windowTitle() + (' (%d)' % context.serial_number()))
context.add_widget(self._widget)
ns = self._node.get_namespace()
self._widget.controller_group.setTitle('ns: ' + ns)
# Setup speed scaler
speed_scaling = DoubleEditor(1.0, 100.0)
speed_scaling.spin_box.setSuffix('%')
speed_scaling.spin_box.setValue(50.0)
speed_scaling.spin_box.setDecimals(0)
speed_scaling.setEnabled(False)
self._widget.speed_scaling_layout.addWidget(speed_scaling)
self._speed_scaling_widget = speed_scaling
speed_scaling.valueChanged.connect(self._on_speed_scaling_change)
self._on_speed_scaling_change(speed_scaling.value())
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). 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._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
# Initialize members
self._jtc_name = [] # Name of selected joint trajectory controller
self._cm_ns = [] # Namespace of the selected controller manager
self._joint_pos = {} # name->pos map for joints of selected controller
self._joint_names = [] # Ordered list of selected controller joints
self._robot_joint_limits = {} # Lazily evaluated on first use
# Timer for sending commands to active controller
self._update_cmd_timer = QTimer(self)
self._update_cmd_timer.setInterval(1000.0 / self._cmd_pub_freq)
self._update_cmd_timer.timeout.connect(self._update_cmd_cb)
# Timer for updating the joint widgets from the controller state
self._update_act_pos_timer = QTimer(self)
self._update_act_pos_timer.setInterval(1000.0 /
self._widget_update_freq)
self._update_act_pos_timer.timeout.connect(self._update_joint_widgets)
# Timer for controller manager updates
# TODO: self._list_cm = ControllerManagerLister()
self._update_cm_list_timer = QTimer(self)
self._update_cm_list_timer.setInterval(1000.0 /
self._ctrlrs_update_freq)
self._update_cm_list_timer.timeout.connect(self._update_cm_list)
self._update_cm_list_timer.start()
# Timer for running controller updates
self._update_jtc_list_timer = QTimer(self)
self._update_jtc_list_timer.setInterval(1000.0 /
self._ctrlrs_update_freq)
self._update_jtc_list_timer.timeout.connect(self._update_jtc_list)
self._update_jtc_list_timer.start()
# Signal connections
w = self._widget
w.enable_button.toggled.connect(self._on_jtc_enabled)
w.jtc_combo.currentIndexChanged[str].connect(self._on_jtc_change)
w.cm_combo.currentIndexChanged[str].connect(self._on_cm_change)
self._cmd_pub = None # Controller command publisher
self._state_sub = None # Controller state subscriber
self._list_controllers = None
self._traj_ns_topic_dict = None
def shutdown_plugin(self):
self._update_cmd_timer.stop()
self._update_act_pos_timer.stop()
self._update_cm_list_timer.stop()
self._update_jtc_list_timer.stop()
self._unregister_state_sub()
self._unregister_cmd_pub()
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('cm_ns', self._cm_ns)
instance_settings.set_value('jtc_name', self._jtc_name)
def restore_settings(self, plugin_settings, instance_settings):
# Restore last session's controller_manager, if present
self._update_cm_list()
cm_ns = instance_settings.value('cm_ns')
cm_combo = self._widget.cm_combo
cm_list = [cm_combo.itemText(i) for i in range(cm_combo.count())]
try:
idx = cm_list.index(cm_ns)
cm_combo.setCurrentIndex(idx)
# Resore last session's controller, if running
self._update_jtc_list()
jtc_name = instance_settings.value('jtc_name')
jtc_combo = self._widget.jtc_combo
jtc_list = [jtc_combo.itemText(i) for i in range(jtc_combo.count())]
try:
idx = jtc_list.index(jtc_name)
jtc_combo.setCurrentIndex(idx)
except (ValueError):
pass
except (ValueError):
pass
# def trigger_configuration(self):
# Comment in to signal that the plugin has a way to configure
# This will enable a setting button (gear icon) in each dock widget
# title bar
# Usually used to open a modal configuration dialog
def _update_cm_list(self):
trajectory_topics = _search_for_trajectory_topics(self._node)
## TODO: remove test code
#trajectory_topics.append('/my_test/controller')
#trajectory_topics.append('/no_namespace')
#trajectory_topics.append('no_root')
self._traj_ns_topic_dict = {}
for full_topic in trajectory_topics:
ns, topic = _split_namespace_from_topic(full_topic)
self._traj_ns_topic_dict.setdefault(ns, []).append(topic)
update_combo(self._widget.cm_combo, list(self._traj_ns_topic_dict.keys()))
def _update_jtc_list(self):
# Clear controller list if no controller information is available
if self._traj_ns_topic_dict is None:
self._widget.jtc_combo.clear()
return
#print(get_joint_limits(self._robot_description))
## List of running controllers with a valid joint limits specification
## for _all_ their joints
#running_jtc = self._running_jtc_info()
#if running_jtc and not self._robot_joint_limits:
# self._robot_joint_limits = get_joint_limits() # Lazy evaluation
#valid_jtc = []
#for jtc_info in running_jtc:
# has_limits = all(name in self._robot_joint_limits
# for name in _jtc_joint_names(jtc_info))
# if has_limits:
# valid_jtc.append(jtc_info);
#valid_jtc_names = [data.name for data in valid_jtc]
# Get the currently selected namespace
curr_ns = self._widget.cm_combo.currentText()
topics = self._traj_ns_topic_dict[curr_ns]
update_combo(self._widget.jtc_combo, topics)
def _on_speed_scaling_change(self, val):
self._speed_scale = val / self._speed_scaling_widget.slider.maximum()
def _on_joint_state_change(self, actual_pos):
assert(len(actual_pos) == len(self._joint_pos))
for name in actual_pos.keys():
self._joint_pos[name]['position'] = actual_pos[name]
def _on_cm_change(self, cm_ns):
self._cm_ns = cm_ns
if cm_ns:
self._widget.jtc_combo.clear()
self._update_jtc_list()
else:
self._list_controllers = None
def _on_jtc_change(self, jtc_name):
self._unload_jtc()
self._jtc_name = jtc_name
if self._jtc_name:
self._load_jtc()
def _on_jtc_enabled(self, val):
# Don't allow enabling if there are no controllers selected
if not self._jtc_name:
self._widget.enable_button.setChecked(False)
return
# Enable/disable joint displays
for joint_widget in self._joint_widgets():
joint_widget.setEnabled(val)
# Enable/disable speed scaling
self._speed_scaling_widget.setEnabled(val)
if val:
# Widgets send desired position commands to controller
self._update_act_pos_timer.stop()
self._update_cmd_timer.start()
else:
# Controller updates widgets with actual position
self._update_cmd_timer.stop()
self._update_act_pos_timer.start()
def _load_jtc(self):
self._robot_joint_limits = get_joint_limits(self._robot_description)
self._joint_names = list(self._robot_joint_limits.keys())
self._joint_pos = { name: {} for name in self._joint_names }
# Update joint display
try:
layout = self._widget.joint_group.layout()
for name in self._joint_names:
limits = self._robot_joint_limits[name]
joint_widget = DoubleEditor(limits['min_position'],
limits['max_position'])
layout.addRow(name, joint_widget)
# NOTE: Using partial instead of a lambda because lambdas
# "will not evaluate/look up the argument values before it is
# effectively called, breaking situations like using a loop
# variable inside it"
from functools import partial
par = partial(self._update_single_cmd_cb, name=name)
joint_widget.valueChanged.connect(par)
except:
# TODO: Can we do better than swallow the exception?
from sys import exc_info
print('Unexpected error:', exc_info()[0])
# Enter monitor mode (sending commands disabled)
self._on_jtc_enabled(False)
# Setup ROS interfaces
jtc_ns = _resolve_controller_ns(self._cm_ns, self._jtc_name)
#state_topic = jtc_ns + '/state'
state_topic = 'state'
#cmd_topic = jtc_ns + '/command'
cmd_topic = '/joint_trajectory_controller/joint_trajectory'
#self._state_sub = rospy.Subscriber(state_topic,
# JointTrajectoryControllerState,
# self._state_cb,
# queue_size=1)
qos_profile = QoSProfile(depth=1,
history=HistoryPolicy.KEEP_LAST,
durability=DurabilityPolicy.TRANSIENT_LOCAL)
self._state_sub = self._node.create_subscription(
JointTrajectoryControllerState, state_topic, self._state_cb,
qos_profile)
self._cmd_pub = self._node.create_publisher(JointTrajectory, cmd_topic, 1)
# Start updating the joint positions
self.jointStateChanged.connect(self._on_joint_state_change)
def _unload_jtc(self):
# Stop updating the joint positions
try:
self.jointStateChanged.disconnect(self._on_joint_state_change)
except:
pass
# Reset ROS interfaces
self._unregister_state_sub()
self._unregister_cmd_pub()
# Clear joint widgets
# NOTE: Implementation is a workaround for:
# https://bugreports.qt-project.org/browse/QTBUG-15990 :(
layout = self._widget.joint_group.layout()
if layout is not None:
while layout.count():
layout.takeAt(0).widget().deleteLater()
# Delete existing layout by reparenting to temporary
QWidget().setLayout(layout)
self._widget.joint_group.setLayout(QFormLayout())
# Reset joint data
self._joint_names = []
self._joint_pos = {}
# Enforce monitor mode (sending commands disabled)
self._widget.enable_button.setChecked(False)
def _running_jtc_info(self):
from controller_manager_msgs.utils\
import filter_by_type, filter_by_state
controller_list = self._list_controllers()
jtc_list = filter_by_type(controller_list,
'JointTrajectoryController',
match_substring=True)
running_jtc_list = filter_by_state(jtc_list, 'running')
return running_jtc_list
def _unregister_cmd_pub(self):
if self._cmd_pub is not None:
self._node.destroy_publisher(self._cmd_pub)
self._cmd_pub = None
def _unregister_state_sub(self):
if self._state_sub is not None:
self._node.destroy_subscription(self._state_sub)
self._state_sub = None
def _robot_description_cb(self, msg):
self._robot_description = msg.data
def _state_cb(self, msg):
actual_pos = {}
for i in range(len(msg.joint_names)):
joint_name = msg.joint_names[i]
joint_pos = msg.actual.positions[i]
actual_pos[joint_name] = joint_pos
self.jointStateChanged.emit(actual_pos)
def _update_single_cmd_cb(self, val, name):
self._joint_pos[name]['command'] = val
def _update_cmd_cb(self):
dur = []
traj = JointTrajectory()
traj.joint_names = self._joint_names
point = JointTrajectoryPoint()
for name in traj.joint_names:
pos = self._joint_pos[name]['position']
cmd = pos
try:
cmd = self._joint_pos[name]['command']
except (KeyError):
pass
max_vel = self._robot_joint_limits[name]['max_velocity']
dur.append(max(abs(cmd - pos) / max_vel, self._min_traj_dur))
point.positions.append(cmd)
max_duration_scaled = max(dur) / self._speed_scale
seconds = floor(max_duration_scaled)
nanoseconds = (max_duration_scaled - seconds) * 1e9
duration = Duration(seconds=seconds, nanoseconds=nanoseconds)
point.time_from_start = duration.to_msg()
traj.points.append(point)
self._cmd_pub.publish(traj)
def _update_joint_widgets(self):
joint_widgets = self._joint_widgets()
for id in range(len(joint_widgets)):
joint_name = self._joint_names[id]
try:
joint_pos = self._joint_pos[joint_name]['position']
joint_widgets[id].setValue(joint_pos)
except (KeyError):
pass # Can happen when first connected to controller
def _joint_widgets(self): # TODO: Cache instead of compute every time?
widgets = []
layout = self._widget.joint_group.layout()
for row_id in range(layout.rowCount()):
widgets.append(layout.itemAt(row_id,
QFormLayout.FieldRole).widget())
return widgets
class ControllerManager(Plugin):
"""
Graphical frontend for managing ros_control controllers.
"""
_cm_update_freq = 1 # Hz
def __init__(self, context):
super(ControllerManager, self).__init__(context)
self.setObjectName('ControllerManager')
# Create QWidget and extend it with all the attributes and children
# from the UI file
self._widget = QWidget()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_controller_manager'),
'resource',
'controller_manager.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('ControllerManagerUi')
# Pop-up that displays controller information
self._popup_widget = QWidget()
ui_file = os.path.join(rp.get_path('rqt_controller_manager'),
'resource',
'controller_info.ui')
loadUi(ui_file, self._popup_widget)
self._popup_widget.setObjectName('ControllerInfoUi')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). 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._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
# Initialize members
self._cm_ns = [] # Namespace of the selected controller manager
self._controllers = [] # State of each controller
self._table_model = None
self._controller_lister = None
# Controller manager service proxies
self._load_srv = None
self._unload_srv = None
self._switch_srv = None
# Controller state icons
rospack = rospkg.RosPack()
path = rospack.get_path('rqt_controller_manager')
self._icons = {'running': QIcon(path + '/resource/led_green.png'),
'stopped': QIcon(path + '/resource/led_red.png'),
'uninitialized': QIcon(path + '/resource/led_off.png'),
'initialized': QIcon(path + '/resource/led_red.png')}
# Controllers display
table_view = self._widget.table_view
table_view.setContextMenuPolicy(Qt.CustomContextMenu)
table_view.customContextMenuRequested.connect(self._on_ctrl_menu)
table_view.doubleClicked.connect(self._on_ctrl_info)
header = table_view.horizontalHeader()
header.setSectionResizeMode(QHeaderView.ResizeToContents)
header.setContextMenuPolicy(Qt.CustomContextMenu)
header.customContextMenuRequested.connect(self._on_header_menu)
# Timer for controller manager updates
self._list_cm = ControllerManagerLister()
self._update_cm_list_timer = QTimer(self)
self._update_cm_list_timer.setInterval(1000.0 /
self._cm_update_freq)
self._update_cm_list_timer.timeout.connect(self._update_cm_list)
self._update_cm_list_timer.start()
# Timer for running controller updates
self._update_ctrl_list_timer = QTimer(self)
self._update_ctrl_list_timer.setInterval(1000.0 /
self._cm_update_freq)
self._update_ctrl_list_timer.timeout.connect(self._update_controllers)
self._update_ctrl_list_timer.start()
# Signal connections
w = self._widget
w.cm_combo.currentIndexChanged[str].connect(self._on_cm_change)
def shutdown_plugin(self):
self._update_cm_list_timer.stop()
self._update_ctrl_list_timer.stop()
self._popup_widget.hide()
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('cm_ns', self._cm_ns)
def restore_settings(self, plugin_settings, instance_settings):
# Restore last session's controller_manager, if present
self._update_cm_list()
cm_ns = instance_settings.value('cm_ns')
cm_combo = self._widget.cm_combo
cm_list = [cm_combo.itemText(i) for i in range(cm_combo.count())]
try:
idx = cm_list.index(cm_ns)
cm_combo.setCurrentIndex(idx)
except (ValueError):
pass
# def trigger_configuration(self):
# Comment in to signal that the plugin has a way to configure
# This will enable a setting button (gear icon) in each dock widget
# title bar
# Usually used to open a modal configuration dialog
def _update_cm_list(self):
update_combo(self._widget.cm_combo, self._list_cm())
def _on_cm_change(self, cm_ns):
self._cm_ns = cm_ns
# Setup services for communicating with the selected controller manager
self._set_cm_services(cm_ns)
# Controller lister for the selected controller manager
if cm_ns:
self._controller_lister = ControllerLister(cm_ns)
self._update_controllers()
else:
self._controller_lister = None
def _set_cm_services(self, cm_ns):
if cm_ns:
# NOTE: Persistent services are used for performance reasons.
# If the controller manager dies, we detect it and disconnect from
# it anyway
load_srv_name = _append_ns(cm_ns, 'load_controller')
self._load_srv = rospy.ServiceProxy(load_srv_name,
LoadController,
persistent=True)
unload_srv_name = _append_ns(cm_ns, 'unload_controller')
self._unload_srv = rospy.ServiceProxy(unload_srv_name,
UnloadController,
persistent=True)
switch_srv_name = _append_ns(cm_ns, 'switch_controller')
self._switch_srv = rospy.ServiceProxy(switch_srv_name,
SwitchController,
persistent=True)
else:
self._load_srv = None
self._unload_srv = None
self._switch_srv = None
def _update_controllers(self):
# Find controllers associated to the selected controller manager
controllers = self._list_controllers()
# Update controller display, if necessary
if self._controllers != controllers:
self._controllers = controllers
self._show_controllers() # NOTE: Model is recomputed from scratch
def _list_controllers(self):
"""
@return List of controllers associated to a controller manager
namespace. Contains both stopped/running controllers, as returned by
the C{list_controllers} service, plus uninitialized controllers with
configurations loaded in the parameter server.
@rtype [str]
"""
if not self._cm_ns:
return []
# Add loaded controllers first
controllers = self._controller_lister()
# Append potential controller configs found in the parameter server
all_ctrls_ns = _resolve_controllers_ns(self._cm_ns)
for name in get_rosparam_controller_names(all_ctrls_ns):
add_ctrl = not any(name == ctrl.name for ctrl in controllers)
if add_ctrl:
type_str = _rosparam_controller_type(all_ctrls_ns, name)
uninit_ctrl = ControllerState(name=name,
type=type_str,
state='uninitialized')
controllers.append(uninit_ctrl)
return controllers
def _show_controllers(self):
table_view = self._widget.table_view
self._table_model = ControllerTable(self._controllers, self._icons)
table_view.setModel(self._table_model)
def _on_ctrl_menu(self, pos):
# Get data of selected controller
row = self._widget.table_view.rowAt(pos.y())
if row < 0:
return # Cursor is not under a valid item
ctrl = self._controllers[row]
# Show context menu
menu = QMenu(self._widget.table_view)
if ctrl.state == 'running':
action_stop = menu.addAction(self._icons['stopped'], 'Stop')
action_kill = menu.addAction(self._icons['uninitialized'],
'Stop and Unload')
elif ctrl.state == 'stopped':
action_start = menu.addAction(self._icons['running'],
'Start again')
action_unload = menu.addAction(self._icons['uninitialized'],
'Unload')
elif ctrl.state == 'initialized':
action_start = menu.addAction(self._icons['running'], 'Start')
action_unload = menu.addAction(self._icons['uninitialized'],
'Unload')
elif ctrl.state == 'uninitialized':
action_load = menu.addAction(self._icons['stopped'], 'Load')
action_spawn = menu.addAction(self._icons['running'],
'Load and Start')
action = menu.exec_(self._widget.table_view.mapToGlobal(pos))
# Evaluate user action
if ctrl.state == 'running':
if action is action_stop:
self._stop_controller(ctrl.name)
elif action is action_kill:
self._stop_controller(ctrl.name)
self._unload_controller(ctrl.name)
elif ctrl.state == 'stopped' or ctrl.state == 'initialized':
if action is action_start:
self._start_controller(ctrl.name)
elif action is action_unload:
self._unload_controller(ctrl.name)
elif ctrl.state == 'uninitialized':
if action is action_load:
self._load_controller(ctrl.name)
if action is action_spawn:
self._load_controller(ctrl.name)
self._start_controller(ctrl.name)
def _on_ctrl_info(self, index):
popup = self._popup_widget
ctrl = self._controllers[index.row()]
popup.ctrl_name.setText(ctrl.name)
popup.ctrl_type.setText(ctrl.type)
res_model = QStandardItemModel()
model_root = QStandardItem('Claimed Resources')
res_model.appendRow(model_root)
for hw_res in ctrl.claimed_resources:
hw_iface_item = QStandardItem(hw_res.hardware_interface)
model_root.appendRow(hw_iface_item)
for res in hw_res.resources:
res_item = QStandardItem(res)
hw_iface_item.appendRow(res_item)
popup.resource_tree.setModel(res_model)
popup.resource_tree.setItemDelegate(FontDelegate(popup.resource_tree))
popup.resource_tree.expandAll()
popup.move(QCursor.pos())
popup.show()
def _on_header_menu(self, pos):
header = self._widget.table_view.horizontalHeader()
# Show context menu
menu = QMenu(self._widget.table_view)
action_toggle_auto_resize = menu.addAction('Toggle Auto-Resize')
action = menu.exec_(header.mapToGlobal(pos))
# Evaluate user action
if action is action_toggle_auto_resize:
if header.resizeMode(0) == QHeaderView.ResizeToContents:
header.setSectionResizeMode(QHeaderView.Interactive)
else:
header.setSectionResizeMode(QHeaderView.ResizeToContents)
def _load_controller(self, name):
self._load_srv.call(LoadControllerRequest(name=name))
def _unload_controller(self, name):
self._unload_srv.call(UnloadControllerRequest(name=name))
def _start_controller(self, name):
strict = SwitchControllerRequest.STRICT
req = SwitchControllerRequest(start_controllers=[name],
stop_controllers=[],
strictness=strict)
self._switch_srv.call(req)
def _stop_controller(self, name):
strict = SwitchControllerRequest.STRICT
req = SwitchControllerRequest(start_controllers=[],
stop_controllers=[name],
strictness=strict)
self._switch_srv.call(req)
class LevelSelectorPlugin(Plugin):
button_signal = pyqtSignal()
button_status_signal = pyqtSignal()
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.button_signal.connect(self.update_buttons)
self.button_status_signal.connect(self.update_button_status)
# Subscribe 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
rospy.loginfo('level selector: subscribed to maps')
# 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()
rospy.loginfo('level selector: found "level_mux/change_current_level" service')
def process_multimap(self, msg):
""" Map metadata topic callback. """
rospy.loginfo('level selector: map metadata received.')
self.levels = msg.levels
# update level buttons in the selection window
self.button_signal.emit()
def update_buttons(self):
""" Update buttons in Qt window. """
rospy.loginfo('level selector: update_buttons called')
self.clean()
for index, level in enumerate(self.levels):
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):
""" Update button status Qt push button callback. """
rospy.loginfo('level selector: update_button_status')
if not self.buttons or not self.current_level:
rospy.logwarn('level selector: current level not available')
return
rospy.logdebug('buttons: ' + str(self.buttons))
for index, level in enumerate(self.levels):
rospy.logdebug('level[' + str(index) + ']: ' + str(level.level_id))
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_mux/current_level topic callback. """
rospy.loginfo('level selector: current_level topic message received')
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.button_status_signal.emit()
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 GroundRobotTeleop(Plugin):
def __init__(self, context):
super(GroundRobotTeleop, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('GroundRobotTeleop')
# 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._widget = QWidget()
# Get path to UI file which should be in the "resource" folder of this package
ui_file = os.path.join(rospkg.RosPack().get_path(
'rqt_ground_robot_teleop'), 'resource', 'GroundRobotTeleop.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('GroundRobotTeleopUi')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). 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._widget.setWindowTitle(
self._widget.windowTitle() + (' (%d)' % context.serial_number()))
# Add logo
pixmap = QPixmap(os.path.join(rospkg.RosPack().get_path(
'rqt_ground_robot_teleop'), 'resource', 'jderobot.png'))
self._widget.img_logo.setPixmap(pixmap.scaled(121, 121))
# Set Variables
self.linear_velocity_scaling_factor = 1
self.angular_velocity_scaling_factor = 0.5
# Set functions for each GUI Item
self._widget.stop_button.clicked.connect(self.stop_robot)
# Add Publishers
self.twist_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
#Add global variables
self.twist_msg = Twist()
self.stop_icon = QIcon()
self.stop_icon.addPixmap(QPixmap(os.path.join(rospkg.RosPack().get_path(
'rqt_ground_robot_teleop'), 'resource', 'stop.png')), QIcon.Normal, QIcon.Off)
self.play_icon = QIcon()
self.play_icon.addPixmap(QPixmap(os.path.join(rospkg.RosPack().get_path(
'rqt_ground_robot_teleop'), 'resource', 'play.png')), QIcon.Normal, QIcon.Off)
self.bridge = CvBridge()
self.teleop = TeleopWidget(self, 'set_twist', 311)
self._widget.teleop_layout.addWidget(self.teleop)
self.teleop.setVisible(True)
# Add widget to the user interface
context.add_widget(self._widget)
# Add Subscibers
rospy.Subscriber('camera/rgb/image_raw', Image, self.cam_cb)
def msg_to_pixmap(self, msg):
cv_img = self.bridge.imgmsg_to_cv2(msg)
h, w, _ = cv_img.shape
bytesPerLine = 3 * w
q_img = QImage(cv_img.data, w, h, bytesPerLine, QImage.Format_RGB888)
return QPixmap.fromImage(q_img).scaled(320, 240)
def cam_cb(self, msg):
self._widget.img_camera.setPixmap(self.msg_to_pixmap(msg))
def stop_robot(self):
rospy.loginfo('Stopping Robot')
self.teleop.stop()
for i in range(5):
self.twist_msg = Twist()
self.twist_pub.publish(self.twist_msg)
rospy.sleep(0.05)
def set_twist(self, u, v):
az = -self.linear_velocity_scaling_factor * u
x = -self.angular_velocity_scaling_factor * v
self._widget.rot_value.setText('%.2f' % az)
self._widget.x_value.setText('%.2f' % x)
self.twist_msg.linear.x = x
self.twist_msg.angular.z = az
self.twist_pub.publish(self.twist_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
class ConversationViewPlugin(Plugin):
signalAddItem = pyqtSignal(str, int)
def __init__(self, context):
super(ConversationViewPlugin, self).__init__(context)
self.setObjectName('ConversationViewPlugin')
self._widget = QWidget()
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
ui_file = os.path.join(
rospkg.RosPack().get_path('rqt_conversation_view'), 'resource',
'conversation_view.ui')
loadUi(ui_file, self._widget)
context.add_widget(self._widget)
self.send_shortcut = QShortcut(QKeySequence("Ctrl+Return"),
self._widget, self.handle_button_send)
self._widget.buttonSend.clicked.connect(self.handle_button_send)
self._widget.textInput.setFocus()
self.signalAddItem.connect(self.add_item_to_conversation_view)
self.pub_input = rospy.Publisher('recognized_word',
RecognizedWord,
queue_size=10)
rospy.Subscriber('raising_events', RaisingEvents,
self.handle_raising_events)
rospy.Subscriber('reply', Reply, self.handle_reply)
def add_item_to_conversation_view(self, msg, type=0):
label_msg = QLabel(msg)
label_msg.setWordWrap(True)
label_msg.setStyleSheet('font-size:10pt;')
inner_text_layout = QHBoxLayout()
horizonalSpacer1 = QSpacerItem(0, 0, QSizePolicy.MinimumExpanding,
QSizePolicy.MinimumExpanding)
if type == 0:
inner_text_layout.addWidget(label_msg)
inner_text_layout.addItem(horizonalSpacer1)
elif type == 1:
inner_text_layout.addItem(horizonalSpacer1)
inner_text_layout.addWidget(label_msg)
inner_layout = QVBoxLayout()
time_msg = QLabel(str(time.asctime(time.localtime(time.time()))))
time_msg.setStyleSheet('font-size:8pt;')
inner_layout.addItem(inner_text_layout)
inner_layout.addWidget(time_msg)
inner_layout.setSizeConstraint(QLayout.SetFixedSize)
outer_layout = QHBoxLayout()
horizonalSpacer2 = QSpacerItem(0, 0, QSizePolicy.MinimumExpanding,
QSizePolicy.MinimumExpanding)
if type == 0:
label_msg.setStyleSheet(
'background: #e5e5ea; padding: 6px; border-radius: 8px;')
time_msg.setAlignment(Qt.AlignLeft)
outer_layout.addItem(inner_layout)
outer_layout.addItem(horizonalSpacer2)
elif type == 1:
label_msg.setStyleSheet(
'background: #1d86f4; padding: 6px; border-radius: 8px; color:#fff;'
)
time_msg.setAlignment(Qt.AlignRight)
outer_layout.addItem(horizonalSpacer2)
outer_layout.addItem(inner_layout)
outer_layout.setSizeConstraint(QLayout.SetMinimumSize)
widget = QWidget()
widget.setLayout(outer_layout)
widget.resize(widget.sizeHint())
item = QListWidgetItem()
item.setSizeHint(widget.sizeHint())
self._widget.listWidget.addItem(item)
self._widget.listWidget.setItemWidget(item, widget)
self._widget.listWidget.scrollToBottom()
def handle_raising_events(self, msg):
event_text = msg.recognized_word
for event in msg.events:
event_text = event_text + '\n -%s' % event
self.signalAddItem.emit(event_text, 1)
def handle_reply(self, msg):
event_text = msg.reply
self.signalAddItem.emit(event_text, 0)
def handle_button_send(self):
input_text = self._widget.textInput.toPlainText()
if input_text == '':
return
msg = RecognizedWord()
msg.recognized_word = input_text
msg.confidence = 1.0
self.pub_input.publish(msg)
self._widget.textInput.clear()
def shutdown_plugin(self):
pass
def save_settings(self, plugin_settings, instance_settings):
pass
def restore_settings(self, plugin_settings, instance_settings):
pass
class StanUiPlugin(Plugin):
def __init__(self, context):
super(StanUiPlugin, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('StanUiPlugin')
# 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._widget = QWidget()
# Get path to UI file which should be in the "resource" folder of this package
ui_file = os.path.join(rospkg.RosPack().get_path('stan_ui'),
'resource', 'stan_ui_widget.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('StanUiWidget')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). 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._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
self._widget.StartMotorButton.clicked[bool].connect(
self._handle_start_motor_button_clicked)
self.motors_started = False
print("UpdatedUI3")
def _handle_start_motor_button_clicked(self, event):
if self.motors_started:
print("Stopping motors")
self._widget.StartMotorButton.setText("Start Motors")
self.motors_started = False
else:
print("Starting motors")
self._widget.StartMotorButton.setText("Stop Motors")
self.motors_started = True
def shutdown_plugin(self):
# TODO unregister all publishers and stop timers 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
class Simulation(Plugin):
def __init__(self, context):
super(Simulation, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName("AgimusSimulation")
self._tfFrameEditors = []
# Create QWidget
self._widget = QWidget()
self._layout = QVBoxLayout(self._widget)
self.parseArguments(context)
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). 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._widget.setWindowTitle(self._widget.windowTitle() +
(" (%d)" % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
def parseArguments(self, context):
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
parser.add_argument(
"--tf-editor",
type=str,
nargs=2,
action='append',
dest="tf_editors",
metavar=("parent_frame_id", "child_frame_id"),
help="Add a TF frame editor between the parent and child frame.")
parser.add_argument(
"--tf-editor-init",
type=str,
nargs=8,
action='append',
dest="tf_editors_init",
metavar=("parent_frame_id", "child_frame_id", "x", "y", "z", "r",
"p", "y"),
help=
"Add a TF frame editor between the parent and child frame, with an initial value."
)
args, unknowns = parser.parse_known_args(context.argv())
if args.tf_editors is not None:
for parent, child in args.tf_editors:
self.addTfFrameEditor(parent, child)
if args.tf_editors_init is not None:
for parent, child, tx, ty, tz, rr, rp, ry in args.tf_editors_init:
self.addTfFrameEditor(parent, child, [
float(tx),
float(ty),
float(tz),
float(rr),
float(rp),
float(ry)
])
def addTfFrameEditor(self,
parent_frame,
child_frame,
initPos=[0, 0, 0, 0, 0, 0]):
if not hasattr(self, 'tf_static_broadcast'):
self.tf_static_broadcast = tf2_ros.StaticTransformBroadcaster()
editor = _TfFrameEditor(self.tf_static_broadcast, parent_frame,
child_frame, initPos)
self._layout.addWidget(editor)
def space():
spacer = QFrame()
spacer.setFrameShape(QFrame.HLine)
return spacer
self._layout.addWidget(space())
self._tfFrameEditors.append(editor)
class MyPlugin(Plugin):
def __init__(self, context):
super(MyPlugin, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('MyPlugin')
rospy.loginfo("Remora: ECU Console")
# 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._widget = QWidget()
# Get path to UI file which is a sibling of this file
# in this example the .ui and .py file are in the same folder
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'ecu_console.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('MyPluginUi')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). 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.
self._widget.setWindowTitle('ECU')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle()+(' (%d)'% context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
self.firstStatusMsg = False
# Register all subsribers here
self.intTempSub_ = rospy.Subscriber('/sunfish/ecu/int_temp', Temperature, self.callback_IntTemp)
self.extTempSub_ = rospy.Subscriber('/sunfish/ecu/ext_temp', Temperature, self.callback_ExtTemp)
self.depthSub_ = rospy.Subscriber('/sunfish/ecu/depth', Depth, self.callback_Depth)
self.insSub_ = rospy.Subscriber('/sunfish/ecu/INS', INS, self.callback_INS)
self.magFieldSub_ = rospy.Subscriber('/sunfish/ecu/MST', MagField, self.callback_MagField)
self.statusSub_ = rospy.Subscriber('/sunfish/ecu/status', Status, self.callback_Status)
return
def shutdown_plugin(self):
self.intTempSub_.unregister()
self.extTempSub_.unregister()
self.depthSub_.unregister()
self.insSub_.unregister()
self.magFieldSub_.unregister()
self.statusSub_.unregister()
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
# This will enable a setting button (gear icon) in each dock widget title bar
# Usually used to open a modal configuration dialog
def formatFloat(self, f):
return ("%2.2f" % f)
def callback_IntTemp(self, value):
self._widget.intTempData.setText(self.formatFloat(value.temperature))
return
def callback_ExtTemp(self, value):
self._widget.extTempData.setText(self.formatFloat(value.temperature))
return
def callback_Depth(self, value):
self._widget.pressureData.setText(self.formatFloat(value.Pressure))
self._widget.tempData.setText(self.formatFloat(value.Temperature))
return
def callback_INS(self, value):
self._widget.ab_data_0.setText(self.formatFloat(value.AbsOrientation[0]))
self._widget.ab_data_1.setText(self.formatFloat(value.AbsOrientation[1]))
self._widget.ab_data_2.setText(self.formatFloat(value.AbsOrientation[2]))
self._widget.av_Data_0.setText(self.formatFloat(value.AngularVelocity[0]))
self._widget.av_Data_1.setText(self.formatFloat(value.AngularVelocity[1]))
self._widget.av_Data_2.setText(self.formatFloat(value.AngularVelocity[2]))
self._widget.af_data_0.setText(self.formatFloat(value.AccelerationVector[0]))
self._widget.af_data_1.setText(self.formatFloat(value.AccelerationVector[1]))
self._widget.af_data_2.setText(self.formatFloat(value.AccelerationVector[2]))
self._widget.la_Data_0.setText(self.formatFloat(value.LinearAcceleration[0]))
self._widget.la_Data_1.setText(self.formatFloat(value.LinearAcceleration[1]))
self._widget.la_Data_2.setText(self.formatFloat(value.LinearAcceleration[2]))
self._widget.gv_data_0.setText(self.formatFloat(value.GravityVector[0]))
self._widget.gv_data_1.setText(self.formatFloat(value.GravityVector[1]))
self._widget.gv_data_2.setText(self.formatFloat(value.GravityVector[2]))
return
def callback_MagField(self, value):
self._widget.mf_data_0.setText(self.formatFloat(value.MagneticField[0]))
self._widget.mf_data_1.setText(self.formatFloat(value.MagneticField[1]))
self._widget.mf_data_2.setText(self.formatFloat(value.MagneticField[2]))
return
def callback_Status(self, value):
if self.firstStatusMsg == False:
self._widget.hwdData.setText("Hwd: " + value.Hardware)
self._widget.firmwareData.setText("Firmware:" + value.Firmware)
self.firstStatusMsg = True
self._widget.voltData.setText(("%2.1f" % value.Voltage))
self._widget.currentData.setText(("%2.2f" % value.Current))
self._widget.pwm_lcd_0.display(10)
self._widget.pwm_lcd_1.display(10)
self._widget.pwm_lcd_2.display(10)
self._widget.pwm_lcd_3.display(10)
self._widget.pwm_lcd_4.display(50)
self._widget.pwm_lcd_5.display(50)
self._widget.pwm_lcd_6.display(50)
self._widget.pwm_lcd_7.display(50)
self._widget.pwm_lcd_8.display(100)
self._widget.pwm_lcd_9.display(100)
self._widget.pwm_lcd_10.display(100)
self._widget.pwm_lcd_11.display(100)
return
class SeeScopeControl(Plugin):
"""
Graphical frontend for a C{JointTrajectoryController}.
There are two modes for interacting with a controller:
1. B{Monitor mode} Joint displays are updated with the state reported
by the controller. This is a read-only mode and does I{not} send
control commands. Every time a new controller is selected, it starts
in monitor mode for safety reasons.
2. B{Control mode} Joint displays update the control command that is
sent to the controller. Commands are sent periodically evan if the
displays are not being updated by the user.
To control the aggressiveness of the motions, the maximum speed of the
sent commands can be scaled down using the C{Speed scaling control}
This plugin is able to detect and keep track of all active controller
managers, as well as the JointTrajectoryControllers that are I{running}
in each controller manager.
For a controller to be compatible with this plugin, it must comply with
the following requisites:
- The controller type contains the C{JointTrajectoryController}
substring, e.g., C{position_controllers/JointTrajectoryController}
- The controller exposes the C{command} and C{state} topics in its
ROS interface.
Additionally, there must be a URDF loaded with a valid joint limit
specification, namely position (when applicable) and velocity limits.
A reference implementation of the C{JointTrajectoryController} is
available in the C{joint_trajectory_controller} ROS package.
"""
def __init__(self, context):
super(SeeScopeControl, self).__init__(context)
self.setObjectName('SeeScopeControl')
# Create QWidget and extend it with all the attributes and children
# from the UI file
self._widget = QWidget()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_see_scope_control'),
'resource',
'see_scope_control.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('SeeScopeControlUi')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
cmd_topic = '/see_scope/projector/command'
self._cmd_pub = rospy.Publisher(cmd_topic,
String,
queue_size=1)
w = self._widget
w.enable_button.toggled.connect(self._on_ssc_enabled)
def _enable_fringes(self):
self._cmd_pub.publish(String("fringes"))
def _disable_fringes(self):
self._cmd_pub.publish(String("white"))
def _on_ssc_enabled(self, val):
if val:
self._enable_fringes();
else:
self._disable_fringes();
class ControllerManager(Plugin):
"""
Graphical frontend for managing ros_control controllers.
"""
_cm_update_freq = 1 # Hz
def __init__(self, context):
super(ControllerManager, self).__init__(context)
self.setObjectName('ControllerManager')
# Create QWidget and extend it with all the attributes and children
# from the UI file
self._widget = QWidget()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_controller_manager'),
'resource',
'controller_manager.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('ControllerManagerUi')
# Pop-up that displays controller information
self._popup_widget = QWidget()
ui_file = os.path.join(rp.get_path('rqt_controller_manager'),
'resource',
'controller_info.ui')
loadUi(ui_file, self._popup_widget)
self._popup_widget.setObjectName('ControllerInfoUi')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). 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._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
# Initialize members
self._cm_ns = [] # Namespace of the selected controller manager
self._controllers = [] # State of each controller
self._table_model = None
self._controller_lister = None
# Controller manager service proxies
self._load_srv = None
self._unload_srv = None
self._switch_srv = None
# Controller state icons
rospack = rospkg.RosPack()
path = rospack.get_path('rqt_controller_manager')
self._icons = {'running': QIcon(path + '/resource/led_green.png'),
'stopped': QIcon(path + '/resource/led_red.png'),
'uninitialized': QIcon(path + '/resource/led_off.png')}
# Controllers display
table_view = self._widget.table_view
table_view.setContextMenuPolicy(Qt.CustomContextMenu)
table_view.customContextMenuRequested.connect(self._on_ctrl_menu)
table_view.doubleClicked.connect(self._on_ctrl_info)
header = table_view.horizontalHeader()
header.setSectionResizeMode(QHeaderView.ResizeToContents)
header.setContextMenuPolicy(Qt.CustomContextMenu)
header.customContextMenuRequested.connect(self._on_header_menu)
# Timer for controller manager updates
self._list_cm = ControllerManagerLister()
self._update_cm_list_timer = QTimer(self)
self._update_cm_list_timer.setInterval(1000.0 /
self._cm_update_freq)
self._update_cm_list_timer.timeout.connect(self._update_cm_list)
self._update_cm_list_timer.start()
# Timer for running controller updates
self._update_ctrl_list_timer = QTimer(self)
self._update_ctrl_list_timer.setInterval(1000.0 /
self._cm_update_freq)
self._update_ctrl_list_timer.timeout.connect(self._update_controllers)
self._update_ctrl_list_timer.start()
# Signal connections
w = self._widget
w.cm_combo.currentIndexChanged[str].connect(self._on_cm_change)
def shutdown_plugin(self):
self._update_cm_list_timer.stop()
self._update_ctrl_list_timer.stop()
self._popup_widget.hide()
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('cm_ns', self._cm_ns)
def restore_settings(self, plugin_settings, instance_settings):
# Restore last session's controller_manager, if present
self._update_cm_list()
cm_ns = instance_settings.value('cm_ns')
cm_combo = self._widget.cm_combo
cm_list = [cm_combo.itemText(i) for i in range(cm_combo.count())]
try:
idx = cm_list.index(cm_ns)
cm_combo.setCurrentIndex(idx)
except (ValueError):
pass
# def trigger_configuration(self):
# Comment in to signal that the plugin has a way to configure
# This will enable a setting button (gear icon) in each dock widget
# title bar
# Usually used to open a modal configuration dialog
def _update_cm_list(self):
update_combo(self._widget.cm_combo, self._list_cm())
def _on_cm_change(self, cm_ns):
self._cm_ns = cm_ns
# Setup services for communicating with the selected controller manager
self._set_cm_services(cm_ns)
# Controller lister for the selected controller manager
if cm_ns:
self._controller_lister = ControllerLister(cm_ns)
self._update_controllers()
else:
self._controller_lister = None
def _set_cm_services(self, cm_ns):
if cm_ns:
# NOTE: Persistent services are used for performance reasons.
# If the controller manager dies, we detect it and disconnect from
# it anyway
load_srv_name = _append_ns(cm_ns, 'load_controller')
self._load_srv = rospy.ServiceProxy(load_srv_name,
LoadController,
persistent=True)
unload_srv_name = _append_ns(cm_ns, 'unload_controller')
self._unload_srv = rospy.ServiceProxy(unload_srv_name,
UnloadController,
persistent=True)
switch_srv_name = _append_ns(cm_ns, 'switch_controller')
self._switch_srv = rospy.ServiceProxy(switch_srv_name,
SwitchController,
persistent=True)
else:
self._load_srv = None
self._unload_srv = None
self._switch_srv = None
def _update_controllers(self):
# Find controllers associated to the selected controller manager
controllers = self._list_controllers()
# Update controller display, if necessary
if self._controllers != controllers:
self._controllers = controllers
self._show_controllers() # NOTE: Model is recomputed from scratch
def _list_controllers(self):
"""
@return List of controllers associated to a controller manager
namespace. Contains both stopped/running controllers, as returned by
the C{list_controllers} service, plus uninitialized controllers with
configurations loaded in the parameter server.
@rtype [str]
"""
if not self._cm_ns:
return []
# Add loaded controllers first
controllers = self._controller_lister()
# Append potential controller configs found in the parameter server
all_ctrls_ns = _resolve_controllers_ns(self._cm_ns)
for name in get_rosparam_controller_names(all_ctrls_ns):
add_ctrl = not any(name == ctrl.name for ctrl in controllers)
if add_ctrl:
type_str = _rosparam_controller_type(all_ctrls_ns, name)
uninit_ctrl = ControllerState(name=name,
type=type_str,
state='uninitialized')
controllers.append(uninit_ctrl)
return controllers
def _show_controllers(self):
table_view = self._widget.table_view
self._table_model = ControllerTable(self._controllers, self._icons)
table_view.setModel(self._table_model)
def _on_ctrl_menu(self, pos):
# Get data of selected controller
row = self._widget.table_view.rowAt(pos.y())
if row < 0:
return # Cursor is not under a valid item
ctrl = self._controllers[row]
# Show context menu
menu = QMenu(self._widget.table_view)
if ctrl.state == 'running':
action_stop = menu.addAction(self._icons['stopped'], 'Stop')
action_kill = menu.addAction(self._icons['uninitialized'],
'Stop and Unload')
elif ctrl.state == 'stopped':
action_start = menu.addAction(self._icons['running'], 'Start')
action_unload = menu.addAction(self._icons['uninitialized'],
'Unload')
elif ctrl.state == 'uninitialized':
action_load = menu.addAction(self._icons['stopped'], 'Load')
action_spawn = menu.addAction(self._icons['running'],
'Load and Start')
action = menu.exec_(self._widget.table_view.mapToGlobal(pos))
# Evaluate user action
if ctrl.state == 'running':
if action is action_stop:
self._stop_controller(ctrl.name)
elif action is action_kill:
self._stop_controller(ctrl.name)
self._unload_controller(ctrl.name)
elif ctrl.state == 'stopped':
if action is action_start:
self._start_controller(ctrl.name)
elif action is action_unload:
self._unload_controller(ctrl.name)
elif ctrl.state == 'uninitialized':
if action is action_load:
self._load_controller(ctrl.name)
if action is action_spawn:
self._load_controller(ctrl.name)
self._start_controller(ctrl.name)
def _on_ctrl_info(self, index):
popup = self._popup_widget
ctrl = self._controllers[index.row()]
popup.ctrl_name.setText(ctrl.name)
popup.ctrl_type.setText(ctrl.type)
res_model = QStandardItemModel()
model_root = QStandardItem('Claimed Resources')
res_model.appendRow(model_root)
for hw_res in ctrl.claimed_resources:
hw_iface_item = QStandardItem(hw_res.hardware_interface)
model_root.appendRow(hw_iface_item)
for res in hw_res.resources:
res_item = QStandardItem(res)
hw_iface_item.appendRow(res_item)
popup.resource_tree.setModel(res_model)
popup.resource_tree.setItemDelegate(FontDelegate(popup.resource_tree))
popup.resource_tree.expandAll()
popup.move(QCursor.pos())
popup.show()
def _on_header_menu(self, pos):
header = self._widget.table_view.horizontalHeader()
# Show context menu
menu = QMenu(self._widget.table_view)
action_toggle_auto_resize = menu.addAction('Toggle Auto-Resize')
action = menu.exec_(header.mapToGlobal(pos))
# Evaluate user action
if action is action_toggle_auto_resize:
if header.resizeMode(0) == QHeaderView.ResizeToContents:
header.setSectionResizeMode(QHeaderView.Interactive)
else:
header.setSectionResizeMode(QHeaderView.ResizeToContents)
def _load_controller(self, name):
self._load_srv.call(LoadControllerRequest(name=name))
def _unload_controller(self, name):
self._unload_srv.call(UnloadControllerRequest(name=name))
def _start_controller(self, name):
strict = SwitchControllerRequest.STRICT
req = SwitchControllerRequest(start_controllers=[name],
stop_controllers=[],
strictness=strict)
self._switch_srv.call(req)
def _stop_controller(self, name):
strict = SwitchControllerRequest.STRICT
req = SwitchControllerRequest(start_controllers=[],
stop_controllers=[name],
strictness=strict)
self._switch_srv.call(req)
class ExperimentGUI(Plugin):
def __init__(self, context):
super(ExperimentGUI, self).__init__(context)
# Give QObjects reasonable names
self.plans = [
'Starting Position', 'Above Panel', 'Panel Grabbed',
'Above Placement Nest', 'Panel Placed'
]
#state_dict ties each state to planlistindex values
#self.state_dict={'reset_position':0,'pickup_prepare':1,'pickup_lower':2,'pickup_grab_first_step':2,'pickup_grab_second_step':2,'pickup_raise':2,'transport_panel':3,'place_lower':4,'place_set_first_step':4,'place_set_second_step':4,'place_raise':4}
self.gui_execute_states = [
"reset", "panel_pickup", "pickup_grab", "transport_panel",
"place_panel"
]
self.execute_states = [
['plan_to_reset_position', 'move_to_reset_position'],
['plan_pickup_prepare', 'move_pickup_prepare'],
[
'plan_pickup_lower', 'move_pickup_lower',
'plan_pickup_grab_first_step', 'move_pickup_grab_first_step',
'plan_pickup_grab_second_step', 'move_pickup_grab_second_step',
'plan_pickup_raise', 'move_pickup_raise'
], ['plan_transport_payload', 'move_transport_payload'],
['plan_place_set_second_step']
]
self.teleop_modes = [
'Error', 'Off', 'Joint', 'Cartesian', 'Cylindrical', 'Spherical'
]
self.current_teleop_mode = 1
self.teleop_button_string = "Tele-op\nMode:\n"
self.setObjectName('MyPlugin')
self._lock = threading.Lock()
#self._send_event=threading.Event()
self.controller_commander = controller_commander_pkg.ControllerCommander(
)
# 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.in_process = None
self.recover_from_pause = False
#rospy.get_param("rosbag_name")
#<param name="start_time" command="date +'%d-%m-%Y_%Ih%Mm%S'"/>
#rosbag record args="record -O arg('start_time')
self._mainwidget = QWidget()
self.layout = QGridLayout()
self._mainwidget.setLayout(self.layout)
self.disconnectreturnoption = False
self.stackedWidget = QStackedWidget() #self._mainwidget)
self.layout.addWidget(self.stackedWidget, 0, 0)
self._welcomescreen = QWidget()
self._runscreen = QWidget()
self._errordiagnosticscreen = QWidget()
self.stackedWidget.addWidget(self._welcomescreen)
self.stackedWidget.addWidget(self._runscreen)
self.stackedWidget.addWidget(self._errordiagnosticscreen)
#self._data_array=collections.deque(maxlen=500)
self._proxy_model = message_proxy_model.MessageProxyModel()
self._rospack = rospkg.RosPack()
#self.console=console_widget.ConsoleWidget(self._proxy_model,self._rospack)
self.robotconnectionled = LEDIndicator()
self.robotconnectionled.setDisabled(True) # Make the led non clickable
self.forcetorqueled = LEDIndicator()
self.forcetorqueled.setDisabled(True) # Make the led non clickable
self.overheadcameraled = LEDIndicator()
self.overheadcameraled.setDisabled(True) # Make the led non clickable
self.grippercameraled = LEDIndicator()
self.grippercameraled.setDisabled(True) # Make the led non clickable
self.runscreenstatusled = LEDIndicator()
self.runscreenstatusled.setDisabled(True)
self.step_executor = GUI_Step_Executor()
self.step_executor.error_signal.connect(self._feedback_receive)
#self.step_executor.error_function=self._feedback_receive
#Need this to pause motions
self.process_client = actionlib.ActionClient('process_step',
ProcessStepAction)
self.process_client.wait_for_server()
self.placement_targets = {
'leeward_mid_panel': 'panel_nest_leeward_mid_panel_target',
'leeward_tip_panel': 'panel_nest_leeward_tip_panel_target'
}
self.placement_target = 'panel_nest_leeward_mid_panel_target'
self.panel_type = 'leeward_mid_panel'
self.client_handle = None
service_list = rosservice.get_service_list()
if ('/overhead_camera/trigger' in service_list):
self.led_change(self.overheadcameraled, True)
else:
self.led_change(self.overheadcameraled, False)
if ('/gripper_camera_2/trigger' in service_list):
self.led_change(self.grippercameraled, True)
else:
self.led_change(self.grippercameraled, False)
self.mode = 0
#self.rewound=False
self.count = 0
self.data_count = 0
self.force_torque_data = np.zeros((6, 1))
self.joint_angle_data = np.zeros((6, 1))
# Get path to UI file which should be in the "resource" folder of this package
self.welcomescreenui = os.path.join(
rospkg.RosPack().get_path('rpi_arm_composites_manufacturing_gui'),
'resource', 'welcomeconnectionscreen.ui')
self.runscreenui = os.path.join(
rospkg.RosPack().get_path('rpi_arm_composites_manufacturing_gui'),
'resource', 'Runscreen.ui')
self.errorscreenui = os.path.join(
rospkg.RosPack().get_path('rpi_arm_composites_manufacturing_gui'),
'resource', 'errordiagnosticscreen.ui')
self.rewound = False
# Extend the widget with all attributes and children from UI file
loadUi(self.welcomescreenui, self._welcomescreen)
loadUi(self.runscreenui, self._runscreen)
loadUi(self.errorscreenui, self._errordiagnosticscreen)
# Give QObjects reasonable names
self._mainwidget.setObjectName('MyPluginUi')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). 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._mainwidget.setWindowTitle(self._mainwidget.windowTitle() +
(' (%d)' %
context.serial_number()))
context.add_widget(self._mainwidget)
self.context = context
self.plugin_settings = None
self.instance_settings = None
#self._errordiagnosticscreen.consoleWidget=console_widget.ConsoleWidget(self._proxy_model,self._rospack)
#####consoleiagnosticscreen.backToRun.pressed.connect(self._to_run_screen)
#self._runscThread=ConsoleThread(self._widget.State_info)
# self._welcomescreen.statusFormLayout.takeAt(0)
self._welcomescreen.statusFormLayout.addWidget(self.robotconnectionled,
0, 0)
self._welcomescreen.statusFormLayout.addWidget(self.forcetorqueled, 2,
0)
self._welcomescreen.statusFormLayout.addWidget(self.overheadcameraled,
4, 0)
self._welcomescreen.statusFormLayout.addWidget(self.grippercameraled,
6, 0)
self._runscreen.connectionLayout.addWidget(self.runscreenstatusled, 0,
1)
#self._welcomescreen.robotConnectionWidget.addWidget(self.led)
#consoleThread.finished.connect(app.exit)
#####consoleThread.start()
self.rviz_starter = os.path.join(
rospkg.RosPack().get_path('rpi_arm_composites_manufacturing_gui'),
'src', 'rpi_arm_composites_manufacturing_gui', 'rviz_starter.py')
# Add widget to the user interface
#context.add_widget(console)==QDialog.Accepted
#context.add_widget(rqt_console)
for entry in self.plans:
listentry = QListWidgetItem(entry)
listentry.setFlags(Qt.ItemIsSelectable)
self._runscreen.planList.addItem(listentry)
self._runscreen.planList.item(0).setSelected(True)
self.planListIndex = 0
self._runscreen.panelType.setText(self.panel_type)
self._runscreen.placementNestTarget.setText("Leeward Mid Panel Nest")
self._runscreen.panelType.setReadOnly(True)
self._runscreen.placementNestTarget.setReadOnly(True)
self.commands_sent = False
rospy.Subscriber("controller_state", controllerstate, self.callback)
self._set_controller_mode = rospy.ServiceProxy("set_controller_mode",
SetControllerMode)
rospy.Subscriber("GUI_state", ProcessState, self.process_state_set)
#rospy.Subscriber('gui_error', String, self._feedback_receive())
self.force_torque_plot_widget = QWidget()
self.joint_angle_plot_widget = QWidget()
self._welcomescreen.openConfig.clicked.connect(
self._open_config_options)
#self._welcomescreen.openAdvancedOptions.pressed.connect(self._open_login_prompt)
self._welcomescreen.toRunScreen.pressed.connect(self._to_run_screen)
self._runscreen.backToWelcome.pressed.connect(self._to_welcome_screen)
#self._runscreen.toErrorScreen.pressed.connect(self._to_error_screen)
self._runscreen.nextPlan.pressed.connect(self._next_plan)
self._runscreen.previousPlan.pressed.connect(self._previousPlan)
self._runscreen.resetToHome.pressed.connect(self._reset_position)
self._runscreen.stopPlan.pressed.connect(self._stopPlan)
self._runscreen.accessTeleop.pressed.connect(self.change_teleop_modes)
#self._errordiagnosticscreen.openOverheadCameraView.pressed.connect(self._open_overhead_camera_view)
#self._errordiagnosticscreen.openGripperCameraViews.pressed.connect(self._open_gripper_camera_views)
self._errordiagnosticscreen.openForceTorqueDataPlot.pressed.connect(
self._open_force_torque_data_plot)
self._errordiagnosticscreen.openJointAngleDataPlot.pressed.connect(
self._open_joint_angle_data_plot)
self._errordiagnosticscreen.backToRun.pressed.connect(
self._to_run_screen)
#self._runscreen.widget.frame=rviz.VisualizationFrame()
#self._runscreen.widget.frame.setSplashPath( "" )
## VisualizationFrame.initialize() must be called before
## VisualizationFrame.load(). In fact it must be called
## before most interactions with RViz classes because it
## instantiates and initializes the VisualizationManager,
## which is the central class of RViz.
#self._runscreen.widget.frame.initialize()
#self.manager = self._runscreen.widget.frame.getManager()
# self._welcomescreen.openAdvancedOptions.pressed.connect(self._open_advanced_options)
def led_change(self, led, state):
led.setChecked(state)
def _to_welcome_screen(self):
self.stackedWidget.setCurrentIndex(0)
def _to_run_screen(self):
self.controller_commander.set_controller_mode(
self.controller_commander.MODE_HALT, 1, [], [])
if (self.stackedWidget.currentIndex() == 0):
self.messagewindow = PanelSelectorWindow()
self.messagewindow.show()
self.messagewindow.setFixedSize(self.messagewindow.size())
if self.messagewindow.exec_():
next_selected_panel = self.messagewindow.get_panel_selected()
if (next_selected_panel != None):
self.panel_type = next_selected_panel
self.placement_target = self.placement_targets[
self.panel_type]
self.stackedWidget.setCurrentIndex(1)
self._runscreen.panelType.setText(self.panel_type)
if (self.panel_type == 'leeward_mid_panel'):
self._runscreen.placementNestTarget.setText(
"Leeward Mid Panel Nest")
elif (self.panel_type == 'leeward_tip_panel'):
self._runscreen.placementNestTarget.setText(
"Leeward Tip Panel Nest")
else:
raise Exception('Unknown panel type selected')
def _to_error_screen(self):
self.stackedWidget.setCurrentIndex(2)
def _login_prompt(self):
self.loginprompt = UserAuthenticationWindow()
if self.loginprompt.exec_():
#self.loginprompt.show()
#while(not self.loginprompt.returned):
#pass
return True
else:
return False
def _open_config_options(self):
if (self._login_prompt()):
self.led_change(self.robotconnectionled, True)
#def _open_overhead_camera_view(self):
#def _open_gripper_camera_views(self):
def _open_force_torque_data_plot(self):
self.plot_container = []
self.x_data = np.arange(1)
self.force_torque_app = QApplication([])
self.force_torque_plot_widget = pg.plot()
self.force_torque_plot_widget.addLegend()
#self.layout.addWidget(self.force_torque_plot_widget,0,1)
self.force_torque_plot_widget.showGrid(x=True, y=True)
self.force_torque_plot_widget.setLabel('left', 'Force/Torque', 'N/N*m')
self.force_torque_plot_widget.setLabel('bottom', 'Sample Number', 'n')
self.plot_container.append(
self.force_torque_plot_widget.plot(pen=(255, 0, 0),
name="Torque X"))
self.plot_container.append(
self.force_torque_plot_widget.plot(pen=(0, 255, 0),
name="Torque Y"))
self.plot_container.append(
self.force_torque_plot_widget.plot(pen=(0, 0, 255),
name="Torque Z"))
self.plot_container.append(
self.force_torque_plot_widget.plot(pen=(255, 255, 0),
name="Force X"))
self.plot_container.append(
self.force_torque_plot_widget.plot(pen=(0, 255, 255),
name="Force Y"))
self.plot_container.append(
self.force_torque_plot_widget.plot(pen=(255, 0, 255),
name="Force Z"))
#self.force_torque_plotter=PlotManager(title='Force Torque Data',nline=3,widget=self.force_torque_plot_widget)
#self.force_torque_plot_widget.show()
#self.force_torque_plotter.add("Hello", np.arange(10))
#self.force_torque_plotter.update()
#self.rosGraph.show()
#self.rosGraph.exec_()
def _open_joint_angle_data_plot(self):
self.plot_container = []
self.x_data = np.arange(1)
self.joint_angle_app = QApplication([])
self.joint_angle_plot_widget = pg.plot()
self.joint_angle_plot_widget.addLegend()
#self.layout.addWidget(self.joint_angle_plot_widget,0,1)
self.joint_angle_plot_widget.showGrid(x=True, y=True)
self.joint_angle_plot_widget.setLabel('left', 'Force/Torque', 'N/N*m')
self.joint_angle_plot_widget.setLabel('bottom', 'Sample Number', 'n')
self.plot_container.append(
self.joint_angle_plot_widget.plot(pen=(255, 0, 0), name="Joint 1"))
self.plot_container.append(
self.joint_angle_plot_widget.plot(pen=(0, 255, 0), name="Joint 2"))
self.plot_container.append(
self.joint_angle_plot_widget.plot(pen=(0, 0, 255), name="Joint 3"))
self.plot_container.append(
self.joint_angle_plot_widget.plot(pen=(255, 255, 0),
name="Joint 4"))
self.plot_container.append(
self.joint_angle_plot_widget.plot(pen=(0, 255, 255),
name="Joint 5"))
self.plot_container.append(
self.joint_angle_plot_widget.plot(pen=(255, 0, 255),
name="Joint 6"))
self.plot_container.append(
self.joint_angle_plot_widget.plot(pen=(255, 255, 255),
name="Joint 7"))
def _open_rviz_prompt(self):
subprocess.Popen(['python', self.rviz_starter])
# def _open_advanced_options(self):
# main = Main()
# sys.exit(main.main(sys.argv, standalone='rqt_rviz/RViz', plugin_argument_provider=add_arguments))
def _raise_rviz_window(self):
subprocess.call(["xdotool", "search", "--name", "rviz", "windowraise"])
def _next_plan(self):
#TODO: Disable previous plan if planListIndex==2 or 4
self._runscreen.nextPlan.setDisabled(True)
self._runscreen.previousPlan.setDisabled(True)
self._runscreen.resetToHome.setDisabled(True)
self.reset_teleop_button()
#TODO Make it change color when in motion
if (self.planListIndex + 1 == self._runscreen.planList.count()):
self.planListIndex = 0
elif (self.recover_from_pause):
self.recover_from_pause = False
else:
self.planListIndex += 1
#g=GUIStepGoal(self.gui_execute_states[self.planListIndex], self.panel_type)
#self.client_handle=self.client.send_goal(g,done_cb=self._process_done,feedback_cb=self._feedback_receive)
self.step_executor._nextPlan(self.panel_type, self.planListIndex,
self.placement_target)
self._runscreen.planList.item(self.planListIndex).setSelected(True)
if (self.rewound):
self.rewound = False
self._runscreen.previousPlan.setDisabled(False)
"""
self._runscreen.vacuum.setText("OFF")
self._runscreen.panel.setText("Detached")
self._runscreen.panelTag.setText("Not Localized")
self._runscreen.nestTag.setText("Not Localized")
self._runscreen.overheadCamera.setText("OFF")
self._runscreen.gripperCamera.setText("OFF")
self._runscreen.forceSensor.setText("Biased to 0")
self._runscreen.pressureSensor.setText("[0,0,0]")
"""
'''
elif(self.planListIndex==1):
self.send_thread=threading.Thread(target=self._execute_steps,args=(1,self.last_step, self.panel_type,0))
rospy.loginfo("thread_started")
self.send_thread.setDaemon(True)
self.send_thread.start()
self._send_event.set()
#self._execute_step('plan_pickup_prepare',self.panel_type)
#self._execute_step('move_pickup_prepare')''' """
self._runscreen.vacuum.setText("OFF")
self._runscreen.panel.setText("Detached")
self._runscreen.panelTag.setText("Localized")
self._runscreen.nestTag.setText("Not Localized")
self._runscreen.overheadCamera.setText("ON")
self._runscreen.gripperCamera.setText("OFF")
self._runscreen.forceSensor.setText("ON")
self._runscreen.pressureSensor.setText("[0,0,0]")
""" """
elif(self.planListIndex==2):
self.send_thread=threading.Thread(target=self._execute_steps,args=(2,self.last_step))
self.send_thread.setDaemon(True)
self.send_thread.start()
self._send_event.set()""" """
self._execute_step('plan_pickup_lower')
self._execute_step('move_pickup_lower')
self._execute_step('plan_pickup_grab_first_step')
self._execute_step('move_pickup_grab_first_step')
self._execute_step('plan_pickup_grab_second_step')
self._execute_step('move_pickup_grab_second_step')
self._execute_step('plan_pickup_raise')
self._execute_step('move_pickup_raise')
self._runscreen.vacuum.setText("OFF")
self._runscreen.panel.setText("Detached")
self._runscreen.panelTag.setText("Localized")self.controller_commander=controller_commander_pkg.arm_composites_manufacturing_controller_commander()
self._runscreen.nestTag.setText("Not Localized")
self._runscreen.overheadCamera.setText("OFF")
self._runscreen.gripperCamera.setText("OFF")
self._runscreen.forceSensor.setText("ON")
self._runscreen.pressureSensor.setText("[0,0,0]")
""" """
elif(self.planListIndex==3):
if(self.panel_type=="leeward_mid_panel"):
subprocess.Popen(['python', self.YC_transport_code, 'leeward_mid_panel'])
elif(self.panel_type=="leeward_tip_panel"):
subprocess.Popen(['python', self.YC_transport_code, 'leeward_tip_panel'])
self.commands_sent=True
"""
#self.send_thread=threading.Thread(target=self._execute_steps,args=(3,self.last_step,self.placement_target,0))
#self.send_thread.setDaemon(True)
#self.send_thread.start()
#self._send_event.set()"""
"""
self._execute_step('plan_transport_payload',self.placement_target)
self._execute_step('move_transport_payload')
self._runscreen.vacuum.setText("ON")
self._runscreen.panel.setText("Attached")
self._runscreen.panelTag.setText("Localized")
self._runscreen.nestTag.setText("Not Localized")
self._runscreen.overheadCamera.setText("OFF")
self._runscreen.gripperCamera.setText("OFF")
self._runscreen.forceSensor.setText("ON")
self._runscreen.pressureSensor.setText("[1,1,1]")
""" """
elif(self.planListIndex==4):
if(self.panel_type=="leeward_mid_panel"):
subprocess.Popen(['python', self.YC_place_code])
elif(self.panel_type=="leeward_tip_panel"):
subprocess.Popen(['python', self.YC_place_code2])
self.commands_sent=True
""" """
self._runscreen.vacuum.setText("ON")
self._runscreen.panel.setText("Attached")
self._runscreen.panelTag.setText("Localized")
self._runscreen.nestTag.setText("Not Localized")
self._runscreen.overheadCamera.setText("OFF")
self._runscreen.gripperCamera.setText("OFF")
self._runscreen.forceSensor.setText("OFF")
self._runscreen.pressureSensor.setText("[1,1,1]")
"""
def _stopPlan(self):
#self.client.cancel_all_goals()
#self.process_client.cancel_all_goals()
#g=GUIStepGoal("stop_plan", self.panel_type)
#self.client_handle=self.client.send_goal(g,feedback_cb=self._feedback_receive)
self.step_executor._stopPlan()
self.recover_from_pause = True
self._runscreen.nextPlan.setDisabled(False)
self._runscreen.previousPlan.setDisabled(False)
self._runscreen.resetToHome.setDisabled(False)
self.reset_teleop_button()
def _previousPlan(self):
if (self.planListIndex == 0):
self.planListIndex = self._runscreen.planList.count() - 1
else:
self.planListIndex -= 1
self.reset_teleop_button()
self._runscreen.planList.item(self.planListIndex).setSelected(True)
# self.rewound=True
#self._runscreen.previousPlan.setDisabled(True)
#g=GUIStepGoal("previous_plan", self.panel_type)
#self.client_handle=self.client.send_goal(g,feedback_cb=self._feedback_receive,done_cb=self._process_done)
self.step_executor._previousPlan()
@pyqtSlot()
def _feedback_receive(self):
with self._lock:
messagewindow = VacuumConfirm()
error_msg = self.step_executor.error
confirm = QMessageBox.warning(
messagewindow, 'Error',
'Operation failed with error:\n' + error_msg, QMessageBox.Ok,
QMessageBox.Ok)
self._runscreen.nextPlan.setDisabled(False)
self._runscreen.previousPlan.setDisabled(False)
self._runscreen.resetToHome.setDisabled(False)
def process_state_set(self, data):
#if(data.state!="moving"):
self._runscreen.nextPlan.setDisabled(False)
self._runscreen.previousPlan.setDisabled(False)
self._runscreen.resetToHome.setDisabled(False)
def _reset_position(self):
messagewindow = VacuumConfirm()
reply = QMessageBox.question(messagewindow, 'Path Verification',
'Proceed to Reset Position',
QMessageBox.Yes | QMessageBox.No,
QMessageBox.No)
if reply == QMessageBox.Yes:
self.planListIndex = 0
#g=GUIStepGoal("reset", self.panel_type)
#self.client_handle=self.client.send_goal(g,feedback_cb=self._feedback_receive)
self.reset_teleop_button()
self.step_executor._nextPlan(None, self.planListIndex)
self._runscreen.planList.item(self.planListIndex).setSelected(True)
#subprocess.Popen(['python', self.reset_code])
else:
rospy.loginfo("Reset Rejected")
def start_shared_control(self):
if (self.planListIndex + 1 == self._runscreen.planList.count()):
self.planListIndex = 0
elif self.recover_from_pause:
self.recover_from_pause = False
else:
self.planListIndex += 1
if (self.planListIndex == 1):
self._execute_step('plan_pickup_prepare', self.panel_type)
elif (self.planListIndex == 2):
self._execute_step('plan_pickup_lower')
elif (self.planListIndex == 3):
self._execute_step('plan_pickup_grab_first_step')
#TODO: How to handle what happens after threshold exceeded to generate next plan step
elif (self.planListIndex == 4):
self._execute_step('plan_pickup_raise')
elif (self.planListIndex == 5):
self._execute_step('plan_transport_payload', self.placement_target)
#self.set_controller_mode(self.controller_commander.MODE_SHARED_TRAJECTORY, 1, [],[])
def change_teleop_modes(self):
with self._lock:
self.current_teleop_mode += 1
try:
if (self.current_teleop_mode == len(self.teleop_modes)):
self.current_teleop_mode = 1
self.controller_commander.set_controller_mode(
self.controller_commander.MODE_HALT, 1, [], [])
elif (self.current_teleop_mode == 1):
self.reset_teleop_button()
elif (self.current_teleop_mode == 2):
self.controller_commander.set_controller_mode(
self.controller_commander.MODE_JOINT_TELEOP, 1, [], [])
elif (self.current_teleop_mode == 3):
self.controller_commander.set_controller_mode(
self.controller_commander.MODE_CARTESIAN_TELEOP, 1, [],
[])
elif (self.current_teleop_mode == 4):
self.controller_commander.set_controller_mode(
self.controller_commander.MODE_CYLINDRICAL_TELEOP, 1,
[], [])
elif (self.current_teleop_mode == 5):
self.controller_commander.set_controller_mode(
self.controller_commander.MODE_SPHERICAL_TELEOP, 1, [],
[])
rospy.loginfo("Entering teleop mode:" +
self.teleop_modes[self.current_teleop_mode])
button_string = self.teleop_button_string + self.teleop_modes[
self.current_teleop_mode]
self._runscreen.accessTeleop.setText(button_string)
except Exception as err:
rospy.loginfo(str(err))
self.step_executor.error = "Controller failed to set teleop mode"
self.step_executor.error_signal.emit()
def set_controller_mode(self,
mode,
speed_scalar=1.0,
ft_bias=[],
ft_threshold=[]):
req = SetControllerModeRequest()
req.mode.mode = mode
req.speed_scalar = speed_scalar
req.ft_bias = ft_bias
req.ft_stop_threshold = ft_threshold
res = self._set_controller_mode(req)
if (res.error_code.mode != ControllerMode.MODE_SUCCESS):
raise Exception("Could not set controller mode")
def error_recovery_button(self):
self.current_teleop_mode = 0
self._runscreen.accessTeleop.setText("Recover from Error")
def reset_teleop_button(self):
self.current_teleop_mode = 1
self.controller_commander.set_controller_mode(
self.controller_commander.MODE_HALT, 1, [], [])
button_string = self.teleop_button_string + self.teleop_modes[
self.current_teleop_mode]
self._runscreen.accessTeleop.setText(button_string)
def callback(self, data):
#self._widget.State_info.append(data.mode)
with self._lock:
if (self.stackedWidget.currentIndex() == 2):
if (self.count > 10):
#stringdata=str(data.mode)
#freeBytes.acquire()
#####consoleData.append(str(data.mode))
self._errordiagnosticscreen.consoleWidget_2.addItem(
str(data.joint_position))
self.count = 0
#print data.joint_position
self.count += 1
#self._widget.State_info.scrollToBottom()
#usedBytes.release()
#self._data_array.append(stringdata)
#print self._widget.State_info.count()
if (self._errordiagnosticscreen.consoleWidget_2.count() > 200):
item = self._errordiagnosticscreen.consoleWidget_2.takeItem(
0)
#print "Hello Im maxed out"
del item
'''
if self.in_process:
if self.client.get_state() == actionlib.GoalStatus.PENDING or self.client.get_state() == actionlib.GoalStatus.ACTIVE:
self._runscreen.nextPlan.setDisabled(True)
self._runscreen.previousPlan.setDisabled(True)
self._runscreen.resetToHome.setDisabled(True)
rospy.loginfo("Pending")
elif self.client.get_state() == actionlib.GoalStatus.SUCCEEDED:
self._runscreen.nextPlan.setDisabled(False)
self._runscreen.previousPlan.setDisabled(False)
self._runscreen.resetToHome.setDisabled(False)
self.in_process=False
rospy.loginfo("Succeeded")
elif self.client.get_state() == actionlib.GoalStatus.ABORTED:
self.in_process=False
if(not self.recover_from_pause):
raise Exception("Process step failed and aborted")
elif self.client.get_state() == actionlib.GoalStatus.REJECTED:
self.in_process=False
raise Exception("Process step failed and Rejected")
elif self.client.get_state() == actionlib.GoalStatus.LOST:
self.in_process=False
raise Exception("Process step failed and lost")
'''
if (self.count > 10):
self.count = 0
if (data.mode.mode < 0):
'''
#self.stackedWidget.setCurrentIndex(2)
if(data.mode.mode==-5 or data.mode.mode==-6):
error_msg="Error mode %d : Controller is not synched or is in Invalid State" %data.mode.mode
self._errordiagnosticscreen.errorLog.setPlainText(error_msg)
if(data.mode.mode==-3 or data.mode.mode==-2):
error_msg="Error mode %d : Controller operation or argument is invalid" %data.mode.mode
self._errordiagnosticscreen.errorLog.setPlainText(error_msg)
if(data.mode.mode==-13 or data.mode.mode==-14):
error_msg="Error mode %d : Sensor fault or communication Error" %data.mode.mode
self._errordiagnosticscreen.errorLog.setPlainText(error_msg)
if(data.mode.mode==-1):
self._errordiagnosticscreen.errorLog.setPlainText("Error mode -1: Internal system error detected")
if(data.mode.mode==-4):
self._errordiagnosticscreen.errorLog.setPlainText("Error mode -4: Robot Fault detected")
if(data.mode.mode==-7):
self._errordiagnosticscreen.errorLog.setPlainText("Error mode -7: Robot singularity detected, controller cannot perform movement")
if(data.mode.mode==-8):
self._errordiagnosticscreen.errorLog.setPlainText("Error mode -8: Robot Setpoint could not be tracked, robot location uncertain")
if(data.mode.mode==-9):
self._errordiagnosticscreen.errorLog.setPlainText("Error mode -9: Commanded Trajectory is invalid and cannot be executed. Please replan")
if(data.mode.mode==-10):
self._errordiagnosticscreen.errorLog.setPlainText("Error mode -10: Trajectory Tracking Error detected, robot position uncertain, consider lowering speed of operation to improve tracking")
if(data.mode.mode==-11):
self._errordiagnosticscreen.errorLog.setPlainText("Error mode -11: Robot trajectory aborted.")
if(data.mode.mode==-12):
self._errordiagnosticscreen.errorLog.setPlainText("Error mode -12: Robot Collision Imminent, operation stopped to prevent damage")
if(data.mode.mode==-15):
self._errordiagnosticscreen.errorLog.setPlainText("Error mode -15: Sensor state is invalid")
if(data.mode.mode==-16):
self._errordiagnosticscreen.errorLog.setPlainText("Error mode -16: Force Torque Threshold Violation detected, stopping motion to prevent potential collisions/damage")
if(data.mode.mode==-17):
self._errordiagnosticscreen.errorLog.setPlainText("Error mode -17: Invalid External Setpoint given")
'''
#messagewindow=VacuumConfirm()
#reply = QMessageBox.question(messagewindow, 'Connection Lost',
# 'Robot Connection Lost, Return to Welcome Screen?', QMessageBox.Yes | QMessageBox.No, QMessageBox.No)
#if reply==QMessageBox.Yes:
#
# self.disconnectreturnoption=False
#else:
# self.disconnectreturnoption=True
self.led_change(self.robotconnectionled, False)
self.led_change(self.runscreenstatusled, False)
self.error_recovery_button()
else:
self.led_change(self.robotconnectionled, True)
self.led_change(self.runscreenstatusled, True)
if (data.ft_wrench_valid == "False"):
self.stackedWidget.setCurrentIndex(0)
self.led_change(self.forcetorqueled, False)
else:
self.led_change(self.forcetorqueled, True)
#self.service_list=rosservice.get_service_list()
#if(self.disconnectreturnoption and data.error_msg==""):
# self.disconnectreturnoption=False
self.count += 1
if (not self.force_torque_plot_widget.isHidden()):
self.x_data = np.concatenate((self.x_data, [data.header.seq]))
incoming = np.array([
data.ft_wrench.torque.x, data.ft_wrench.torque.y,
data.ft_wrench.torque.z, data.ft_wrench.force.x,
data.ft_wrench.force.y, data.ft_wrench.force.z
]).reshape(6, 1)
self.force_torque_data = np.concatenate(
(self.force_torque_data, incoming), axis=1)
if (self.data_count > 500):
self.force_torque_data = self.force_torque_data[..., 1:]
self.x_data = self.x_data[1:]
self.force_torque_plot_widget.setRange(
xRange=(self.x_data[1], self.x_data[-1]))
else:
self.data_count += 1
for i in range(6):
self.plot_container[i].setData(
self.x_data, self.force_torque_data[i, ...])
self.force_torque_app.processEvents()
if (not self.joint_angle_plot_widget.isHidden()):
self.x_data = np.concatenate((self.x_data, [data.header.seq]))
incoming = np.array([
data.ft_wrench.torque.x, data.ft_wrench.torque.y,
data.ft_wrench.torque.z, data.ft_wrench.force.x,
data.ft_wrench.force.y, data.ft_wrench.force.z
]).reshape(7, 1)
self.joint_angle_data = np.concatenate(
(self.joint_angle_data, incoming), axis=1)
if (self.data_count > 500):
self.joint_angle_data = self.joint_angle_data[..., 1:]
self.x_data = self.x_data[1:]
self.joint_angle_plot_widget.setRange(
xRange=(self.x_data[1], self.x_data[-1]))
else:
self.data_count += 1
for i in range(7):
self.plot_container[i].setData(
self.x_data, self.joint_angle_data[i, ...])
self.joint_angle_app.processEvents()
#if(len(self._data_array)>10):
# for x in self._data_array:
# self._widget.State_info.append(x)
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
class HilPlugin(Plugin):
# MAV mode flags
MAV_MODE_FLAG_SAFETY_ARMED = 128
MAV_MODE_FLAG_MANUAL_INPUT_ENABLED = 64
MAV_MODE_FLAG_HIL_ENABLED = 32
MAV_MODE_FLAG_STABILIZE_ENABLED = 16
MAV_MODE_FLAG_GUIDED_ENABLED = 8
MAV_MODE_FLAG_AUTO_ENABLED = 4
MAV_MODE_FLAG_TEST_ENABLED = 2
MAV_MODE_FLAG_CUSTOM_MODE_ENABLED = 1
# MAV state dictionary
mav_state = {
0: 'Uninitialized',
1: 'Booting up',
2: 'Calibrating',
3: 'Standby',
4: 'Active',
5: 'Critical',
6: 'Emergency',
7: 'Poweroff'
}
# Constants
STR_ON = 'ON'
STR_OFF = 'OFF'
STR_UNKNOWN = 'N/A'
STR_MAVROS_ARM_SERVICE_NAME = '/mavros/cmd/arming'
STR_MAVROS_COMMAND_LONG_SERVICE_NAME = '/mavros/cmd/command'
STR_MAVROS_SET_MODE_SERVICE_NAME = '/mavros/set_mode'
STR_SYS_STATUS_SUB_TOPIC = '/mavros/state'
TIMEOUT_HIL_HEARTBEAT = 2.0
def __init__(self, context):
super(HilPlugin, self).__init__(context)
self.setObjectName('HilPlugin')
self._widget = QWidget()
rp = rospkg.RosPack()
ui_file = os.path.join(rospkg.RosPack().get_path('rqt_rotors'),
'resource', 'HilPlugin.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('HilPluginUi')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
context.add_widget(self._widget)
# Set the initial parameters of UI elements
self._widget.button_set_hil_mode.setEnabled(False)
self._widget.button_arm.setEnabled(False)
self._widget.button_reboot_autopilot.setEnabled(False)
self._widget.text_state.setText(self.STR_UNKNOWN)
self.clear_mav_mode()
# Initialize class variables
self.last_heartbeat_time = time.time()
self.mav_mode = 65
self.mav_status = 255
self.armed = False
self.connected = False
self.guided = False
self.hil_enabled = False
# Set the functions that are called when signals are emitted
self._widget.button_set_hil_mode.pressed.connect(
self.on_set_hil_mode_button_pressed)
self._widget.button_arm.pressed.connect(self.on_arm_button_pressed)
self._widget.button_reboot_autopilot.pressed.connect(
self.on_reboot_autopilot_button_pressed)
# Create ROS service proxies
self.arm = rospy.ServiceProxy(self.STR_MAVROS_ARM_SERVICE_NAME,
CommandBool)
self.send_command_long = rospy.ServiceProxy(
self.STR_MAVROS_COMMAND_LONG_SERVICE_NAME, CommandLong)
self.set_mode = rospy.ServiceProxy(
self.STR_MAVROS_SET_MODE_SERVICE_NAME, SetMode)
# Initialize ROS subscribers and publishers
self.sys_status_sub = rospy.Subscriber(self.STR_SYS_STATUS_SUB_TOPIC,
State,
self.sys_status_callback,
queue_size=1)
def on_set_hil_mode_button_pressed(self):
new_mode = self.mav_mode | self.MAV_MODE_FLAG_HIL_ENABLED
self.hil_enabled = True
self.mav_mode = new_mode
self.set_mode(new_mode, '')
self._widget.text_mode_hil.setText(self.mav_mode_text(
self.hil_enabled))
def on_arm_button_pressed(self):
self.arm(True)
def on_reboot_autopilot_button_pressed(self):
self.send_command_long(False, 246, 1, 1, 0, 0, 0, 0, 0, 0)
def sys_status_callback(self, msg):
if (not self.connected and msg.connected):
self._widget.button_set_hil_mode.setEnabled(True)
self._widget.button_arm.setEnabled(True)
self._widget.button_reboot_autopilot.setEnabled(True)
self.connected = True
self.last_heartbeat_time = time.time()
self._widget.text_mode_safety_armed.setText(
self.mav_mode_text(msg.armed))
self._widget.text_mode_guided.setText(
self.mav_mode_text(msg.guided))
return
if (((time.time() - self.last_heartbeat_time) >=
self.TIMEOUT_HIL_HEARTBEAT) and self.hil_enabled):
new_mode = self.mav_mode | self.MAV_MODE_FLAG_HIL_ENABLED
self.set_mode(new_mode, '')
if (self.armed != msg.armed):
self.armed = msg.armed
self._widget.text_mode_safety_armed.setText(
self.mav_mode_text(self.armed))
self._widget.button_arm.setEnabled(not (self.armed))
self.mav_mode = self.mav_mode | self.MAV_MODE_FLAG_SAFETY_ARMED
if (self.guided != msg.guided):
self.guided = msg.guided
self._widget.text_mode_guided.setText(
self.mav_mode_text(self.guided))
self.last_heartbeat_time = time.time()
def clear_mav_mode(self):
count = self._widget.mav_mode_layout.rowCount()
for i in range(count):
self._widget.mav_mode_layout.itemAt(
i, QFormLayout.FieldRole).widget().setText(self.STR_UNKNOWN)
def mav_mode_text(self, mode_enabled):
return self.STR_ON if mode_enabled else self.STR_OFF
def shutdown_plugin(self):
if self.sys_status_sub is not None:
self.sys_status_sub.unregister()
class HelpingHandGUI(Plugin):
# Various member variables
# _topic_subscriber = None
# _topic_good = False
# _digitalinput_subscriber = None
# _namespace = None
_available_ns = []
_conf_yaml = None
_trigger_sources = []
_data_buffer = {}
_refresh_data_table_contents_sig = pyqtSignal()
# _pattern = re.compile(r'\/[^/]+\/')
# _digitalinput = None
# _gravcomp_service = None
# _dmp_action_server_client = None
# _topic_name = None
# _num_weights = 0
# _recorded_dmp = None
# _msg_store = None
# _database_service = None
# _dmp_save_name = None
# _pyaudio = None
# _stream = None
# _beep = None
# _joint_space = None
# _cartesian_space = None
# _available_frames = []
# DMP trajectory buffer
_joint_traj = []
# DMP encoding service
_dmp_ecoder = None
# Logging
_init_status_report = {
'status': 'idle',
'last_entry': 'None',
'last_type': 'None',
}
# Logics
_old_gravbutton = 0
_old_rec_button = 0
_recording = False
_gravComp = False
# Some signal
_dmp_recording_started_signal = pyqtSignal()
_dmp_recording_stopped_signal = pyqtSignal()
# TF2
_tf2_buffer = tf2_ros.Buffer()
_tf2_listener = tf2_ros.TransformListener(_tf2_buffer)
_tf_listener = tf.TransformListener()
# Available topics
_available_topics_array = []
_available_topics_dict = dict()
_old_index = 0
def __init__(self, context):
super(HelpingHandGUI, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('HelpingHandGUI')
# 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:
# rospy.logdebug 'arguments: ', args
# rospy.logdebug 'unknowns: ', unknowns
# Create QWidget
self._widget = QWidget()
# Get path to UI file which should be in the "resource" folder of this package
ui_file = os.path.join(rospkg.RosPack().get_path('helping_hand_gui'), 'qt', 'helpinghand.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
# self._widget.setObjectName('DMPRecordToolGUIUi')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). 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._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
# Populate configuration table's headers
self._conf_table = self._widget.findChild(QTableWidget, 'configTable')
self._conf_table.setHorizontalHeaderLabels((
'Trigger Name',
'Robot Namespace',
'Trigger Topic',
'Trigger Type',
'Trigger Value',
'Trigger Callback'))
# Populate data table's headers
self._data_table = self._widget.findChild(QTableWidget, 'data_table')
self._data_table.setHorizontalHeaderLabels((
'Timestamp',
'Trigger Conf',
'Trigger Reason',
'Data Type'))
header = self._data_table.horizontalHeader()
header.setSectionResizeMode(QHeaderView.ResizeToContents)
# Hide the 5th column that contains data identifiers
self._data_table.setColumnCount(self._data_table.columnCount()+1)
self._data_table.setColumnHidden(self._data_table.columnCount()-1, True)
# Connect the refresh signal with the update function
self._refresh_data_table_contents_sig.connect(self._update_data_table)
# Connect the item selected signal to the display details
self._data_table.itemSelectionChanged.connect(self._display_item_details)
# Parse config
self._parse_yaml_config()
# Show conf in table
self._write_conf_to_table()
# Display what we are listening to
self._display_addresses()
# Print initial status report
self.status_report = StatusReport(self._init_status_report, self)
self._print_status()
# DMP Encoding service
self._dmp_ecoder = rospy.ServiceProxy('/encode_traj_to_dmp', EncodeTrajectoryToDMP)
# Save button widget
save_button = self._widget.findChild(QPushButton, 'save_button')
save_button.clicked.connect(partial(self._save_button_pressed, save_button))
# Create database proxy
self._msg_store = MessageStoreProxy()
# Create the service proxies for into saving databse
self._tf_capture_srv = rospy.ServiceProxy('tf_capture', CaptureTF)
self._joint_capture_srv = rospy.ServiceProxy('joint_capture', CaptureJoint)
self._dmp_capture_srv = rospy.ServiceProxy('dmp_capture', CaptureDMP)
# Make sure the services are running !!
try:
self._tf_capture_srv.wait_for_service(0.5)
self._joint_capture_srv.wait_for_service(0.5)
self._dmp_capture_srv.wait_for_service(0.5)
except Exception as e:
rospy.logerr("Could not establish a connection to services. See exception:\n{}".format(e))
exit()
def _print_status(self):
status_text_widget = self._widget.findChild(QTextEdit, 'statusWindow')
status_text = \
"Status: {0}\n"\
"Last saved entry: {1}\n"\
"Last entry type: {2}".format(
self.status_report['status'],
self.status_report['last_entry'],
self.status_report['last_type']
)
status_text_widget.setText(status_text)
# YAML config parser
def _parse_yaml_config(self):
conf_file = os.path.join(rospkg.RosPack().get_path('helping_hand_gui'), 'conf', 'triggers_conf.yml')
self._conf_yaml = yaml.load(open(conf_file), Loader=yaml.FullLoader)
tmp_ns = []
for conf_key, conf_val in self._conf_yaml.items():
tmp_ns.append(conf_val['robot_ns'])
if conf_val['trig_type'] == 'rising_edge':
conf_val['subscriber'] = rospy.Subscriber(conf_val['trig_topic'], Bool, callback=self._rising_edge_cb, callback_args=(conf_key))
elif conf_val['trig_type'] == 'falling_edge':
conf_val['subscriber'] = rospy.Subscriber(conf_val['trig_topic'], Bool, callback=self._falling_edge_cb, callback_args=(conf_key))
elif conf_val['trig_type'] == 'hold':
conf_val['subscriber'] = rospy.Subscriber(conf_val['trig_topic'], Bool, callback=self._hold_cb, callback_args=(conf_key))
else:
self.status_report['status'] = "Error parsing trigger types."
if conf_val['trig_callback'] == 'joint_save':
joint_topic_addr = conf_val['robot_ns'] + conf_val['joint_topic']
conf_val['subscriber'] = rospy.Subscriber(joint_topic_addr, JointState, callback=self._joint_state_cb, callback_args=(conf_key))
if conf_val['trig_callback'] == 'joint_pose_save':
joint_topic_addr = conf_val['robot_ns'] + conf_val['joint_topic']
conf_val['subscriber'] = rospy.Subscriber(joint_topic_addr, JointState, callback=self._joint_state_cb, callback_args=(conf_key))
if conf_val['trig_callback'] == 'joint_dmp_save':
joint_topic_addr = conf_val['robot_ns'] + conf_val['joint_topic']
conf_val['subscriber'] = rospy.Subscriber(joint_topic_addr, JointState, callback=self._joint_state_cb, callback_args=(conf_key))
# Add TF
# Remove duplicate namespaces
self._available_ns = list(dict.fromkeys(tmp_ns))
def _write_conf_to_table(self):
self._conf_table.setRowCount(len(self._conf_yaml))
for row, conf_key in enumerate(self._conf_yaml.items()):
conf = conf_key[1]
self._conf_table.setItem(row, 0, QTableWidgetItem(conf['trig_name']))
self._conf_table.setItem(row, 1, QTableWidgetItem(conf['robot_ns']))
self._conf_table.setItem(row, 2, QTableWidgetItem(conf['trig_topic']))
self._conf_table.setItem(row, 3, QTableWidgetItem(conf['trig_type']))
self._conf_table.setItem(row, 4, QTableWidgetItem(str(conf['trig_value'])))
self._conf_table.setItem(row, 5, QTableWidgetItem(conf['trig_callback']))
def _update_data_table(self):
self._data_table.setRowCount(0)
for data_key, data_dict in self._data_buffer.items():
current_row = self._data_table.rowCount()
self._data_table.insertRow(current_row)
self._data_table.setItem(current_row, 0, QTableWidgetItem(data_dict['time']))
self._data_table.setItem(current_row, 1, QTableWidgetItem(data_dict['trig_name']))
self._data_table.setItem(current_row, 2, QTableWidgetItem(data_dict['trig_type']))
self._data_table.setItem(current_row, 3, QTableWidgetItem(data_dict['data']._type))
self._data_table.setItem(current_row, 4, QTableWidgetItem(data_key))
self._data_table.sortItems(0, Qt.DescendingOrder)
def _display_item_details(self):
text_box = self._widget.findChild(QTextEdit, 'entry_details')
try:
curent_item = self._data_table.currentItem()
current_row = curent_item.row()
entry_identifier = self._data_table.item(current_row, 4).text()
self.status_report['status'] = 'Item selected'
data_details = self._data_buffer[entry_identifier]['data'],
text_box.setText(format(data_details))
except Exception as e:
pass
def _display_addresses(self):
self.discover_available_topics_array()
addr_list_widget = self._widget.findChild(QListWidget, 'addr_list')
for topic in self._available_topics_array:
addr_list_widget.addItem(QListWidgetItem(topic))
# topic = '/ur10_2/joint_states'
# self._available_ns
# Get a list of topics that match a certain type
def discover_available_topics_array(self):
all_topics = rospy.get_published_topics()
for topic, msg_type in all_topics:
if msg_type in DESIRED_MSGS:
self._available_topics_array.append(topic)
self._available_topics_dict[topic] = msg_type
self._available_topics_array.sort()
# The following line removes possible duplicate entries from the array
self._available_topics_array = list(set(self._available_topics_array))
# Discover available robots
def discover_frames(self):
# Define a search pattern, it will look for all namespaces that contain the 'received_control_mode' topic
search_pattern = re.compile(r'^(?!.*link).*')
# search_pattern = re.compile(r'^((?!.*link)|(.*base)).*$')
# Get the list of all currently availabel frames in TF1 (this is a temporary solution until a better one is found)
time.sleep(1) # Give tf time to start working
# all_frames = self._tf_listener.getFrameStrings()
yaml_frames = yaml.load(self._tf2_buffer.all_frames_as_yaml())
# Look through all avaiable topics
for entry in yaml_frames:
namespace = search_pattern.findall(entry)
if len(namespace):
self._available_frames.append(namespace[0])
# Sort the namespaces alphabetically
self._available_frames.sort()
# rospy.logdebug out the result of the search
# rospy.loginfo("Found robots with namespace: \n%s", self._available_frames)
# Add robots namespaces to combobox
self._combobox = self._widget.findChildren(QComboBox, QRegExp('frameSelector'))[0]
self._combobox.insertItems(0, self._available_frames)
self._namespace = self._combobox.currentText()
# Quit if no robots are found
if len(self._available_frames) == 0:
rospy.logerr("[%s]: No robots available !!", rospy.get_name())
sys.exit()
def _save_button_pressed(self, save_button):
save_name_widget = self._widget.findChild(QLineEdit, 'save_name')
save_name = save_name_widget.text()
# Check if a name was entered
if not(len(save_name)):
self.status_report['status'] = 'No name provided !!'
return -1
# Get selected item
curent_item = self._data_table.currentItem()
if curent_item is None:
self.status_report['status'] = 'No item selected !!'
return -1
# Get selected item row
current_row = curent_item.row()
if current_row == -1:
self.status_report['status'] = 'No item selected !!'
return -1
# Get the item identifier and retrieve the data
entry_identifier = self._data_table.item(current_row, 4).text()
data = self._data_buffer[entry_identifier]['data']
# Update the status box
self.status_report['status'] = 'Save button pressed'
rospy.logdebug("Save button pressed !!")
# Handle transform stamped
if data._type == 'geometry_msgs/TransformStamped':
trig_name = self._data_buffer[entry_identifier]['trig_name']
for keys, vals in self._data_buffer.items():
if (vals['trig_name'] == trig_name) and (vals['data']._type == 'sensor_msgs/JointState'):
pass
# Handle Joint space configuration
elif data._type == 'sensor_msgs/JointState':
self._joint_capture_srv.call(data, save_name)
# Handle Joint space configuration
elif data._type == 'robot_module_msgs/JointSpaceDMP':
self._dmp_capture_srv.call(data, CartesianSpaceDMP(), save_name)
# Handle Joint space DMP
# # Saving to database according to the text in the saveName field
# if self._joint_space:
# existingDMP = self._msg_store.query_named(saveName, JointSpaceDMP._type)[0]
# if existingDMP == None:
# rospy.loginfo("Saving new DMP with ID = " + self._dmp_save_name + " to database!")
# self._msg_store.insert_named(saveName, self._recorded_dmp)
# else:
# rospy.loginfo("Updating DMP with ID = " + self._dmp_save_name + " in database!")
# self._msg_store.update_named(saveName, self._recorded_dmp)
# else:
# existingDMP = self._msg_store.query_named(saveName, CartesianSpaceDMP._type)[0]
# if existingDMP == None:
# rospy.loginfo("Saving new DMP with ID = " + self._dmp_save_name + " to database!")
# self._msg_store.insert_named(saveName, self._recorded_dmp)
# else:
# rospy.loginfo("Updating DMP with ID = " + self._dmp_save_name + " in database!")
# self._msg_store.update_named(saveName, self._recorded_dmp)
# Write to report
self.status_report['status'] = 'Data saved'
self.status_report['last_entry'] = save_name
self.status_report['last_type'] = data._type
# Update text fields
save_name_widget.setText('')
details_text_box = self._widget.findChild(QTextEdit, 'entry_details')
details_text_box.setText('')
# Clean up table
self._data_table.clearSelection()
self._data_table.removeRow(current_row)
# # Remove data from buffer
# self._data_buffer.pop(entry_identifier)
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
# Topic subscribers
def _joint_state_cb(self, data, conf_name):
self._conf_yaml[conf_name]['joint_data'] = data
# def save_joint_conf_sig_h(self):
# rospy.loginfo('Saving joint conf !!')
def _rising_edge_cb(self, data, conf_name):
if not(self._conf_yaml[conf_name].has_key('sig_val')):
self._conf_yaml[conf_name]['sig_val'] = data.data
old_val = self._conf_yaml[conf_name]['sig_val']
new_val = data.data
if old_val - new_val == -1:
rospy.logdebug('Rising edge!!')
self._handle_trigger(self._conf_yaml[conf_name])
self._conf_yaml[conf_name]['sig_val'] = new_val
def _falling_edge_cb(self, data, conf_name):
if not(self._conf_yaml[conf_name].has_key('sig_val')):
self._conf_yaml[conf_name]['sig_val'] = data.data
old_val = self._conf_yaml[conf_name]['sig_val']
new_val = data.data
if old_val - new_val == 1:
rospy.logdebug('Falling edge!!')
self._handle_trigger(self._conf_yaml[conf_name])
self._conf_yaml[conf_name]['sig_val'] = new_val
def _hold_cb(self, data, conf_name):
if not('sig_val' in self._conf_yaml[conf_name]):
self._conf_yaml[conf_name]['sig_val'] = data.data
old_val = self._conf_yaml[conf_name]['sig_val']
new_val = data.data
if falling_edge(old_val, new_val):
rospy.logdebug("[_hold_cb] Falling edge")
if len(self._joint_traj):
self._handle_trigger(self._conf_yaml[conf_name])
if rising_edge(old_val, new_val):
pass
rospy.logdebug("[_hold_cb] Rising edge")
if old_val + new_val == 2:
rospy.logdebug("[_hold_cb] Holding")
trig_type = self._conf_yaml[conf_name]['trig_callback']
if trig_type == "joint_dmp_save":
rospy.logdebug("Appending !!")
self._joint_traj.append(self._conf_yaml[conf_name]['joint_data'])
self._conf_yaml[conf_name]['sig_val'] = new_val
def _handle_trigger(self, trigg_conf):
trigg_conf['robot_ns']
trig_type = trigg_conf['trig_callback']
save_data = []
if trig_type == "joint_save":
rospy.logdebug('joint_save')
save_data = [trigg_conf['joint_data']]
elif trig_type == "joint_pose_save":
joint_data = trigg_conf['joint_data']
rospy.logdebug('joint_pose_save')
save_data = [joint_data, TransformStamped()] # , tf_data]
elif trig_type == "joint_dmp_save":
rospy.logdebug('joint_dmp_save')
traj_dur = traj_duration_sec(self._joint_traj)
if traj_dur < 1:
rospy.logwarn("Trajectory too short to store")
else:
dmp_request = EncodeTrajectoryToDMPRequest()
dmp_request.demonstratedTrajectory = self._joint_traj
dmp_request.dmpParameters.N = trigg_conf['joint_dmp_N']
result_dmp = self._dmp_ecoder.call(dmp_request).encodedDMP
save_data = [result_dmp]
self._joint_traj = []
else:
rospy.logerr("Unknown saving type !!")
rospy.logerr("trig_type = {}".format(trig_type))
# self.status_report['status'] = "Error. See terminal"
for data in save_data:
now = datetime.now()
entry_identifier = now.strftime("%H%M%S%f")
current_time = now.strftime("%H:%M:%S")
self._data_buffer[entry_identifier] = {
'data': data,
'time': current_time,
'trig_name': trigg_conf['trig_name'],
'trig_type': trigg_conf['trig_type']
}
self._refresh_data_table_contents_sig.emit()
class SpeechStatusPlugin(Plugin):
def __init__(self, context):
super(SpeechStatusPlugin, self).__init__(context)
self.setObjectName('SpeechStatusPlugin')
self._widget = QWidget()
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
ui_file = os.path.join(rospkg.RosPack().get_path('rqt_speech_status'),
'resource', 'speech_widget.ui')
loadUi(ui_file, self._widget)
context.add_widget(self._widget)
file_live = os.path.join(
rospkg.RosPack().get_path('rqt_speech_status'), 'resource',
'microphone.png')
file_mute = os.path.join(
rospkg.RosPack().get_path('rqt_speech_status'), 'resource',
'muted.png')
file_silency = os.path.join(
rospkg.RosPack().get_path('rqt_speech_status'), 'resource',
'silence-face.png')
file_speech = os.path.join(
rospkg.RosPack().get_path('rqt_speech_status'), 'resource',
'surprise-face.png')
self.img_mute = QPixmap(file_mute).scaled(160, 160)
self.img_live = QPixmap(file_live).scaled(160, 160)
self.img_silency = QPixmap(file_silency).scaled(160, 160)
self.img_speech = QPixmap(file_speech).scaled(160, 160)
self._widget.imgRecognition.setPixmap(self.img_live)
self._widget.imgSilency.setPixmap(self.img_silency)
self.signal_render_recog_status = SignalRender()
self.signal_render_recog_status.renderSignal.connect(
self.render_recog_status)
self.signal_render_silency_status = SignalRender()
self.signal_render_silency_status.renderSignal.connect(
self.render_silency_status)
rospy.Subscriber('enable_recognition', Bool,
self.handle_enable_recognition)
rospy.Subscriber('start_of_speech', Empty, self.handle_start_speech)
rospy.Subscriber('end_of_speech', Empty, self.handle_end_speech)
rospy.Subscriber('silency_detected', Empty,
self.handle_silency_detection)
def handle_enable_recognition(self, msg):
if msg.data:
self.signal_render_recog_status.renderSignal.emit(True)
else:
self.signal_render_recog_status.renderSignal.emit(False)
def handle_silency_detection(self, msg):
self.signal_render_silency_status.renderSignal.emit(True)
def handle_start_speech(self, msg):
self.signal_render_silency_status.renderSignal.emit(False)
def handle_end_speech(self, msg):
self.signal_render_silency_status.renderSignal.emit(True)
def render_recog_status(self, status):
if status:
self._widget.imgRecognition.setPixmap(self.img_live)
else:
self._widget.imgRecognition.setPixmap(self.img_mute)
def render_silency_status(self, status):
if status:
self._widget.imgSilency.setPixmap(self.img_silency)
else:
self._widget.imgSilency.setPixmap(self.img_speech)
def shutdown_plugin(self):
pass
def save_settings(self, plugin_settings, instance_settings):
pass
def restore_settings(self, plugin_settings, instance_settings):
pass
class DriverStationSim(Plugin):
def __init__(self, context):
super(DriverStationSim, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('DriverStationSim')
# 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._widget = QWidget()
# Get path to UI file which should be in the "resource" folder of this package
ui_file = os.path.join(
rospkg.RosPack().get_path('rqt_driver_station_sim'), 'resource',
'driverStationSim.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('DriverStationSim')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). 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.
auto_pub = rospy.Publisher("/frcrobot/autonomous_mode",
AutoMode,
queue_size=3)
match_pub = rospy.Publisher("/frcrobot/match_data",
MatchSpecificData,
queue_size=3)
def pub_data(self):
r = rospy.Rate(100)
auto_msg = AutoMode()
auto_msg.mode = [0, 0, 0, 0]
auto_msg.delays = [0, 0, 0, 0]
match_msg = MatchSpecificData()
modes = [0, 0, 0, 0]
match_msg = MatchSpecificData()
start_time = rospy.get_time()
enable_last = False
auto_last = False
practice_last = False
auto_duration = 0
while (not rospy.is_shutdown()):
#Robot State Values
enable = self._widget.enable_button_2.isChecked()
disable = self._widget.disable_button_2.isChecked()
auto = self._widget.auto_mode.isChecked()
practice = self._widget.practice_button.isChecked()
#Time Start and Restart Handling
if (not enable_last and enable):
rospy.logwarn("enableLast")
start_time = rospy.get_time()
if (not auto_last and auto and not practice):
rospy.logwarn("autoLast")
start_time = rospy.get_time()
if (not practice_last and practice):
rospy.logwarn("practiceLast")
start_time = rospy.get_time()
auto_duration = 15 #TODO read from DS
if (enable and practice):
if (rospy.get_time() < start_time + auto_duration):
auto = True
enable = True
disable = False
elif (rospy.get_time() >= start_time + auto_duration
and rospy.get_time < start_time + 150):
auto = False
enable = True
disable = False
elif (rospy.get_time() >= start_time + 150):
auto = False
enable = False
disable = True
if (enable):
time_diff = int(rospy.get_time() - start_time)
minutes = ((150 - time_diff) / 60)
seconds = ((149 - time_diff) % 60 +
(1.0 - (rospy.get_time() - start_time) % 1))
time = str(minutes) + ":" + str(seconds)
if (not self._widget.timer_pause.isChecked()):
self._widget.time.setText(time[:7])
match_msg.matchTimeRemaining = 150 - time_diff
match_msg.Disabled = False
match_msg.Enabled = True
else:
match_msg.matchTimeRemaining = 0
match_msg.Disabled = True
match_msg.Enabled = False
#Publish Data
# match_msg.allianceData = self._widget.match_data.text()
match_msg.allianceColor = 1
match_msg.driverStationLocation = 1
match_msg.matchNumber = 1
match_msg.Autonomous = auto
enable_last = match_msg.Enabled
auto_last = auto
practice_last = practice
auto_msg.header.stamp = rospy.Time.now()
auto_msg.position = self._widget.start_pos.value()
auto_msg.mode[0] = self._widget.mode_0.value()
auto_msg.mode[1] = self._widget.mode_1.value()
auto_msg.mode[2] = self._widget.mode_2.value()
auto_msg.mode[3] = self._widget.mode_3.value()
auto_msg.delays[0] = self._widget.delay_0.value()
auto_msg.delays[1] = self._widget.delay_1.value()
auto_msg.delays[2] = self._widget.delay_2.value()
auto_msg.delays[3] = self._widget.delay_3.value()
match_pub.publish(match_msg)
auto_pub.publish(auto_msg)
r.sleep()
load_thread = threading.Thread(target=pub_data, args=(self, ))
load_thread.start()
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
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
class RQTDuckpit(Plugin):
def __init__(self, context):
super(RQTDuckpit, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('Duckpit')
# 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._widget = QWidget()
# Get path to UI file which should be in the "resource" folder of this package
ui_file = os.path.join(rospkg.RosPack().get_path('rqt_duckpit'),
'resource', 'rqt_duckpit.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('rqt_duckpit')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). 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._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
self.veh = rospy.get_param('/veh', "megabot08")
# Subscribers
# self.camera_topic = '/' + self.veh + '/camera_node/image/compressed'
# self.sub_camera = rospy.Subscriber(self.camera_topic, sensor_msgs.msg.CompressedImage, self.cbImage, queue_size=1)
self.steps_ahead = 5
self.cmd_topics = []
self.cmd_topics.append('/' + self.veh +
'/inverse_kinematics_node/car_cmd')
self.cmd_node_name = 'cnn_node'
for i in range(1, self.steps_ahead):
self.cmd_topics.append('/' + self.veh + '/' + self.cmd_node_name +
'/car_cmd_' + str(i + 1))
self.sub_cmds = dict()
for i in range(0, self.steps_ahead):
self.sub_cmds[i] = rospy.Subscriber(
self.cmd_topics[i],
duckietown_msgs.msg.Twist2DStamped,
self.cbCarCmds,
callback_args=i + 1)
self._widget.h_slider.setValue(0)
self._widget.h_slider_2.setValue(0)
self._widget.h_slider_3.setValue(0)
self._widget.h_slider_4.setValue(0)
self._widget.h_slider_5.setValue(0)
self._widget.label_val.setText("0")
self._widget.label_val_2.setText("0")
self._widget.label_val_3.setText("0")
self._widget.label_val_4.setText("0")
self._widget.label_val_5.setText("0")
def cbCarCmds(self, msg, num):
if num == 1:
self._widget.h_slider.setValue(msg.omega)
self._widget.label_val.setText(str(msg.omega))
elif num == 2:
self._widget.h_slider_2.setValue(msg.omega)
self._widget.label_val_2.setText(str(msg.omega))
elif num == 3:
self._widget.h_slider_3.setValue(msg.omega)
self._widget.label_val_3.setText(str(msg.omega))
elif num == 4:
self._widget.h_slider_4.setValue(msg.omega)
self._widget.label_val_4.setText(str(msg.omega))
elif num == 5:
self._widget.h_slider_5.setValue(msg.omega)
self._widget.label_val_5.setText(str(msg.omega))
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
class RobotSteering(Plugin):
slider_factor = 1000.0
def __init__(self, context):
super(RobotSteering, self).__init__(context)
self.setObjectName('RobotSteering')
self._publisher = None
self._widget = QWidget()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_robot_steering'), 'resource',
'RobotSteering.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('RobotSteeringUi')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
context.add_widget(self._widget)
self._widget.topic_line_edit.textChanged.connect(
self._on_topic_changed)
self._widget.stop_push_button.pressed.connect(self._on_stop_pressed)
self._widget.x_linear_slider.valueChanged.connect(
self._on_x_linear_slider_changed)
self._widget.z_angular_slider.valueChanged.connect(
self._on_z_angular_slider_changed)
self._widget.increase_x_linear_push_button.pressed.connect(
self._on_strong_increase_x_linear_pressed)
self._widget.reset_x_linear_push_button.pressed.connect(
self._on_reset_x_linear_pressed)
self._widget.decrease_x_linear_push_button.pressed.connect(
self._on_strong_decrease_x_linear_pressed)
self._widget.increase_z_angular_push_button.pressed.connect(
self._on_strong_increase_z_angular_pressed)
self._widget.reset_z_angular_push_button.pressed.connect(
self._on_reset_z_angular_pressed)
self._widget.decrease_z_angular_push_button.pressed.connect(
self._on_strong_decrease_z_angular_pressed)
self._widget.max_x_linear_double_spin_box.valueChanged.connect(
self._on_max_x_linear_changed)
self._widget.min_x_linear_double_spin_box.valueChanged.connect(
self._on_min_x_linear_changed)
self._widget.max_z_angular_double_spin_box.valueChanged.connect(
self._on_max_z_angular_changed)
self._widget.min_z_angular_double_spin_box.valueChanged.connect(
self._on_min_z_angular_changed)
self.shortcut_w = QShortcut(QKeySequence(Qt.Key_W), self._widget)
self.shortcut_w.setContext(Qt.ApplicationShortcut)
self.shortcut_w.activated.connect(self._on_increase_x_linear_pressed)
self.shortcut_x = QShortcut(QKeySequence(Qt.Key_X), self._widget)
self.shortcut_x.setContext(Qt.ApplicationShortcut)
self.shortcut_x.activated.connect(self._on_reset_x_linear_pressed)
self.shortcut_s = QShortcut(QKeySequence(Qt.Key_S), self._widget)
self.shortcut_s.setContext(Qt.ApplicationShortcut)
self.shortcut_s.activated.connect(self._on_decrease_x_linear_pressed)
self.shortcut_a = QShortcut(QKeySequence(Qt.Key_A), self._widget)
self.shortcut_a.setContext(Qt.ApplicationShortcut)
self.shortcut_a.activated.connect(self._on_increase_z_angular_pressed)
self.shortcut_z = QShortcut(QKeySequence(Qt.Key_Z), self._widget)
self.shortcut_z.setContext(Qt.ApplicationShortcut)
self.shortcut_z.activated.connect(self._on_reset_z_angular_pressed)
self.shortcut_d = QShortcut(QKeySequence(Qt.Key_D), self._widget)
self.shortcut_d.setContext(Qt.ApplicationShortcut)
self.shortcut_d.activated.connect(self._on_decrease_z_angular_pressed)
self.shortcut_shift_w = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_W),
self._widget)
self.shortcut_shift_w.setContext(Qt.ApplicationShortcut)
self.shortcut_shift_w.activated.connect(
self._on_strong_increase_x_linear_pressed)
self.shortcut_shift_x = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_X),
self._widget)
self.shortcut_shift_x.setContext(Qt.ApplicationShortcut)
self.shortcut_shift_x.activated.connect(
self._on_reset_x_linear_pressed)
self.shortcut_shift_s = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_S),
self._widget)
self.shortcut_shift_s.setContext(Qt.ApplicationShortcut)
self.shortcut_shift_s.activated.connect(
self._on_strong_decrease_x_linear_pressed)
self.shortcut_shift_a = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_A),
self._widget)
self.shortcut_shift_a.setContext(Qt.ApplicationShortcut)
self.shortcut_shift_a.activated.connect(
self._on_strong_increase_z_angular_pressed)
self.shortcut_shift_z = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_Z),
self._widget)
self.shortcut_shift_z.setContext(Qt.ApplicationShortcut)
self.shortcut_shift_z.activated.connect(
self._on_reset_z_angular_pressed)
self.shortcut_shift_d = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_D),
self._widget)
self.shortcut_shift_d.setContext(Qt.ApplicationShortcut)
self.shortcut_shift_d.activated.connect(
self._on_strong_decrease_z_angular_pressed)
self.shortcut_space = QShortcut(QKeySequence(Qt.Key_Space),
self._widget)
self.shortcut_space.setContext(Qt.ApplicationShortcut)
self.shortcut_space.activated.connect(self._on_stop_pressed)
self.shortcut_space = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_Space),
self._widget)
self.shortcut_space.setContext(Qt.ApplicationShortcut)
self.shortcut_space.activated.connect(self._on_stop_pressed)
self._widget.stop_push_button.setToolTip(
self._widget.stop_push_button.toolTip() + ' ' +
self.tr('([Shift +] Space)'))
self._widget.increase_x_linear_push_button.setToolTip(
self._widget.increase_x_linear_push_button.toolTip() + ' ' +
self.tr('([Shift +] W)'))
self._widget.reset_x_linear_push_button.setToolTip(
self._widget.reset_x_linear_push_button.toolTip() + ' ' +
self.tr('([Shift +] X)'))
self._widget.decrease_x_linear_push_button.setToolTip(
self._widget.decrease_x_linear_push_button.toolTip() + ' ' +
self.tr('([Shift +] S)'))
self._widget.increase_z_angular_push_button.setToolTip(
self._widget.increase_z_angular_push_button.toolTip() + ' ' +
self.tr('([Shift +] A)'))
self._widget.reset_z_angular_push_button.setToolTip(
self._widget.reset_z_angular_push_button.toolTip() + ' ' +
self.tr('([Shift +] Z)'))
self._widget.decrease_z_angular_push_button.setToolTip(
self._widget.decrease_z_angular_push_button.toolTip() + ' ' +
self.tr('([Shift +] D)'))
# timer to consecutively send twist messages
self._update_parameter_timer = QTimer(self)
self._update_parameter_timer.timeout.connect(
self._on_parameter_changed)
self._update_parameter_timer.start(100)
self.zero_cmd_sent = False
@Slot(str)
def _on_topic_changed(self, topic):
topic = str(topic)
self._unregister_publisher()
if topic == '':
return
try:
self._publisher = rospy.Publisher(topic, Twist, queue_size=10)
except TypeError:
self._publisher = rospy.Publisher(topic, Twist)
def _on_stop_pressed(self):
self._widget.x_linear_slider.setValue(0)
self._widget.z_angular_slider.setValue(0)
def _on_x_linear_slider_changed(self):
self._widget.current_x_linear_label.setText(
'%0.2f m/s' % (self._widget.x_linear_slider.value() /
RobotSteering.slider_factor))
self._on_parameter_changed()
def _on_z_angular_slider_changed(self):
self._widget.current_z_angular_label.setText(
'%0.2f rad/s' % (self._widget.z_angular_slider.value() /
RobotSteering.slider_factor))
self._on_parameter_changed()
def _on_increase_x_linear_pressed(self):
self._widget.x_linear_slider.setValue(
self._widget.x_linear_slider.value() +
self._widget.x_linear_slider.singleStep())
def _on_reset_x_linear_pressed(self):
self._widget.x_linear_slider.setValue(0)
def _on_decrease_x_linear_pressed(self):
self._widget.x_linear_slider.setValue(
self._widget.x_linear_slider.value() -
self._widget.x_linear_slider.singleStep())
def _on_increase_z_angular_pressed(self):
self._widget.z_angular_slider.setValue(
self._widget.z_angular_slider.value() +
self._widget.z_angular_slider.singleStep())
def _on_reset_z_angular_pressed(self):
self._widget.z_angular_slider.setValue(0)
def _on_decrease_z_angular_pressed(self):
self._widget.z_angular_slider.setValue(
self._widget.z_angular_slider.value() -
self._widget.z_angular_slider.singleStep())
def _on_max_x_linear_changed(self, value):
self._widget.x_linear_slider.setMaximum(value *
RobotSteering.slider_factor)
def _on_min_x_linear_changed(self, value):
self._widget.x_linear_slider.setMinimum(value *
RobotSteering.slider_factor)
def _on_max_z_angular_changed(self, value):
self._widget.z_angular_slider.setMaximum(value *
RobotSteering.slider_factor)
def _on_min_z_angular_changed(self, value):
self._widget.z_angular_slider.setMinimum(value *
RobotSteering.slider_factor)
def _on_strong_increase_x_linear_pressed(self):
self._widget.x_linear_slider.setValue(
self._widget.x_linear_slider.value() +
self._widget.x_linear_slider.pageStep())
def _on_strong_decrease_x_linear_pressed(self):
self._widget.x_linear_slider.setValue(
self._widget.x_linear_slider.value() -
self._widget.x_linear_slider.pageStep())
def _on_strong_increase_z_angular_pressed(self):
self._widget.z_angular_slider.setValue(
self._widget.z_angular_slider.value() +
self._widget.z_angular_slider.pageStep())
def _on_strong_decrease_z_angular_pressed(self):
self._widget.z_angular_slider.setValue(
self._widget.z_angular_slider.value() -
self._widget.z_angular_slider.pageStep())
def _on_parameter_changed(self):
self._send_twist(
self._widget.x_linear_slider.value() / RobotSteering.slider_factor,
self._widget.z_angular_slider.value() /
RobotSteering.slider_factor)
def _send_twist(self, x_linear, z_angular):
if self._publisher is None:
return
twist = Twist()
twist.linear.x = x_linear
twist.linear.y = 0
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = z_angular
# Only send the zero command once so other devices can take control
if x_linear == 0 and z_angular == 0:
if not self.zero_cmd_sent:
self.zero_cmd_sent = True
self._publisher.publish(twist)
else:
self.zero_cmd_sent = False
self._publisher.publish(twist)
def _unregister_publisher(self):
if self._publisher is not None:
self._publisher.unregister()
self._publisher = None
def shutdown_plugin(self):
self._update_parameter_timer.stop()
self._unregister_publisher()
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('topic',
self._widget.topic_line_edit.text())
instance_settings.set_value(
'vx_max', self._widget.max_x_linear_double_spin_box.value())
instance_settings.set_value(
'vx_min', self._widget.min_x_linear_double_spin_box.value())
instance_settings.set_value(
'vw_max', self._widget.max_z_angular_double_spin_box.value())
instance_settings.set_value(
'vw_min', self._widget.min_z_angular_double_spin_box.value())
def restore_settings(self, plugin_settings, instance_settings):
value = instance_settings.value('topic', "/cmd_vel")
value = rospy.get_param("~default_topic", value)
self._widget.topic_line_edit.setText(value)
value = self._widget.max_x_linear_double_spin_box.value()
value = instance_settings.value('vx_max', value)
value = rospy.get_param("~default_vx_max", value)
self._widget.max_x_linear_double_spin_box.setValue(float(value))
value = self._widget.min_x_linear_double_spin_box.value()
value = instance_settings.value('vx_min', value)
value = rospy.get_param("~default_vx_min", value)
self._widget.min_x_linear_double_spin_box.setValue(float(value))
value = self._widget.max_z_angular_double_spin_box.value()
value = instance_settings.value('vw_max', value)
value = rospy.get_param("~default_vw_max", value)
self._widget.max_z_angular_double_spin_box.setValue(float(value))
value = self._widget.min_z_angular_double_spin_box.value()
value = instance_settings.value('vw_min', value)
value = rospy.get_param("~default_vw_min", value)
self._widget.min_z_angular_double_spin_box.setValue(float(value))
class TfEcho(Plugin):
# do_update_label = QtCore.pyqtSignal(String)
def __init__(self, context):
super(TfEcho, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('TfEcho')
rp = rospkg.RosPack()
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# TODO(lucasw) these aren't working
# parser.add_argument("source_frame", default='') # parent
# parser.add_argument("target_frame", default='') # child
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.source_frame = ''
self.target_frame = ''
# self.source_frame = self.args.source_frame
# self.target_frame = self.args.target_frame
# Create QWidget
self._widget = QWidget()
# Get path to UI file which is a sibling of this file
# in this example the .ui and .py file are in the same folder
ui_file = os.path.join(rp.get_path('rqt_tf_echo'), 'resource', 'tf_echo.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('TfEchoUi')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). 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._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
cache_time = 10.0
self.tf_buffer = tf2_ros.Buffer(cache_time=rospy.Duration(cache_time))
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
# TODO(lucasw) ros param
self.source_frame = ""
self.target_frame = ""
self.label = {}
self.label['current_time'] = self._widget.findChild(QLabel, 'current_time_label')
self.label['transform_time'] = self._widget.findChild(QLabel, 'transform_time_label')
for axis in ['x', 'y', 'z', 'label']:
self.label['trans_' + axis] = self._widget.findChild(QLabel, 'trans_' + axis + '_label')
self.label['trans_' + axis + '_2'] = self._widget.findChild(QLabel, 'trans_' + axis + '_label_2')
for axis in ['x', 'y', 'z', 'w']:
self.label['quat_' + axis] = self._widget.findChild(QLabel, 'quat_' + axis + '_label')
self.label['quat_' + axis + '_2'] = self._widget.findChild(QLabel, 'quat_' + axis + '_label_2')
for unit in ['rad', 'deg']:
for axis in ['roll', 'pitch', 'yaw', 'label']:
name = 'rot_' + axis + '_' + unit
self.label[name] = self._widget.findChild(QLabel, name + '_label')
self.label[name + '_2'] = self._widget.findChild(QLabel, name + '_label_2')
self.label['source'] = self._widget.findChild(QLineEdit, 'source_line_edit')
self.label['target'] = self._widget.findChild(QLineEdit, 'target_line_edit')
self._widget.setContextMenuPolicy(QtCore.Qt.ActionsContextMenu)
self.actions = {}
self.setup_menu()
self.qt_timer = QTimer()
self.qt_timer.start(200)
self.qt_timer.timeout.connect(self.qt_update)
def add_menu_item(self, menu_name, labels):
menu_text = menu_name
if self.label[labels[0]].isHidden():
menu_text = "Show " + menu_text
else:
menu_text = "Hide " + menu_text
action = QAction(menu_text, self._widget)
action.triggered.connect(partial(self.toggle_labels, labels))
self._widget.addAction(action)
self.actions[menu_name] = action
def setup_menu(self):
for key in self.actions.keys():
self._widget.removeAction(self.actions[key])
self.add_menu_item("transform time", ["transform_time"])
self.add_menu_item("current time", ["current_time"])
self.add_menu_item("source/parent frame", ["source"])
self.add_menu_item("target/child frame", ["target"])
self.add_menu_item("translation", ["trans_x", "trans_y", "trans_z", "trans_label"])
self.add_menu_item("rotation quaternion", ["quat_x", "quat_y", "quat_z", "quat_w"])
self.add_menu_item("rotation (radians)",
["rot_roll_rad", "rot_pitch_rad", "rot_yaw_rad", "rot_label_rad"])
self.add_menu_item("rotation (degrees)",
["rot_roll_deg", "rot_pitch_deg", "rot_yaw_deg", "rot_label_deg"])
def toggle(self, name):
if self.label[name].isHidden():
self.label[name].show()
if name + '_2' in self.label.keys():
self.label[name + '_2'].show()
else:
self.label[name].hide()
if name + '_2' in self.label.keys():
self.label[name + '_2'].hide()
def toggle_labels(self, labels):
for label in labels:
self.toggle(label)
self.setup_menu()
def qt_update(self):
lookup_time = rospy.Time()
cur_time = rospy.Time.now().to_sec()
self.source_frame = self.label['source'].text()
self.target_frame = self.label['target'].text()
# TODO(lucasw) add a text box that can be disabled,
# put any error messages into it.
msg = ''
ts = None
if self.source_frame != "" and self.target_frame != "":
try:
ts = self.tf_buffer.lookup_transform(self.source_frame,
self.target_frame,
lookup_time,
rospy.Duration(0.005))
except tf2.TransformException as ex:
msg = "At time {}, (current time {}) ".format(lookup_time.to_sec(), cur_time)
rospy.logdebug(msg + str(ex))
except rospy.exceptions.ROSTimeMovedBackwardsException as ex:
msg = str(ex)
rospy.logdebug(str(ex))
# update the cur time in case lookup_transform was slow
cur_time = rospy.Time.now().to_sec()
self.label['current_time'].setText("{:1.2f}".format(cur_time))
if ts is None:
# TODO(lucasw) need a dedicated status label in case the user
# hides this?
for label in ['source', 'target']:
self.label[label].setStyleSheet('background-color: yellow')
return
for label in ['source', 'target']:
self.label[label].setStyleSheet('background-color: white')
self.label['transform_time'].setStyleSheet('')
self.label['transform_time'].setText("{:1.2f}".format(ts.header.stamp.to_sec()))
self.label['trans_x'].setText("{:1.3f}".format(ts.transform.translation.x))
self.label['trans_y'].setText("{:1.3f}".format(ts.transform.translation.y))
self.label['trans_z'].setText("{:1.3f}".format(ts.transform.translation.z))
quat = ts.transform.rotation
self.label['quat_x'].setText("{:1.3f}".format(quat.x))
self.label['quat_y'].setText("{:1.3f}".format(quat.y))
self.label['quat_z'].setText("{:1.3f}".format(quat.z))
self.label['quat_w'].setText("{:1.3f}".format(quat.w))
euler = _euler_from_quaternion_msg(quat)
axes = ['roll', 'pitch', 'yaw']
for i in range(3):
self.label['rot_' + axes[i] + '_rad'].setText("{:1.3f}".format(euler[i]))
self.label['rot_' + axes[i] + '_deg'].setText("{:1.3f}".format(math.degrees(euler[i])))
def shutdown_plugin(self):
# TODO unregister all publishers here
pass
def get_param_setting(self, name, label_name, val, instance_settings):
if val == '':
val = rospy.get_param('~' + name, '')
# load from instance_settings only if args/params aren't set
if val == '' and instance_settings.contains(name):
val = instance_settings.value(name)
self.label[label_name].setText(val)
return val
def restore_settings(self, plugin_settings, instance_settings):
self.source_frame = self.get_param_setting("source_frame", "source",
self.source_frame, instance_settings)
self.target_frame = self.get_param_setting("target_frame", "target",
self.target_frame, instance_settings)
for key in self.label.keys():
name = key + '_hidden'
if instance_settings.contains(name):
if instance_settings.value(name) == str(True):
self.label[key].hide()
self.setup_menu()
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('source_frame', self.source_frame)
instance_settings.set_value('target_frame', self.target_frame)
for key in self.label.keys():
is_hidden = self.label[key].isHidden()
name = key + '_hidden'
instance_settings.set_value(name, str(is_hidden))
class MAVROSGUI(Plugin):
def __init__(self, context):
super(MAVROSGUI, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('MAVROSGUI')
rp = rospkg.RosPack()
# 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._widget = QWidget()
# Get path to UI file which is a sibling of this file
# in this example the .ui and .py file are in the same folder
ui_file = os.path.join(rp.get_path('rqt_mavros_gui'), 'resource',
'MAVROSGUI.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('MAVROSGUIUi')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). 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._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
self.mavros_namespace = "/mavros"
self._widget.button_safety_arm.clicked.connect(
self.button_safety_arm_pressed)
self._widget.button_safety_disarm.clicked.connect(
self.button_safety_disarm_pressed)
self._widget.button_param_refresh.clicked.connect(
self.button_param_refresh_pressed)
self._widget.button_param_set.clicked.connect(
self.button_param_set_pressed)
self._widget.button_flight_mode_set.clicked.connect(
self.button_flight_mode_set_pressed)
self._widget.combo_param_list.currentIndexChanged.connect(
self.combo_param_list_pressed)
self.flight_modes = sorted([
"MANUAL", "ACRO", "ALTCTL", "POSCTL", "OFFBOARD", "STABILIZED",
"RATTITUDE", "AUTO.MISSION", "AUTO.LOITER", "AUTO.RTL",
"AUTO.LAND", "AUTO.RTGS", "AUTO.READY", "AUTO.TAKEOFF"
])
for fm in self.flight_modes:
self._widget.combo_flight_mode_list.addItem(fm)
# Handled in the restore settings callback
#self.refresh_mavros_backend()
self.sub_state = None
self.timer_state = None
self.msg_state_last = rospy.Time(0)
def shutdown_plugin(self):
if self.sub_state:
self.sub_state.unregister()
if self.timer_state:
self.timer_state.shutdown()
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('namespace', self.mavros_namespace)
instance_settings.set_value(
'mode_selection',
self._widget.combo_flight_mode_list.currentText())
def restore_settings(self, plugin_settings, instance_settings):
ns = instance_settings.value('namespace')
if ns:
self.mavros_namespace = str(ns)
mode = str(instance_settings.value('mode_selection'))
if mode in self.flight_modes:
self._widget.combo_flight_mode_list.setCurrentIndex(
self.flight_modes.index(mode))
self._refresh_mavros_backend()
def trigger_configuration(self):
"""Present the user with a dialog for choosing the topic to view,
the data type, and other settings used to generate the HUD.
This displays a SimpleSettingsDialog asking the user to choose
the settings as desired.
This method is blocking"""
dialog = SimpleSettingsDialog(title='Quaternion View Options')
dialog.add_lineedit("namespace", str(self.mavros_namespace),
"Namespace")
settings = dialog.get_settings()
if settings is not None:
for s in settings:
if s[0] == "namespace":
self.mavros_namespace = str(s[1])
self._refresh_mavros_backend()
def button_safety_arm_pressed(self):
self._arm(True)
rospy.logdebug("Safety arm button pressed!")
def button_safety_disarm_pressed(self):
self._arm(False)
rospy.logdebug("Safety disarm button pressed!")
def button_param_refresh_pressed(self):
param_received = 0
try:
param_received, param_list = param_get_all(False)
rospy.logdebug("Parameters received: %s" % str(param_received))
except IOError as e:
rospy.logerr(e)
self._widget.combo_param_list.clear()
param_id_list = list()
if param_received > 0:
self._widget.textbox_param_value.setEnabled(True)
self._widget.button_param_set.setEnabled(True)
for p in param_list:
param_id_list.append(p.param_id)
for p in sorted(param_id_list):
self._widget.combo_param_list.addItem(p)
def button_param_set_pressed(self):
param_id = str(self._widget.combo_param_list.currentText())
val_str = str(self._widget.textbox_param_value.text())
if '.' in val_str:
val = float(val_str)
else:
val = int(val_str)
try:
rospy.loginfo(param_set(param_id, val))
except IOError as e:
rospy.logerr(e)
rospy.logdebug("Set param button pressed!")
def button_flight_mode_set_pressed(self):
mode_req = str(self._widget.combo_flight_mode_list.currentText())
try:
set_mode = rospy.ServiceProxy(mavros.get_topic('set_mode'),
SetMode)
ret = set_mode(base_mode=0, custom_mode=mode_req)
if not ret.mode_sent:
rospy.logerr("Request failed. Check mavros logs")
except (rospy.ServiceException, IOError) as e:
rospy.logerr(e)
rospy.logdebug("Set param button pressed!")
def combo_param_list_pressed(self, ):
param_id = self._widget.combo_param_list.currentText()
try:
if param_id:
self._widget.textbox_param_value.setText(
str(param_get(param_id)))
else:
self._widget.textbox_param_value.setText("")
except IOError as e:
rospy.logerr(e)
def _arm(self, state):
try:
ret = command.arming(value=state)
if not ret.success:
rospy.logerr("Request failed. Check mavros logs")
rospy.loginfo("Command result: %s" % str(ret.result))
except rospy.ServiceException as ex:
rospy.logerr(ex)
def _cb_state(self, msg_in):
self.msg_state_last = rospy.Time.now()
if msg_in.connected:
self._widget.label_status_live_conn.setText("Connected")
else:
self._widget.label_status_live_conn.setText("Disconnected")
if msg_in.armed:
self._widget.label_status_live_armed.setText("Armed")
self._widget.label_status_live_armed.setStyleSheet(
"QLabel{color: rgb(255, 255, 255);background-color: rgb(25, 116, 2);}"
)
else:
self._widget.label_status_live_armed.setText("Disarmed")
self._widget.label_status_live_armed.setStyleSheet(
"QLabel{color: rgb(255, 255, 255);background-color: rgb(116,2,25);}"
)
if msg_in.mode == "CMODE(0)":
self._widget.label_status_live_mode.setText("UNKNOWN")
else:
self._widget.label_status_live_mode.setText(msg_in.mode)
def _state_monitor(self, event):
if self.msg_state_last != rospy.Time(0):
if (event.current_real - self.msg_state_last) > rospy.Duration(5):
self.msg_state_last = rospy.Time(0)
self._widget.label_status_live_conn.setText("Disconnected")
self._widget.label_status_live_armed.setText("Disconnected")
self._widget.label_status_live_armed.setStyleSheet("")
self._widget.label_status_live_mode.setText("Disconnected")
def _refresh_mavros_backend(self):
mavros.set_namespace(self.mavros_namespace)
if self.sub_state:
self.sub_state.unregister()
if self.timer_state:
self.timer_state.shutdown()
#Timer should trigger immidiately
self.timer_state = rospy.Timer(rospy.Duration(1), self._state_monitor)
self.sub_state = rospy.Subscriber(self.mavros_namespace + "/state",
State, self._cb_state)
class MyPlugin(Plugin):
sig_sysmsg = Signal(str)
l_location = pyqtSignal(float, float)
r_location = pyqtSignal(float, float)
LOS_optimize_chal=pyqtSignal(str,str)
NLOS_optimize_chal=pyqtSignal(str)
com_timer = QTimer()
rssi_timer = QTimer()
baro_timer = QTimer()
ConnInd_timer = QTimer()
_pid = 0000000
# save_file_processing
def __init__(self, context):
super(MyPlugin, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('MyPlugin')
rp = rospkg.RosPack()
self._lrssivalue=0.0
self._rrssivalue=0.0
self._lcomvalue=0.0
self._rcomvalue=0.0
self._lbaroaltvalue = 0.0
self._rbaroaltvalue = 0.0
self._enable_gps=0
self._LOS_NLOS=1
self.imu_done=4
self.imu_run_stop_value=1
self.initial_heading=500
self.ros_run_stop_value=1
self.run_initial_scan=1
self.initial_scan_state=1
self.localimu=0
self.remoteimu=0
self.camera_run_save_stop_state=1
self.initial_map=1
self.rssi_test_gps_rssi=1
# 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
## def goCoords():
## def resetError():
## # coordsEdit.setStyleSheet('')
##
## try:
## print("work")
## #latitude, longitude = coordsEdit.text().split(",")
##
## except ValueError:
## #coordsEdit.setStyleSheet("color: red;")
## QTimer.singleShot(500, resetError)
## else:
## map.centerAt(latitude, longitude)
## # map.moveMarker("MyDragableMark", latitude, longitude)
def onMarkerMoved(key, latitude, longitude):
print("Moved!!", key, latitude, longitude)
if key == 'local GPS':
l_coordsEdit.setText("{0:.8}, {1:.8}".format(latitude, longitude))
else:
r_coordsEdit.setText("{0:.8}, {1:.8}".format(latitude, longitude))
#coordsEdit.setText("{}, {}".format(latitude, longitude))
#self.map.moveMarker("local GPS",latitude+0.01, longitude+0.01)
def onMarkerRClick(key):
print("RClick on ", key)
# map.setMarkerOptions(key, draggable=False)
def onMarkerLClick(key):
print("LClick on ", key)
def onMarkerDClick(key):
print("DClick on ", key)
# map.setMarkerOptions(key, draggable=True)
def onMapMoved(latitude, longitude):
print("Moved to ", latitude, longitude)
def onMapRClick(latitude, longitude):
print("RClick on ", latitude, longitude)
def onMapLClick(latitude, longitude):
print("LClick on ", latitude, longitude)
def onMapDClick(latitude, longitude):
print("DClick on ", latitude, longitude)
def move_l_mark(lat_l,lon_l):
#onMarkerMoved("local gps",lat_,lon_)
#print("get location",lat_,lon_)
self.map.moveMarker("local GPS",lat_l,lon_l)
l_coordsEdit.setText("{0:.9}, {1:.9}".format(lat_l,lon_l))
if self.initial_map==1:
self.map.centerAt(lat_l,lon_l)
self.map.setZoom(15)
self.initial_map=0
#time.sleep(0.01)
def local_callback(llocation):
llat=llocation.data[0]
llon=llocation.data[1]
#move_mark(self,lat,lon)
self.l_location.emit(llat,llon)
def move_r_mark(lat_r,lon_r):
#onMarkerMoved("local gps",lat_,lon_)
#print("get location",lat_,lon_)
self.map.moveMarker("remote GPS",lat_r,lon_r)
r_coordsEdit.setText("{0:.9}, {1:.9}".format(lat_r,lon_r))
#time.sleep(0.01)
def remote_callback(rlocation):
rlat=rlocation.data[0]
rlon=rlocation.data[1]
#move_mark(self,lat,lon)
self.r_location.emit(rlat,rlon)
@pyqtSlot()
def change_los_nos_value():
if self._LOS_NLOS==1:
self._LOS_NLOS=0
initial_heading_calculation_button.setText("Run")
else:
self._LOS_NLOS=1
initial_heading_calculation_button.setText("Scan")
print(self._LOS_NLOS)
def optimize_channel_fun():
os.system(
"sudo /home/dnc2/Documents/catkin_ws/src/directional_antenna_system/src/directional_antenna_system/GetRSSI.sh")
if self._LOS_NLOS == 1:
a = localchangechannel.local_change_channel()
print("channel a", a)
time.sleep(2)
b = remotechangechannel.remote_change_channel()
print("channel b", b)
self.LOS_optimize_chal.emit(a,b)
# channel_textedit.setText("L:%s, R:%s" % (a, b))
else:
a = localchangechannel.local_change_channel()
self.NLOS_optimize_chal.emit(a)
# channel_textedit.setText("L:%s, R:N" % a)
@pyqtSlot()
def LOS_channel_testedit(vala,valb):
channel_textedit.setText("L:%s, R:%s" % (vala, valb))
@pyqtSlot()
def NLOS_channel_testedit(vala):
channel_textedit.setText("L:%s, R:N" % vala)
@pyqtSlot()
def channel_on_click():
optimize_channel_process= threading.Thread(target=optimize_channel_fun)
optimize_channel_process.start()
# optimize_channel_process.start()
# imu calibration *******************************************
def run_calibration_fun():
print("calibration")
if (self._LOS_NLOS == 1):
# print("calibrate two IMUs")
os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/stop_IMU_calibration.sh 1")
time.sleep(7)
os.system( "/home/dnc2/Documents/catkin_ws/devel/sshnode/start_IMU_calibration.sh 1")
else:
# print("calibrate local IMU")
os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/stop_IMU_calibration.sh 0")
time.sleep(7)
os.system( "/home/dnc2/Documents/catkin_ws/devel/sshnode/start_IMU_calibration.sh 0")
@pyqtSlot()
def imu_run_stop_click():
# imu_threading = threading.Thread(target=run_calibration_fun)
imu_threading= multiprocessing.Process(target=run_calibration_fun)
if self.imu_run_stop_value==1:
print("stat threading")
IMU_run_stop_button.setText("Stop")
IMU_state_state_label.setText("Working")
time.sleep(1)
self.imu_state_timer.start(60000)
imu_threading.start()
self.imu_run_stop_value = 0
else:
IMU_run_stop_button.setText("Run")
self.imu_run_stop_value = 1
IMU_state_state_label.setText("State")
os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/stop_IMU_calibration.sh 1")
# if imu_threading.isAlive():
# self.imu_threading = threading.Thread(target=run_calibration_fun(selqf, self._LOS_NLOS))
# self.imu_threading.start()
# parent_conn, child_conn = multiprocessing.Pipe()
# self.imu_processing = multiprocessing.Process(target=run_calibration_fun)
# self.imu_processing.start()
def stat_ros_nodes():
print("stat ROS node ...")
# os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/save_rosbag.sh")
# if self._LOS_NLOS==1:
if self.initial_heading==500:
os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/stop_ROS_nodes.sh %d" % (self._LOS_NLOS))
time.sleep(7)
os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/start_ros_no_initial.sh %d " % (self._LOS_NLOS))
else:
os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/stop_ROS_nodes.sh %d" % (self._LOS_NLOS))
time.sleep(7)
os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/start_ros_with_initial.sh %d %d" %(self._LOS_NLOS,self.initial_heading))
# else:
# os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/start_local.sh %d %f" % self._LOS_NLOS,self.initial_heading)
@pyqtSlot()
def stat_ros_click():
# self.imu_timer.stop();
start_ros_node_threading = threading.Thread(target=stat_ros_nodes)
# if not self.monitor_ccq.isRunning():
# self.monitor_ccq.start()
# start_save_threading = threading.Thread(target=start_save_data)
if self.ros_run_stop_value==1:
self.ros_run_stop_value=0
Ros_node_run_button.setText("Stop")
Ros_node_state_label.setText("Working")
IMU_run_stop_button.setEnabled(False)
self.distance_timer.start(10000)
IMU_run_stop_button.setStyleSheet(
"background-color: rgb(192,197,197);padding: 6px; border-radius:5px;text_align:left")
start_ros_node_threading.start()
else:
self.ros_run_stop_value=1
# self.monitor_ccq.terminate()
Ros_node_run_button.setText("Run")
Ros_node_state_label.setText("Done")
IMU_run_stop_button.setEnabled(True)
self.distance_timer.stop()
IMU_run_stop_button.setStyleSheet(
"background-color: rgb(245,128,38);padding: 6px; border-radius:5px;text_align:left")
os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/stop_ROS_nodes.sh %d" % (self._LOS_NLOS))
# if start_ros_node_threading.isAlive():
# start_save_threading.start()
def run_camera_nodes():
if self.camera_run_save_stop_state==1:
if infrared_camera.isChecked():
camera_comman=10
elif not infrared_camera.isChecked():
camera_comman=0
if optical_camera.isChecked():
opt_camera_comman=10
elif not optical_camera.isChecked():
opt_camera_comman=0
camera_run_button.setText("Confirm")
self.camera_run_save_stop_state = 2
os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/start_camera_nodes.sh %d %d" %(camera_comman,opt_camera_comman))
elif self.camera_run_save_stop_state==2:
if save_infrared.isChecked():
save_inf_var=1
else:
save_inf_var=0
if save_optical.isChecked():
save_opt_var=1
else:
save_opt_var=0
camera_run_button.setText("Stop")
self.camera_run_save_stop_state = 3
os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/save_camera_nodes.sh %d %d" % (save_inf_var, save_opt_var))
elif self.camera_run_save_stop_state==3:
camera_run_button.setText("Confirm")
self.camera_run_save_stop_state = 1
os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/stop_camera_nodes.sh")
# if optical_camera.isChecked() and infrared_camera.isChecked():
# os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/start_camera_nodes.sh 1 1")
# elif optical_camera.isChecked() and infrared_camera.isChecked()==0:
# os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/start_camera_nodes.sh 1 0")
# elif optical_camera.isChecked()==0 and infrared_camera.isChecked()==1:
# os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/start_camera_nodes.sh 0 1")
@pyqtSlot()
def start_camera_click():
start_camera_threading=threading.Thread(target=run_camera_nodes)
if optical_camera.isChecked() or infrared_camera.isChecked():
start_camera_threading.start()
def initial_scan_run():
os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/start_initial_scan.sh")
@pyqtSlot()
def calculate_Azimuth():
if self._LOS_NLOS==1 and self.run_initial_scan==1:
initial_heading_calculation_button.setText("Stop")
self.run_initial_scan = 0
IMU_run_stop_button.setEnabled(False)
IMU_run_stop_button.setStyleSheet("background-color: rgb(192,197,197);padding: 6px; border-radius:5px;text_align:left")
initial_scan_threading =threading.Thread(target=initial_scan_run)
initial_scan_threading.start()
elif self._LOS_NLOS ==1 and self.run_initial_scan==0:
initial_heading_calculation_button.setText("Run")
self.run_initial_scan = 1
IMU_run_stop_button.setEnabled(True)
IMU_run_stop_button.setStyleSheet(
"background-color: rgb(245,128,38);padding: 6px; border-radius:5px;text_align:left")
# os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/stop_initial_scan.sh")
if self._LOS_NLOS==0:
if l_coordsEdit.text() and l_coordsEdit.text().find(",") !=-1:
llocation_string = l_coordsEdit.text()
print(llocation_string)
l_lat_lon = llocation_string.split(",")
try:
l_latitiude = float(l_lat_lon[0])
l_longitude = float(l_lat_lon[1])
except:
Azimuth_warning()
return
if r_coordsEdit.text() and r_coordsEdit.text().find(",") != -1:
rlocation_string = r_coordsEdit.text()
print(rlocation_string)
r_lat_lon = rlocation_string.split(",")
try:
r_latitiude = float(r_lat_lon[0])
r_longitude = float(r_lat_lon[1])
temp_remote_gps_msg.data= [r_latitiude, r_longitude]
temp_remote_gps.publish(temp_remote_gps_msg)
# print("temp_gps_test1")
# temp_remote_gps.publish(temp_remote_gps_msg)
# print("temp_gps_test")
except:
Azimuth_warning()
return
try:
LON1 = l_longitude*0.0174532925199433
LAT1 = l_latitiude*0.0174532925199433
LON2 = r_longitude*0.0174532925199433
LAT2 = r_latitiude*0.0174532925199433
az = math.atan2(math.sin(LON2-LON1) * math.cos(LAT2),math.cos(LAT1) * math.sin(LAT2) - math.sin(LAT1) * math.cos(LAT2) * math.cos(LON2-LON1))
az = math.fmod(az, 9.118906528)
az = math.fmod(az, 6.283185307179586)*(57.2957795131)
if az<0 :
az = az+360
Azimuth =az
except:
Azimuth_warning()
return
initial_heading_display_label.setText(" %0.1f"%Azimuth)
self.initial_heading=int(Azimuth)
@pyqtSlot()
def calculate_distance():
if l_coordsEdit.text() and l_coordsEdit.text().find(",") != -1:
llocation_string = l_coordsEdit.text()
l_lat_lon = llocation_string.split(",")
try:
l_latitiude = float(l_lat_lon[0])
l_longitude = float(l_lat_lon[1])
except:
# Azimuth_warning()
# print("error1")
return
if r_coordsEdit.text() and r_coordsEdit.text().find(",") != -1:
rlocation_string = r_coordsEdit.text()
r_lat_lon = rlocation_string.split(",")
try:
r_latitiude = float(r_lat_lon[0])
r_longitude = float(r_lat_lon[1])
except:
# Azimuth_warning()
# print("error2")
return
try:
LON1 = l_longitude * 0.0174532925199433
LAT1 = l_latitiude * 0.0174532925199433
LON2 = r_longitude * 0.0174532925199433
LAT2 = r_latitiude * 0.0174532925199433
A =math.sin((LAT2-LAT1)/2)*math.sin((LAT2-LAT1)/2)+math.cos(LAT1)*math.cos(LAT2)*math.sin((LON2-LON1)/2)*math.sin((LON2-LON1)/2)
# print('A',A)
AB= 2*math.atan2(math.sqrt(A),math.sqrt(1-A))
# print('AB',AB)
AB_DIS=6371000*AB
# print('AB_DIS',AB_DIS)
# distance_text.setText("%0.1f m"%AB_DIS)
self.distance_label.setText("%0.1f m"%AB_DIS)
# distance_lable.setText("0.1f m"%AB_DIS)
except:
# print("error3")
# Azimuth_warning()
return
def Azimuth_warning():
msg = QMessageBox()
msg.setIcon(QMessageBox.Warning)
msg.setWindowTitle("Azimuth calculation")
msg.setText("The locations of local and remote UAVs cannot be empty.\nPlease input locations(latitude,longitude) correctly!")
msg.exec_()
# @pyqtSlot()
# def pub_new_Azimuth():
# try:
# N_Azimuth = float(Azimuth_data.text())
# except:
# Azimuth_warning()
# return
# if math.fabs(N_Azimuth)>= 360:
# Azimuth_warning()
# return
# pub_Azimuth.publish(N_Azimuth)
@pyqtSlot()
def change_baroalt():
# self._rrssivalue=str(rssi2)
r_baro_text.setText("%.1f" % self._rbaroaltvalue)
l_baro_text.setText("%.1f" % self._lbaroaltvalue)
time.sleep(0.01)
# time.sleep(0.01)
def l_baro_callback(lbaroinfo):
self._lbaroaltvalue = lbaroinfo.data[2]
def r_baro_callback(rbaroinfo):
self._rbaroaltvalue = rbaroinfo.data[2]
@pyqtSlot()
def change_rssi():
#self._rrssivalue=str(rssi2)
r_rssi_text.setText(str(self._rrssivalue))
l_rssi_text.setText(str(self._lrssivalue))
time.sleep(0.01)
#time.sleep(0.01)
def l_rssi_callback(rssil):
self._lrssivalue=rssil.data
# self.l_rssi.emit(rssil.data)
# del rssil
#l_rssi_text.update()
#l_rssi_text.setText(str(rssi.data))
def r_rssi_callback(rssir):
self._rrssivalue=rssir.data
# self.r_rssi.emit(rssir.data)
# self.memory_tracker.print_diff()
# print(sys.getsizeof(move_l_compass(headidng1)))
#r_rssi_text.setText(str(rssi.data))
@pyqtSlot()
def move_compass():
l_com_text.setText("%.1f" % self._lcomvalue)
time.sleep(0.01)
# print('work')
l_com_widget.setAngle(self._lcomvalue)
time.sleep(0.01)
r_com_text.setText("%.1f" % self._rcomvalue)
time.sleep(0.01)
r_com_widget.setAngle(self._rcomvalue)
time.sleep(0.01)
# def map_rssi_fun_click():
# if self.rssi_test_gps_rssi==1:
# for i in range(5):
# pub_gps_Azimuth_enable.publish(0)
# time.sleep(0.2)
# self.rssi_test_gps_rssi=0
# else:
# for i in range(5):
# pub_gps_Azimuth_enable.publish(1)
# time.sleep(0.2)
# self.rssi_test_gps_rssi = 1
def l_com_callback(heading):
self._lcomvalue=heading.data
# print('work1')
#l_com_text.setText("%.1f" % heading.data)
# self.l_com.emit(heading.data)
# heading.data =None
#self.update()
#l_com_widget.setAngle(heading.data)
def r_com_callback(heading):
self._rcomvalue=heading.data
#r_com_text.setText("%.1f" % heading.data)
# self.r_com.emit(heading.data)
# del heading
#r_com_widget.setAngle(heading.data)
def local_imucal_callback(l_imu_msg):
self.localimu=l_imu_msg.data
# if self.localimu == 1 and self.remoteimu == 1:
# imu_lock.acquire()
# try:
# IMU_state_state_label.setText("Done")
# finally:
# imu_lock.release()
def remote_imucal_callback(r_imu_msg):
self.remoteimu=r_imu_msg.data
# if self.localimu == 1 and self.remoteimu == 1:
# imu_lock.acquire()
# try:
# IMU_state_state_label.setText("Done")
# finally:
# imu_lock.release()
@pyqtSlot()
def imu_state_display():
if self._LOS_NLOS==1:
if self.remoteimu==1 and self.localimu==1:
IMU_state_state_label.setText("Done")
self.imu_state_timer.stop()
else:
if self.localimu == 1:
IMU_state_state_label.setText("Done")
self.imu_state_timer.stop()
@pyqtSlot()
def local_bbb_conn(var):
if var ==1:
lbbb_conn_icon.setPixmap(green_icon)
else:
lbbb_conn_icon.setPixmap(red_icon)
@pyqtSlot()
def remote_bbb_conn(var):
if var==1:
rbbb_conn_icon.setPixmap(green_icon)
else:
rbbb_conn_icon.setPixmap(red_icon)
@pyqtSlot()
def local_connection_indicator(): # var: RSSI
l_var = self._lrssivalue
r_var = self._rrssivalue
#r_var = -55
#l_var = -75
if l_var>=-60 and l_var<0:
l_con_ind_icon.setPixmap(ConnectionBar5)
elif l_var<-60 and l_var>=-70:
l_con_ind_icon.setPixmap(ConnectionBar4)
elif l_var <-70 and l_var>= -80:
l_con_ind_icon.setPixmap(ConnectionBar3)
elif l_var <-80 and l_var > -96:
l_con_ind_icon.setPixmap(ConnectionBar2)
else:
l_con_ind_icon.setPixmap(ConnectionBar1)
if r_var >= -60 and l_var<0:
r_con_ind_icon.setPixmap(ConnectionBar5)
elif r_var < -60 and r_var >= -70:
r_con_ind_icon.setPixmap(ConnectionBar4)
elif r_var < -70 and r_var >= -80:
r_con_ind_icon.setPixmap(ConnectionBar3)
elif r_var < -80 and r_var > -96:
r_con_ind_icon.setPixmap(ConnectionBar2)
else:
r_con_ind_icon.setPixmap(ConnectionBar1)
# def local_connection_indicator(): # var: RSSI
# #var = self._rrssivalue
# var = -55
# if var>=-60:
# l_con_ind_icon.setPixmap(ConnectionBar5)
# elif var<-60 and var>=-70:
# l_con_ind_icon.setPixmap(ConnectionBar4)
# elif var <-70 and var>= -80:
# l_con_ind_icon.setPixmap(ConnectionBar3)
# elif var <-80 and var > -96:
# l_con_ind_icon.setPixmap(ConnectionBar2)
# else:
# l_con_ind_icon.setPixmap(ConnectionBar1)
self._widget = QWidget()
gcv=QVBoxLayout(self._widget)
v = QVBoxLayout()
h = QHBoxLayout()
FUN_FONT= QFont()
FUN_FONT.setBold(True)
FUN_FONT.setPointSize(14)
FUN_FONT.setWeight(87)
pub_gps_Azimuth_enable = rospy.Publisher('lenablegpsazimuth', Int32, queue_size=10)
temp_remote_gps = rospy.Publisher('temp_gps_location', Float64MultiArray,queue_size=1)
temp_remote_gps_msg= Float64MultiArray()
#ros_threading
#############################
# star_ros_threading = threading.Thread(target= stat_ros_nodes)
# # save_file_processing = multiprocessing.Process(target= save_topic_fun)
# save_file_processing = threading.Thread(target= save_topic_fun)
# star_ros_manual_com_threading = threading.Thread(target= stat_ros_manual_com_nodes)
# imu_threading = threading.Thread(target=run_calibration_fun)
green_icon = QPixmap(("/home/dnc2/Documents/catkin_ws/src/directional_antenna_system/src/directional_antenna_system/greenenergy.png"))
green_icon = green_icon.scaled(40, 40, Qt.KeepAspectRatio)
red_icon = QPixmap(("/home/dnc2/Documents/catkin_ws/src/directional_antenna_system/src/directional_antenna_system/redenergy.png"))
red_icon = red_icon.scaled(40, 40, Qt.KeepAspectRatio)
ConnectionBar1 = QPixmap(("/home/dnc2/Documents/catkin_ws/src/directional_antenna_system/src/directional_antenna_system/ConnectionIndex1.png"))
ConnectionBar2 = QPixmap(("/home/dnc2/Documents/catkin_ws/src/directional_antenna_system/src/directional_antenna_system/ConnectionIndex2.png"))
ConnectionBar3 = QPixmap(("/home/dnc2/Documents/catkin_ws/src/directional_antenna_system/src/directional_antenna_system/ConnectionIndex3.png"))
ConnectionBar4 = QPixmap(("/home/dnc2/Documents/catkin_ws/src/directional_antenna_system/src/directional_antenna_system/ConnectionIndex4.png"))
ConnectionBar5 = QPixmap(("/home/dnc2/Documents/catkin_ws/src/directional_antenna_system/src/directional_antenna_system/ConnectionIndex5.png"))
ConnectionBar1 = ConnectionBar1.scaled(50, 110, Qt.KeepAspectRatio)
ConnectionBar2 = ConnectionBar2.scaled(50, 110, Qt.KeepAspectRatio)
ConnectionBar3 = ConnectionBar3.scaled(50, 110, Qt.KeepAspectRatio)
ConnectionBar4 = ConnectionBar4.scaled(50, 110, Qt.KeepAspectRatio)
ConnectionBar5 = ConnectionBar5.scaled(50, 110, Qt.KeepAspectRatio)
##################################
# local widget
local_widget = QWidget()
local_Vlayout = QVBoxLayout(local_widget)
local_widget.setStyleSheet("background-color: white")
l_coordsEdit = QLineEdit()
l_coordsEdit.setFont(FUN_FONT)
l_coordsEdit.setMinimumWidth(230)
lgps_label = QLabel('GPS:')
lgps_label.setFont(FUN_FONT)
l_com_widget = CompassWidget()
l_rssi_lable = QLabel('RSSI:')
l_rssi_lable.setFont(FUN_FONT)
l_rssi_text = QLineEdit()
l_rssi_text.setFont(FUN_FONT)
l_rssi_text.setMaximumWidth(70)
l_baro_lable = QLabel('Alt:')
l_baro_lable.setFont(FUN_FONT)
l_baro_text = QLineEdit()
l_baro_text.setFont(FUN_FONT)
l_baro_text.setMaximumWidth(80)
l_com_label = QLabel('Antenna Heading:')
l_com_label.setFont(FUN_FONT)
l_com_text = QLineEdit()
l_com_text.setFont(FUN_FONT)
# l_com_text.setText(self._lcomvalue)
l_com_text.setMaximumWidth(80)
l_h1 = QHBoxLayout()
l_h1.addStretch()
l_h1.addWidget(l_rssi_lable)
l_h1.addWidget(l_rssi_text)
l_h1.addWidget(l_com_label)
l_h1.addWidget(l_com_text)
l_h1.addStretch()
l_h2 = QHBoxLayout()
l_h2.addStretch()
l_h2.addWidget(lgps_label)
l_h2.addWidget(l_coordsEdit)
l_h2.addWidget(l_baro_lable)
l_h2.addWidget(l_baro_text)
l_h2.addStretch()
# l_h1.setSpacing(0)
local_Vlayout.addWidget(l_com_widget)
local_Vlayout.addLayout(l_h1)
local_Vlayout.addLayout(l_h2)
local_widget.setMinimumSize(450,300)
local_widget.setMaximumWidth(500)
lbbb_conn_icon=QLabel(l_com_widget)
lbbb_conn_icon.setText("ICON")
lbbb_conn_icon.setGeometry(380,200,40,40)
lbbb_conn_icon.setPixmap(red_icon)
l_con_ind_icon = QLabel(l_com_widget)
l_con_ind_icon.setText("ICON")
l_con_ind_icon.setGeometry(380,40,50,110)
l_con_ind_icon.setPixmap(ConnectionBar1)
##################################
# remote widget
remote_widget = QWidget()
remote_vlayout = QVBoxLayout(remote_widget)
remote_widget.setStyleSheet("background-color: white")
r_coordsEdit = QLineEdit()
r_coordsEdit.setFont(FUN_FONT)
r_coordsEdit.setMinimumWidth(230)
rgps_label = QLabel('GPS:')
rgps_label.setFont(FUN_FONT)
r_com_widget = RCompassWidget()
r_rssi_lable=QLabel('RSSI')
r_rssi_lable.setFont(FUN_FONT)
r_rssi_text=QLineEdit()
r_rssi_text.setFont(FUN_FONT)
r_rssi_text.setMaximumWidth(70)
r_baro_lable = QLabel('Alt:')
r_baro_lable.setFont(FUN_FONT)
r_baro_text = QLineEdit()
r_baro_text.setFont(FUN_FONT)
r_baro_text.setMaximumWidth(80)
r_com_label=QLabel('Antenna Heading:')
r_com_label.setFont(FUN_FONT)
r_com_text=QLineEdit()
r_com_text.setFont(FUN_FONT)
r_com_text.setMaximumWidth(80)
r_h1 = QHBoxLayout()
r_h1.addStretch()
r_h1.addWidget(r_rssi_lable)
r_h1.addWidget(r_rssi_text)
r_h1.addWidget(r_com_label)
r_h1.addWidget(r_com_text)
r_h1.addStretch()
r_h2 =QHBoxLayout()
r_h2.addStretch()
r_h2.addWidget(rgps_label)
r_h2.addWidget(r_coordsEdit)
r_h2.addWidget(r_baro_lable)
r_h2.addWidget(r_baro_text)
r_h2.addStretch()
remote_vlayout.addWidget(r_com_widget)
remote_vlayout.addLayout(r_h1)
remote_vlayout.addLayout(r_h2)
remote_widget.setMinimumSize(450,300)
# remote_widget.setMinimumSize(local_widget.width())
rbbb_conn_icon = QLabel(r_com_widget)
rbbb_conn_icon.setText("ICON")
rbbb_conn_icon.setGeometry(380, 200, 40, 40)
rbbb_conn_icon.setPixmap(red_icon)
r_con_ind_icon = QLabel(r_com_widget)
r_con_ind_icon.setText("ICON")
r_con_ind_icon.setGeometry(380, 40, 50, 110)
r_con_ind_icon.setPixmap(ConnectionBar1)
##################################
#distance and CCQ
distance_lable=QLabel("Distance")
distance_text=QLabel("")
CCQ_lable=QLabel("Transmit CCQ: ")
CCQ_text=QLabel()
DIS_CCQ_widget= QWidget()
DIS_CCQ_layout=QHBoxLayout(DIS_CCQ_widget)
DIS_CCQ_layout.addStretch(1)
DIS_CCQ_layout.addWidget(distance_lable)
DIS_CCQ_layout.addWidget(distance_text)
DIS_CCQ_layout.addStretch(2)
DIS_CCQ_layout.addWidget(CCQ_lable)
DIS_CCQ_layout.addWidget(CCQ_text)
DIS_CCQ_layout.addStretch(1)
DIS_CCQ_widget.setStyleSheet("background-color: white")
# functional buttons
func_widget = QWidget()
Func_widget_layout= QGridLayout(func_widget)
# Func_widget_layout.setRowStretch(0,1)
Func_widget_layout.setHorizontalSpacing(5)
# Func_widget_layout.setSpacing(1)
distan_option_label= QLabel("Distance Option: ")
LOS = QRadioButton("LOS")
LOS.setChecked(True)
NLOS = QRadioButton("NLOS")
NLOS.setChecked(False)
channel_selection_label= QLabel("Channel Selection: ")
channel_textedit= QLineEdit("CH")
channel_optimize= QPushButton("Optimize")
self.LOS_optimize_chal.connect(LOS_channel_testedit)
self.NLOS_optimize_chal.connect(NLOS_channel_testedit)
IMU_calibration_label= QLabel("IMU Calibration: ")
IMU_run_stop_button = QPushButton("Run")
# IMU_run_stop_button.seta
IMU_state_state_label= QLineEdit("state")
initial_heading_label=QLabel("Initial Heading: ")
initial_heading_display_label= QLineEdit("Default")
initial_heading_calculation_button = QPushButton("Scan")
ROS_node_label= QLabel("ROS Nodes: ")
Ros_node_run_button= QPushButton("Run")
Ros_node_state_label= QLineEdit("state")
camera_label =QLabel("Cameras in Use: ")
camera_group_widget=QWidget()
camera_group=QGridLayout(camera_group_widget)
# camera_group.setAlignment(Qt.AlignLeft)
optical_camera=QCheckBox("Optical")
optical_camera.setChecked(True)
infrared_camera=QCheckBox("Infrared")
infrared_camera.setChecked(True)
save_infrared=QCheckBox("Save")
save_optical=QCheckBox("Save")
camera_group.addWidget(optical_camera,0,0)
camera_group.addWidget(save_optical,0,1)
camera_group.addWidget(infrared_camera,1,0)
camera_group.addWidget(save_infrared,1,1)
camera_run_button= QPushButton("Confirm")
#map_rssi_button=QPushButton("RSSI_MAPING")
Func_widget_layout.addWidget(distan_option_label,0,0)
Func_widget_layout.addWidget(LOS,0,1)
Func_widget_layout.addWidget(NLOS,0,2)
Func_widget_layout.addWidget(channel_selection_label,1,0)
Func_widget_layout.addWidget(channel_textedit,1,2)
Func_widget_layout.addWidget(channel_optimize,1,1)
Func_widget_layout.addWidget(IMU_calibration_label,2,0)
Func_widget_layout.addWidget(IMU_run_stop_button,2,1)
Func_widget_layout.addWidget(IMU_state_state_label,2,2)
Func_widget_layout.addWidget(initial_heading_label,3,0)
Func_widget_layout.addWidget(initial_heading_display_label,3,2)
Func_widget_layout.addWidget(initial_heading_calculation_button,3,1)
Func_widget_layout.addWidget(ROS_node_label,4,0)
Func_widget_layout.addWidget(Ros_node_run_button,4,1)
Func_widget_layout.addWidget(Ros_node_state_label,4,2)
Func_widget_layout.addWidget(camera_label, 5,0)
Func_widget_layout.addWidget(camera_group_widget,5,1)
Func_widget_layout.addWidget(camera_run_button,5,2)
#Func_widget_layout.addWidget(map_rssi_button, 6,0)
Func_widget_layout.setColumnMinimumWidth(2,70)
# Func_widget_layout.set
func_widget.setStyleSheet("color:white;font:bold 15px")
# font1 = QFont()
# font1.setPointSize(20)
# # font1.setPixelSize(30)
# # font1.setBold(True)
# font1.setWeight(75)
LOS.setStyleSheet("background-color: rgb(245,128,38);padding: 6px; border-radius:5px")
channel_optimize.setStyleSheet("background-color: rgb(245,128,38);padding: 6px; border-radius:5px")
# channel_textedit.setStyleSheet("background-color: rgb(245,128,38)")
NLOS.setStyleSheet("background-color: rgb(245,128,38);padding: 6px; border-radius:5px")
camera_run_button.setStyleSheet("background-color: rgb(245,128,38);padding: 6px; border-radius:5px")
IMU_run_stop_button.setStyleSheet("background-color: rgb(245,128,38);padding: 6px; border-radius:5px;text_align:left")
# IMU_state_state_label.setStyleSheet("background-color: rgb(245,128,38)")
initial_heading_calculation_button.setStyleSheet("background-color: rgb(245,128,38);padding: 6px; border-radius:5px")
Ros_node_run_button.setStyleSheet("background-color: rgb(245,128,38);padding: 6px; border-radius:5px;text_align:left")
camera_run_button.setStyleSheet("background-color: rgb(245,128,38);padding: 6px; border-radius:5px")
#map_rssi_button.setStyleSheet("background-color: rgb(245,128,38);padding: 6px; border-radius:5px")
# camera_group.setStyleSheet("font:bold 11px")
###############################
LOS.toggled.connect(change_los_nos_value)
channel_optimize.clicked.connect(channel_on_click)
IMU_run_stop_button.clicked.connect(imu_run_stop_click)
initial_heading_calculation_button.clicked.connect(calculate_Azimuth)
Ros_node_run_button.clicked.connect(stat_ros_click)
camera_run_button.clicked.connect(start_camera_click)
#map_rssi_button.clicked.connect(map_rssi_fun_click)
# channel_button.clicked.connect(channel_on_click)
# ROS_button.clicked.connect(stat_ros_click)
# save_button.clicked.connect(save_file_click)
# Azimuth_button.clicked.connect(calculate_Azimuth)
# New_Azimuth_button.clicked.connect(pub_new_Azimuth)
# Enable_gps_Azimuth.clicked.connect(enable_disable_gps_Azimuth)
# manual_com.clicked.connect(stat_ros_manual_com_click)
mid_layout=QHBoxLayout()
#mid_layout.setSpacing(20)
mid_layout.addWidget(local_widget)
mid_layout.addWidget(remote_widget)
top_layout=QVBoxLayout()
# top_layout.addWidget(DIS_CCQ_widget)
top_layout.addLayout(mid_layout)
# top_layout.setStretchFactor(DIS_CCQ_layout,1)
# top_layout.setStretchFactor(mid_layout,9)
com_fun_layout= QHBoxLayout()
com_fun_layout.addLayout(top_layout)
com_fun_layout.addWidget(func_widget)
com_fun_layout.setStretchFactor(mid_layout,5)
com_fun_layout.setStretchFactor(func_widget,1.5)
# mid_layout.addWidget(func_widget)
# mid_layout.setStretchFactor(local_widget,4)
# mid_layout.setStretchFactor(remote_vlayout,4)
# mid_layout.setStretchFactor(func_widget,2)
# mid_layout.addStretch(1)
# gcv.addLayout(mid_layout)
gcv.addLayout(com_fun_layout)
# l.addRow('Coords:', coordsEdit)
# l.addRow('lcoords:',r_coordsEdit)
#h.addLayout(v)
#l = QFormLayout()
#h.addLayout(l)
#coordsEdit = QLineEdit()
#l.addRow('Coords:', coordsEdit)
#coordsEdit.editingFinished.connect(goCoords)
self.map = QOSM(self._widget)
self.map.mapMoved.connect(onMapMoved)
self.map.markerMoved.connect(onMarkerMoved)
self.map.mapClicked.connect(onMapLClick)
self.map.mapDoubleClicked.connect(onMapDClick)
self.map.mapRightClicked.connect(onMapRClick)
self.map.markerClicked.connect(onMarkerLClick)
self.map.markerDoubleClicked.connect(onMarkerDClick)
self.map.markerRightClicked.connect(onMarkerRClick)
self.l_location.connect(move_l_mark)
self.r_location.connect(move_r_mark)
self.distance_widget=QWidget(self.map)
self.distance_widget_layout=QHBoxLayout(self.distance_widget)
self.distance_widget_layout.addWidget(QLabel("Distance: "))
self.distance_label = QLabel()
self.distance_label.setText("the distance")
self.distance_widget_layout.addWidget(self.distance_label)
self.distance_widget.setGeometry(50,10,180,30)
self.distance_widget.setStyleSheet("background-color: white")
# self.distance_label.setGeometry(50,10,120,30)
# self.distance_label.setStyleSheet("background-color: rgb(245,128,38);padding: 6px; border-radius:5px")
# self.distance_label.setGeometry()
# self.distance_dock= QDockWidget()
# self.distance_dock.setWidget(self.distance_label)
# self.distance_dock.setFloating(True)
# self.addDockWidget(Qt.LeftDockWidgetArea,self.distance_dock)
# self._widget.addDockWidget(Qt.LeftDockWidgetArea, self.distance_dock)
gcv.addWidget(self.map)
gcv.setStretchFactor(com_fun_layout, 5)
gcv.setStretchFactor(self.map, 4)
self.map.setSizePolicy(
QSizePolicy.MinimumExpanding,
QSizePolicy.MinimumExpanding)
# l_com_timer = QTimer()
# self.l_com_timer.setSingleShot(True)
self.com_timer.timeout.connect(move_compass)
self.com_timer.start(200)
self.rssi_timer.timeout.connect(change_rssi)
self.rssi_timer.start(1000)
self.baro_timer.timeout.connect(change_baroalt)
self.baro_timer.start(1000)
self.ConnInd_timer.timeout.connect(local_connection_indicator)
self.ConnInd_timer.start(1000)
self.imu_state_timer=QTimer()
self.imu_state_timer.setInterval(2000)
# self.imu_state_timer.interval(2000)
self.imu_state_timer.timeout.connect(imu_state_display)
self.distance_timer = QTimer()
self.distance_timer.setInterval(1000)
self.distance_timer.timeout.connect(calculate_distance)
self.distance_timer.start()
# self.monitor_ccq = CCQ_threading()
self.CONN_state=conn_Test()
self.CONN_state.lBBB_conn.connect(local_bbb_conn)
self.CONN_state.rBBB_conn.connect(remote_bbb_conn)
self.CONN_state.start()
# self.memory_tracker=tracker.SummaryTracker(o)
# Get path to UI file which is a sibling of this file
# in this example the .ui and .py file are in the same folder
ui_file = os.path.join(rp.get_path('gps_com_node_v2'), 'resource', 'MyPlugin.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
self._widget.setStyleSheet("background-color:rgb(0,68,124)")
# Give QObjects reasonable names
self._widget.setObjectName('MyPluginUi')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). 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._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
self.map.waitUntilReady()
self.map.centerAt(32.731263, -97.114334)
#self.map.centerAt(38.833005, -77.117415)
self.map.setZoom(12)
# Many icons at: https://sites.google.com/site/gmapsdevelopment/
coords = self.map.center()
self.map.addMarker("local GPS", *coords, **dict(
icon="file:///home/dnc2/Documents/catkin_ws/src/gps_com_node _v2/src/gps_com_node_v2/local_uav.png",
draggable=True,
title="locat GPS marker!"
))
coords = coords[0] , coords[1]
self.map.addMarker("remote GPS", *coords, **dict(
# icon="https://thumb.ibb.co/bvE9FT/remote_uav.png",
icon= "file:///home/dnc2/Documents/catkin_ws/src/gps_com_node _v2/src/gps_com_node_v2/remote_uav.png",
draggable=True,
title="remote GPS marker"
))
sub1 = rospy.Subscriber("/local_gps", Float64MultiArray, local_callback)
sub2 = rospy.Subscriber("/remote_gps", Float64MultiArray, remote_callback)
sub3=rospy.Subscriber("/local_rssi",Int32,l_rssi_callback)
sub4=rospy.Subscriber("/remote_rssi",Int32,r_rssi_callback)
sub5=rospy.Subscriber("/local_com",Float64,l_com_callback)
sub6=rospy.Subscriber("/remote_com",Float64,r_com_callback)
sub7=rospy.Subscriber("/local_imucal_msg", Int32, local_imucal_callback)
sub8=rospy.Subscriber("/remote_imucal_msg", Int32, remote_imucal_callback)
sub9=rospy.Subscriber("/local_baro",Float64MultiArray,l_baro_callback)
sub10=rospy.Subscriber("/remote_baro",Float64MultiArray,r_baro_callback)
#time.sleep(10)
def shutdown_plugin(self):
# TODO unregister all publishers here
# self.save_file_processing.terminate()
# os.system("kill ")
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
class VacuumGripper(Plugin):
def __init__(self, context):
super(VacuumGripper, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('VacuumGripper')
# 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)
rospy.loginfo("Opening GUI")
# Create QWidget
self._widget = QWidget()
# Get path to UI file which should be in the "resource" folder of this package
ui_file = os.path.join(rospkg.RosPack().get_path('rqt_industrial_robot'), 'resources', 'VacuumGripper.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('VacuumGripper')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). 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._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
rospy.Subscriber("/joint_states", JointState, self.jointstate_callback)
self.jointstate_pub = rospy.Publisher("/joint_states", JointState, queue_size=0)
self.robot = RobotWrapper()
filename = os.path.join(rospkg.RosPack().get_path('rqt_industrial_robot'), 'src','rqt_vacuum_gripper', 'joints_setup.yaml')
with open(filename) as file:
joints_setup = yaml.load(file)
jointslimit = joints_setup["joints_limit"]
self._widget.xEdit.editingFinished.connect(self.set_x)
self._widget.yEdit.editingFinished.connect(self.set_y)
self._widget.zEdit.editingFinished.connect(self.set_z)
self._widget.rollEdit.editingFinished.connect(self.set_roll)
self._widget.pitchEdit.editingFinished.connect(self.set_pitch)
self._widget.yawEdit.editingFinished.connect(self.set_yaw)
self._widget.planButton.clicked.connect(self.plan)
self._widget.executeButton.clicked.connect(self.execute)
self._widget.planexeButton.clicked.connect(self.planexe)
self._widget.stopexeButton.clicked.connect(self.stopexe)
self._widget.homeButton.clicked.connect(self.backtohome)
self._widget.jointSlider_1.sliderReleased.connect(self.setjoint1)
self._widget.jointSlider_1.valueChanged.connect(self.viewjoint1)
self._widget.jointSlider_1.setMinimum(jointslimit["joint_1"]["low"])
self._widget.jointSlider_1.setMaximum(jointslimit["joint_1"]["high"])
self._widget.joint1Browser.append(str(round(numpy.rad2deg(self.robot.get_joint_value(1)),2))+DEG)
self._widget.joint1_low.setText(str(jointslimit["joint_1"]["low"])+DEG)
self._widget.joint1_high.setText(str(jointslimit["joint_1"]["high"])+DEG)
self._widget.jointSlider_2.sliderReleased.connect(self.setjoint2)
self._widget.jointSlider_2.valueChanged.connect(self.viewjoint2)
self._widget.jointSlider_2.setMinimum(jointslimit["joint_2"]["low"])
self._widget.jointSlider_2.setMaximum(jointslimit["joint_2"]["high"])
self._widget.joint2Browser.append(str(round(numpy.rad2deg(self.robot.get_joint_value(2)),2))+DEG)
self._widget.joint2_low.setText(str(jointslimit["joint_2"]["low"])+DEG)
self._widget.joint2_high.setText(str(jointslimit["joint_2"]["high"])+DEG)
self._widget.jointSlider_3.sliderReleased.connect(self.setjoint3)
self._widget.jointSlider_3.valueChanged.connect(self.viewjoint3)
self._widget.jointSlider_3.setMinimum(jointslimit["joint_3"]["low"])
self._widget.jointSlider_3.setMaximum(jointslimit["joint_3"]["high"])
self._widget.joint3Browser.append(str(round(numpy.rad2deg(self.robot.get_joint_value(3)),2))+DEG)
self._widget.joint3_low.setText(str(jointslimit["joint_3"]["low"])+DEG)
self._widget.joint3_high.setText(str(jointslimit["joint_3"]["high"])+DEG)
self._widget.jointSlider_4.sliderReleased.connect(self.setjoint4)
self._widget.jointSlider_4.valueChanged.connect(self.viewjoint4)
self._widget.jointSlider_4.setMinimum(jointslimit["joint_4"]["low"])
self._widget.jointSlider_4.setMaximum(jointslimit["joint_4"]["high"])
self._widget.joint4Browser.append(str(round(numpy.rad2deg(self.robot.get_joint_value(4)),2))+DEG)
self._widget.joint4_low.setText(str(jointslimit["joint_4"]["low"])+DEG)
self._widget.joint4_high.setText(str(jointslimit["joint_4"]["high"])+DEG)
self._widget.jointSlider_5.sliderReleased.connect(self.setjoint5)
self._widget.jointSlider_5.valueChanged.connect(self.viewjoint5)
self._widget.jointSlider_5.setMinimum(jointslimit["joint_5"]["low"])
self._widget.jointSlider_5.setMaximum(jointslimit["joint_5"]["high"])
self._widget.joint5Browser.append(str(round(numpy.rad2deg(self.robot.get_joint_value(5)),2))+DEG)
self._widget.joint5_low.setText(str(jointslimit["joint_5"]["low"])+DEG)
self._widget.joint5_high.setText(str(jointslimit["joint_5"]["high"])+DEG)
self._widget.jointSlider_6.sliderReleased.connect(self.setjoint6)
self._widget.jointSlider_6.valueChanged.connect(self.viewjoint6)
self._widget.jointSlider_6.setMinimum(jointslimit["joint_6"]["low"])
self._widget.jointSlider_6.setMaximum(jointslimit["joint_6"]["high"])
self._widget.joint6Browser.append(str(round(numpy.rad2deg(self.robot.get_joint_value(6)),2))+DEG)
self._widget.joint6_low.setText(str(jointslimit["joint_6"]["low"])+DEG)
self._widget.joint6_high.setText(str(jointslimit["joint_6"]["high"])+DEG)
self._widget.GraspButton.clicked.connect(self.gripper_grasp)
self._widget.ReleaseButton.clicked.connect(self.gripper_release)
self._widget.rvizButton.clicked.connect(self.launchrviz)
self._widget.start_button.clicked.connect(self.playClicked)
self._widget.stop_button.clicked.connect(self.stopClicked)
self._widget.pause_button.clicked.connect(self.pauseClicked)
self._widget.restart_button.clicked.connect(self.restartClicked)
self._widget.updatefkButton.clicked.connect(self.updatefk)
self._widget.updateikButton.clicked.connect(self.updateik)
self.updatefk()
self.updateik()
self._widget.respawnButton.clicked.connect(self.respawn_all_objects)
rospy.Subscriber("/updatepose", Bool, self.updatepose)
rospy.Subscriber("/gui_message", String, self.browser_callback)
self.message_pub = rospy.Publisher("/gui_message", String, queue_size=0)
self.updatepose_pub = rospy.Publisher("/updatepose", Bool, queue_size=0)
self.startalgorithm_pub = rospy.Publisher("/start_algorithm", Bool, queue_size=0)
self.stopalgorithm_pub = rospy.Publisher("/stop_algorithm", Bool, queue_size=0)
self.pausealgorithm_pub = rospy.Publisher("/pause_algorithm", Bool, queue_size=0)
self.startalgorithm_sub = rospy.Subscriber("/start_algorithm", Bool, self.startalgorithm_callback)
self.stopalgorithm_sub = rospy.Subscriber("/stop_algorithm", Bool, self.stopalgorithm_callback)
self.algorithm_is_on = False
def startalgorithm_callback(self, msg):
self.updatepose_trigger(True)
if msg.data == True:
self.algorithm_is_on = True
last_time = rospy.Time.now().to_sec()
while self.algorithm_is_on:
if rospy.Time.now().to_sec()-last_time > 0.1:
self.updatepose_trigger(True)
last_time = rospy.Time.now().to_sec()
def stopalgorithm_callback(self, msg):
self.algorithm_is_on = False
def browser_callback(self, msg):
self._widget.browser.append(msg.data)
def send_message(self, msg):
message = String()
message.data = msg
self.message_pub.publish(message)
def updatepose_trigger(self, value):
msg = Bool()
msg.data = value
self.updatepose_pub.publish(msg)
def updatepose(self, msg):
if msg.data == True:
self.updateik()
def jointstate_callback(self, msg):
if len(msg.name)==6:
msg.name.extend(["gripper_joint", "gripper_joint1", "gripper_joint2", "gripper_joint3", "gripper_joint4",
"gripper_joint5", "gripper_joint6", "gripper_joint7", "gripper_joint8"])
position = list(msg.position)
position.extend([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
msg.position = position
self.jointstate_pub.publish(msg)
def respawn_all_objects(self):
self.robot.modelmanager.respawn_all_objects()
def updatefk(self):
self._widget.joint1Browser.clear()
self._widget.joint1Browser.append(str(round(numpy.rad2deg(self.robot.get_joint_value(1)),2))+DEG)
self._widget.jointSlider_1.setValue(numpy.rad2deg(self.robot.get_joint_value(1)))
self._widget.joint2Browser.clear()
self._widget.joint2Browser.append(str(round(numpy.rad2deg(self.robot.get_joint_value(2)),2))+DEG)
self._widget.jointSlider_2.setValue(numpy.rad2deg(self.robot.get_joint_value(2)))
self._widget.joint3Browser.clear()
self._widget.joint3Browser.append(str(round(numpy.rad2deg(self.robot.get_joint_value(3)),2))+DEG)
self._widget.jointSlider_3.setValue(numpy.rad2deg(self.robot.get_joint_value(3)))
self._widget.joint4Browser.clear()
self._widget.joint4Browser.append(str(round(numpy.rad2deg(self.robot.get_joint_value(4)),2))+DEG)
self._widget.jointSlider_4.setValue(numpy.rad2deg(self.robot.get_joint_value(4)))
self._widget.joint5Browser.clear()
self._widget.joint5Browser.append(str(round(numpy.rad2deg(self.robot.get_joint_value(5)),2))+DEG)
self._widget.jointSlider_5.setValue(numpy.rad2deg(self.robot.get_joint_value(5)))
self._widget.joint6Browser.clear()
self._widget.joint6Browser.append(str(round(numpy.rad2deg(self.robot.get_joint_value(6)),2))+DEG)
self._widget.jointSlider_6.setValue(numpy.rad2deg(self.robot.get_joint_value(6)))
def updateik(self):
roll, pitch, yaw, x, y, z = self.robot.get_arm_pose()
self._widget.xEdit.setText(str(round(x,4)))
self._widget.yEdit.setText(str(round(y,4)))
self._widget.zEdit.setText(str(round(z,4)))
self._widget.rollEdit.setText(str(round(numpy.rad2deg(roll),2)))
self._widget.pitchEdit.setText(str(round(numpy.rad2deg(pitch),2)))
self._widget.yawEdit.setText(str(round(numpy.rad2deg(yaw),2)))
def launchrviz(self):
os.system("gnome-terminal -x sh -c \"roslaunch rqt_industrial_robot rviz.launch\"")
def algorithm_trigger(self, pub, value):
msg = Bool()
msg.data = value
pub.publish(msg)
def playClicked(self):
self.send_message("Start Algorithm")
self.algorithm_trigger(self.startalgorithm_pub, True)
def stopClicked(self):
self.send_message("Stopping Algorithm")
self.algorithm_trigger(self.stopalgorithm_pub, True)
self.algorithm_trigger(self.startalgorithm_pub, False)
def pauseClicked(self):
self.send_message("Pausing Algorithm")
self.algorithm_trigger(self.pausealgorithm_pub, True)
def restartClicked(self):
self.send_message("Retart Algorithm")
self.algorithm_trigger(self.pausealgorithm_pub, False)
def update(self):
while True:
self.updatefk()
self.updateik()
print("updating")
time.sleep(1)
def plan(self):
self.set_x()
self.set_y()
self.set_z()
self.set_roll()
self.set_pitch()
self.set_yaw()
self.robot.plan()
def execute(self):
last_joints = self.robot.get_joints_value()
self.robot.execute()
# print("start moving")
# update after robot stops moving
while abs(sum(last_joints)-sum(self.robot.get_joints_value()) > 1e-10):
last_joints = self.robot.get_joints_value()
#print(last_joints)
time.sleep(0.5)
self.updatefk()
self.updateik()
time.sleep(1.5)
# print("double check movement")
# update after robot stops moving
while abs(sum(last_joints)-sum(self.robot.get_joints_value()) > 1e-10):
last_joints = self.robot.get_joints_value()
#print(last_joints)
time.sleep(0.5)
self.updatefk()
self.updateik()
def planexe(self):
last_joints = self.robot.get_joints_value()
self.set_x()
self.set_y()
self.set_z()
self.set_roll()
self.set_pitch()
self.set_yaw()
self.robot.plan()
self.robot.execute()
# print("start moving")
# update after robot stops moving
while abs(sum(last_joints)-sum(self.robot.get_joints_value()) > 1e-10):
last_joints = self.robot.get_joints_value()
#print(last_joints)
time.sleep(0.5)
self.updatefk()
self.updateik()
time.sleep(1.5)
# print("double check movement")
# update after robot stops moving
while abs(sum(last_joints)-sum(self.robot.get_joints_value()) > 1e-10):
last_joints = self.robot.get_joints_value()
#print(last_joints)
time.sleep(0.5)
self.updatefk()
self.updateik()
def stopexe(self):
self.robot.stop_execution()
self.updatefk()
self.updateik()
def backtohome(self):
self.robot.back_to_home()
self.updatefk()
self.updateik()
def set_x(self):
self.robot.set_x(float(self._widget.xEdit.text()))
def set_y(self):
self.robot.set_y(float(self._widget.yEdit.text()))
def set_z(self):
self.robot.set_z(float(self._widget.zEdit.text()))
def set_roll(self):
self.robot.set_roll(numpy.deg2rad(float(self._widget.rollEdit.text())))
def set_pitch(self):
self.robot.set_pitch(numpy.deg2rad(float(self._widget.pitchEdit.text())))
def set_yaw(self):
self.robot.set_yaw(numpy.deg2rad(float(self._widget.yawEdit.text())))
def viewjoint1(self):
self._widget.joint1Browser.clear()
self._widget.joint1Browser.append(str(self._widget.jointSlider_1.value())+DEG)
def viewjoint2(self):
self._widget.joint2Browser.clear()
self._widget.joint2Browser.append(str(self._widget.jointSlider_2.value())+DEG)
def viewjoint3(self):
self._widget.joint3Browser.clear()
self._widget.joint3Browser.append(str(self._widget.jointSlider_3.value())+DEG)
def viewjoint4(self):
self._widget.joint4Browser.clear()
self._widget.joint4Browser.append(str(self._widget.jointSlider_4.value())+DEG)
def viewjoint5(self):
self._widget.joint5Browser.clear()
self._widget.joint5Browser.append(str(self._widget.jointSlider_5.value())+DEG)
def viewjoint6(self):
self._widget.joint6Browser.clear()
self._widget.joint6Browser.append(str(self._widget.jointSlider_6.value())+DEG)
def setjoint1(self):
angle = numpy.deg2rad(self._widget.jointSlider_1.value())
self.robot.set_arm_joint(1, angle)
self.updateik()
def setjoint2(self):
angle = numpy.deg2rad(self._widget.jointSlider_2.value())
self.robot.set_arm_joint(2, angle)
self.updateik()
def setjoint3(self):
angle = numpy.deg2rad(self._widget.jointSlider_3.value())
self.robot.set_arm_joint(3, angle)
self.updateik()
def setjoint4(self):
angle = numpy.deg2rad(self._widget.jointSlider_4.value())
self.robot.set_arm_joint(4, angle)
self.updateik()
def setjoint5(self):
angle = numpy.deg2rad(self._widget.jointSlider_5.value())
self.robot.set_arm_joint(5, angle)
self.updateik()
def setjoint6(self):
angle = numpy.deg2rad(self._widget.jointSlider_6.value())
self.robot.set_arm_joint(6, angle)
self.updateik()
def setRobotWrapper(self, robot):
self.robot = robot
def gripper_grasp(self):
self.robot.gripper_grasp()
def gripper_release(self):
self.robot.gripper_release()
class MyPlugin(Plugin):
def __init__(self, context):
super(MyPlugin, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('MyPlugin')
# 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._widget = QWidget()
# Get path to UI file which should be in the "resource" folder of this package
ui_file = os.path.join(rospkg.RosPack().get_path('rqt_mypkg'), 'resource', 'MyPlugin.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('MyPluginUi')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). 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._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
#MOVE ROCAM
self._widget.Up.clicked[bool].connect(self.move_up)
self._widget.Down.clicked[bool].connect(self.move_down)
self._widget.Right.clicked[bool].connect(self.move_right)
self._widget.Left.clicked[bool].connect(self.move_left)
#MOVE MOTOR SHIELD
self._widget.MUp.clicked[bool].connect(self.move_mup)
self._widget.MDown.clicked[bool].connect(self.move_mdown)
self._widget.MRight.clicked[bool].connect(self.move_mright)
self._widget.MLeft.clicked[bool].connect(self.move_mleft)
self.pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1) #no need to instantiate another rosnode
self.mpub = rospy.Publisher('/motorshield',Vector3, queue_size=1) #no need to instantiate another rosnode
self.forward_delta = 1
self.left_delta = 1
self.cmd_vel = Twist() #instantiating the twist object to be modified for sending
self.motor_vel = Vector3()
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 move_up(self,checked):
print("up")
self.cmd_vel.angular.z = self.forward_delta
self.publish()
def move_down(self,checked):
print("down")
self.cmd_vel.angular.z = -1*self.forward_delta
self.publish()
def move_right(self,checked):
print("right")
self.cmd_vel.linear.x = self.left_delta
self.publish()
def move_left(self,checked):
print("left")
self.cmd_vel.linear.x = -1*self.left_delta
self.publish()
def publish(self):
self.pub.publish(self.cmd_vel)
def move_mup(self,checked):
print("mup")
num12 = 400.0
self.motor_vel.x = num12
self.motor_vel.y = num12
self.publishmotor()
def move_mdown(self,checked):
print("mdown")
num12 = 400.0
self.motor_vel.x = -1*num12
self.motor_vel.y = -1*num12
self.publishmotor()
def move_mright(self,checked):
print("mright")
num12 = 400.0
self.motor_vel.x = num12
self.motor_vel.y = 0
self.publishmotor()
def move_mleft(self,checked):
print("mleft")
num12 = 400.0
self.motor_vel.x = 0
self.motor_vel.y = num12
self.publishmotor()
def publishmotor(self):
self.mpub.publish(self.motor_vel)
class MyPlugin(Plugin):
target_pos = []
target_force = []
s = []
s_cmd = []
port = 8002 # where do you expect to get a msg?
bufferSize = 12 # whatever you need
server_thread = []
client_address = ('192.168.0.102', 8000)
def __init__(self, context):
super(MyPlugin, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('MyPlugin')
# 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._widget = QWidget()
# Get path to UI file which should be in the "resource" folder of this package
ui_file = os.path.join(rospkg.RosPack().get_path('m3_rqt'), 'resource',
'MyPlugin.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('M3')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). 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._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
self.target_pos = self._widget.findChild(QSlider, "target_pos")
self.target_force = self._widget.findChild(QSlider, "target_force")
self.target_pos.valueChanged.connect(self.pos_target_change)
self.target_force.valueChanged.connect(self.force_target_change)
import select, socket
self.s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
self.s.bind(('255.255.255.255', self.port))
self.s.setblocking(0)
self.server_thread = threading.Thread(target=self.receiveStatus)
self.server_thread.daemon = True
self.server_thread.start()
self.s_cmd = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
# thread fuction
def receiveStatus(self):
while not rospy.is_shutdown():
result = select.select([self.s], [], [])
msg = result[0][0].recv(self.bufferSize)
print(unpack('fff', 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
# This will enable a setting button (gear icon) in each dock widget title bar
# Usually used to open a modal configuration dialog
def pos_target_change(self):
setpoint = self.target_pos.value()
print(setpoint)
self.s_cmd.sendto(pack('fB', setpoint, 0), self.client_address)
def force_target_change(self):
setpoint = self.target_force.value()
print(setpoint)
self.s_cmd.sendto(pack('fB', setpoint, 1), self.client_address)
class RosTfTree(QObject):
_deferred_fit_in_view = Signal()
def __init__(self, context):
super(RosTfTree, self).__init__(context)
self.initialized = False
self.setObjectName('RosTfTree')
self._current_dotcode = None
self._widget = QWidget()
# factory builds generic dotcode items
self.dotcode_factory = PydotFactory()
# self.dotcode_factory = PygraphvizFactory()
# generator builds rosgraph
self.dotcode_generator = RosTfTreeDotcodeGenerator()
self.tf2_buffer_ = tf2_ros.Buffer()
self.tf2_listener_ = tf2_ros.TransformListener(self.tf2_buffer_)
# dot_to_qt transforms into Qt elements using dot layout
self.dot_to_qt = DotToQtGenerator()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_tf_tree'), 'resource', 'RosTfTree.ui')
loadUi(ui_file, self._widget, {'InteractiveGraphicsView': InteractiveGraphicsView})
self._widget.setObjectName('RosTfTreeUi')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
self._scene = QGraphicsScene()
self._scene.setBackgroundBrush(Qt.white)
self._widget.graphics_view.setScene(self._scene)
self._widget.refresh_graph_push_button.setIcon(QIcon.fromTheme('view-refresh'))
self._widget.refresh_graph_push_button.pressed.connect(self._update_tf_graph)
self._widget.highlight_connections_check_box.toggled.connect(self._redraw_graph_view)
self._widget.auto_fit_graph_check_box.toggled.connect(self._redraw_graph_view)
self._widget.fit_in_view_push_button.setIcon(QIcon.fromTheme('zoom-original'))
self._widget.fit_in_view_push_button.pressed.connect(self._fit_in_view)
self._widget.load_dot_push_button.setIcon(QIcon.fromTheme('document-open'))
self._widget.load_dot_push_button.pressed.connect(self._load_dot)
self._widget.save_dot_push_button.setIcon(QIcon.fromTheme('document-save-as'))
self._widget.save_dot_push_button.pressed.connect(self._save_dot)
self._widget.save_as_svg_push_button.setIcon(QIcon.fromTheme('document-save-as'))
self._widget.save_as_svg_push_button.pressed.connect(self._save_svg)
self._widget.save_as_image_push_button.setIcon(QIcon.fromTheme('image-x-generic'))
self._widget.save_as_image_push_button.pressed.connect(self._save_image)
self._deferred_fit_in_view.connect(self._fit_in_view,
Qt.QueuedConnection)
self._deferred_fit_in_view.emit()
context.add_widget(self._widget)
self._force_refresh = False
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('auto_fit_graph_check_box_state',
self._widget.auto_fit_graph_check_box.isChecked())
instance_settings.set_value('highlight_connections_check_box_state',
self._widget.highlight_connections_check_box.isChecked())
def restore_settings(self, plugin_settings, instance_settings):
self._widget.auto_fit_graph_check_box.setChecked(
instance_settings.value('auto_fit_graph_check_box_state', True) in [True, 'true'])
self._widget.highlight_connections_check_box.setChecked(
instance_settings.value('highlight_connections_check_box_state', True) in [True, 'true'])
self.initialized = True
self._refresh_tf_graph()
def _update_tf_graph(self):
self._force_refresh = True
self._refresh_tf_graph()
def _refresh_tf_graph(self):
if not self.initialized:
return
self._update_graph_view(self._generate_dotcode())
def _generate_dotcode(self):
force_refresh = self._force_refresh
self._force_refresh = False
rospy.wait_for_service('~tf2_frames')
tf2_frame_srv = rospy.ServiceProxy('~tf2_frames', FrameGraph)
return self.dotcode_generator.generate_dotcode(dotcode_factory=self.dotcode_factory,
tf2_frame_srv=tf2_frame_srv,
force_refresh=force_refresh)
def _update_graph_view(self, dotcode):
if dotcode == self._current_dotcode:
return
self._current_dotcode = dotcode
self._redraw_graph_view()
def _generate_tool_tip(self, url):
return url
def _redraw_graph_view(self):
self._scene.clear()
if self._widget.highlight_connections_check_box.isChecked():
highlight_level = 3
else:
highlight_level = 1
(nodes, edges) = self.dot_to_qt.dotcode_to_qt_items(self._current_dotcode,
highlight_level)
for node_item in nodes.itervalues():
self._scene.addItem(node_item)
for edge_items in edges.itervalues():
for edge_item in edge_items:
edge_item.add_to_scene(self._scene)
self._scene.setSceneRect(self._scene.itemsBoundingRect())
if self._widget.auto_fit_graph_check_box.isChecked():
self._fit_in_view()
def _load_dot(self, file_name=None):
if file_name is None:
file_name, _ = QFileDialog.getOpenFileName(
self._widget,
self.tr('Open graph from file'),
None,
self.tr('DOT graph (*.dot)'))
if file_name is None or file_name == '':
return
try:
fhandle = open(file_name, 'rb')
dotcode = fhandle.read()
fhandle.close()
except IOError:
return
self._update_graph_view(dotcode)
def _fit_in_view(self):
self._widget.graphics_view.fitInView(self._scene.itemsBoundingRect(),
Qt.KeepAspectRatio)
def _save_dot(self):
file_name, _ = QFileDialog.getSaveFileName(self._widget,
self.tr('Save as DOT'),
'frames.dot',
self.tr('DOT graph (*.dot)'))
if file_name is None or file_name == '':
return
file = QFile(file_name)
if not file.open(QIODevice.WriteOnly | QIODevice.Text):
return
file.write(self._current_dotcode)
file.close()
def _save_svg(self):
file_name, _ = QFileDialog.getSaveFileName(
self._widget,
self.tr('Save as SVG'),
'frames.svg',
self.tr('Scalable Vector Graphic (*.svg)'))
if file_name is None or file_name == '':
return
generator = QSvgGenerator()
generator.setFileName(file_name)
generator.setSize((self._scene.sceneRect().size() * 2.0).toSize())
painter = QPainter(generator)
painter.setRenderHint(QPainter.Antialiasing)
self._scene.render(painter)
painter.end()
def _save_image(self):
file_name, _ = QFileDialog.getSaveFileName(
self._widget,
self.tr('Save as image'),
'frames.png',
self.tr('Image (*.bmp *.jpg *.png *.tiff)'))
if file_name is None or file_name == '':
return
img = QImage((self._scene.sceneRect().size() * 2.0).toSize(),
QImage.Format_ARGB32_Premultiplied)
painter = QPainter(img)
painter.setRenderHint(QPainter.Antialiasing)
self._scene.render(painter)
painter.end()
img.save(file_name)
