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 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):
#Creating a QObject
super(MyPlugin, self).__init__(context)
self.setObjectName('MyPlugin')
#Process standalone plugin command-line arguments
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())
if not args.quiet:
print 'arguments: ', args
print 'unknowns: ', unknowns
#Creating QWidget
self._widget = QWidget()
ui_file = os.path.join(rospkg.RosPack().get_path('user_interface'),
'resource', 'user_interface_gui.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('MyPluginUi')
#ROS publishers and subscribers--------------->
self.coord_to_gui_sub = rospy.Subscriber('/coord_to_gui', String,
self.comm_to_gui_callback)
self.gui_to_coord_pub = rospy.Publisher('/gui_to_coord',
String,
queue_size=1000)
self.send_num_wp_pub = rospy.Publisher('/num_wp',
Int16,
queue_size=1000)
#---------------------------------------------
#GUI Button Connections---------------------------->
self._widget.initialize_robot_btn.clicked[bool].connect(
self.intialize_robots)
self._widget.load_wp_btn.clicked[bool].connect(self.load_wp)
## Robot1--------------->
self._widget.approach_pick_wrkpiece_rob1_btn.clicked[bool].connect(
self.approach_pick_wrkpiece)
self._widget.place_workpiece_btn.clicked[bool].connect(
self.place_workpiece)
##Robot2---------------->
self._widget.pick_workpiece_rob2_btn.clicked[bool].connect(
self.pick_workpiece_rob2)
self._widget.segregate_workpiece_btn.clicked[bool].connect(
self.segregate_workpiece)
##CNN------------------->
self._widget.train_model_btn.clicked[bool].connect(self.train_model)
self._widget.classify_btn.clicked[bool].connect(self.classify_image)
##Robot3---------------->
self._widget.load_milling_btn.clicked[bool].connect(
self.load_milling_plan)
self._widget.execute_miling_btn.clicked[bool].connect(
self.execute_milling)
#------------------------------------------------
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
context.add_widget(self._widget)
##Utility funcitons for User Interface
def publish_msg_to_coord(self, msg):
msg_to_publish = msg
self.gui_to_coord_pub.publish(msg_to_publish)
def intialize_robots(self):
self.publish_msg_to_coord('initialize_robots')
pass
def load_wp(self):
num_wp = self._widget.num_wp_txtbx.text()
msg = int(num_wp)
self.send_num_wp_pub.publish(msg)
###Robot 1----->
def approach_pick_wrkpiece(self):
self.publish_msg_to_coord('approach_and_pick_workpiece_rob1')
pass
# def pick_workpiece_rob1(self):
# self.publish_msg_to_coord('pick_workpiece_rob1')
# pass
def place_workpiece(self):
self.publish_msg_to_coord('place_workpiece_rob1')
pass
###Robot 2------>
def pick_workpiece_rob2(self):
self.publish_msg_to_coord('pick_workpiece_rob2')
pass
def segregate_workpiece(self):
self.publish_msg_to_coord('segregate_workpiece_rob2')
pass
###Robot 3------>
def load_milling_plan(self):
self.publish_msg_to_coord('load_milling_plan')
pass
def execute_milling(self):
self.publish_msg_to_coord('execute_milling')
pass
###Update gui status------->
def comm_to_gui_callback(self, msg):
status = msg.data
self._widget.gui_status_txtbx.setText(status)
pass
def train_model(self):
self.publish_msg_to_coord('train_model')
pass
def classify_image(self):
self.publish_msg_to_coord('classify_image')
pass
###Rqt functions
def shutdown_plugin(self):
#Unregister all publishers and subscribers
self.gui_to_coord_pub.unregister()
self.coord_to_gui_sub.unregister()
self.send_num_wp_pub.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
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.values():
self._scene.addItem(node_item)
for edge_items in edges.values():
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 GaussPlugin(Plugin):
pop_up = Signal()
def __init__(self, context):
super(GaussPlugin, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('GaussPlugin')
rp = rospkg.RosPack()
rospy.loginfo('[RQt] Started Gauss RQt node!')
# 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 '[RQt] arguments: ', args
print '[RQt] 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_gauss'), 'resource', 'GaussPlugin.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('GaussPluginUi')
# Do connections and stuff here. For complex plugins, consider
# creating custom helper classes instead of QWidget
self.status_pub = rospy.Publisher('/gauss/flight', RPSChangeFlightStatus, queue_size=1)
self.acceptance_pub = rospy.Publisher('/gauss/flightacceptance', RPSFlightPlanAccept, queue_size=1)
rospy.Subscriber('/gauss/alternative_flight_plan', UTMAlternativeFlightPlan, self.alternative_flight_plan_callback)
self.read_icao_service = rospy.ServiceProxy('/gauss/read_icao', ReadIcao)
self._widget.refresh_button.clicked.connect(self.handle_refresh_button_clicked)
self._widget.start_button.clicked.connect(self.handle_start_button_clicked)
self._widget.stop_button.clicked.connect(self.handle_stop_button_clicked)
self.pop_up.connect(self.show_pop_up)
self.alternative_flight_plan = None
self.handle_refresh_button_clicked() # Try to refresh here
# 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 handle_refresh_button_clicked(self):
try:
read_icao_response = self.read_icao_service.call(ReadIcaoRequest())
self._widget.icao_list.clear()
for icao in read_icao_response.icao_address:
self._widget.icao_list.addItem(icao)
except rospy.ServiceException as e:
print("[RQt] Service call failed: %s"%e)
def change_flight_status(self, status):
selected_icao = self._widget.icao_list.currentItem()
if selected_icao is None:
rospy.logerr('[RQt] Select an icao address')
return
icao = selected_icao.text()
change_flight_msg = RPSChangeFlightStatus()
change_flight_msg.icao = int(icao)
# change_flight_msg.flight_plan_id # TODO
change_flight_msg.status = status
# change_flight_msg.timestamp # TODO
rospy.loginfo('[RQt] RPA[{}]: mission {}'.format(icao, status))
self.status_pub.publish(change_flight_msg)
def handle_start_button_clicked(self):
self.change_flight_status('start')
def handle_stop_button_clicked(self):
self.change_flight_status('stop')
def alternative_flight_plan_callback(self, data):
if self.alternative_flight_plan is None:
self.alternative_flight_plan = data
rospy.loginfo('[RQt] Received: [{}]'.format(data))
self.pop_up.emit()
else:
rospy.loginfo('[RQt] Waiting for pilot response...')
def show_pop_up(self):
msg = QMessageBox()
msg.setWindowTitle('Alternative flight plan received')
msg.setText('Accept alternative flight plan?')
msg.setIcon(QMessageBox.Question)
msg.setStandardButtons(QMessageBox.Yes|QMessageBox.No)
msg.setDefaultButton(QMessageBox.No)
# msg.setInformativeText('Accept new plan?')
msg.setDetailedText('{}'.format(self.alternative_flight_plan))
# msg.buttonClicked.connect(self.handle_message_box) # TODO: do handling here?
pop_up_response = msg.exec_()
ros_response = RPSFlightPlanAccept()
ros_response.icao = self.alternative_flight_plan.icao
ros_response.flight_plan_id = self.alternative_flight_plan.flight_plan_id
if pop_up_response == QMessageBox.Yes:
ros_response.accept = True
# Try to call light_sim service to change flight plan
try:
light_sim_change_flight_plan = rospy.ServiceProxy('/gauss_light_sim/change_flight_plan', ChangeFlightPlan)
light_sim_change_flight_plan(ChangeFlightPlanRequest(alternative=copy.deepcopy(self.alternative_flight_plan)))
except rospy.ServiceException as e:
print("[RQt] Service call failed: %s"%e)
if pop_up_response == QMessageBox.No:
ros_response.accept = False
self.acceptance_pub.publish(ros_response)
self.alternative_flight_plan = None
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 Top(Plugin):
NODE_FIELDS = [ 'pid', 'get_cpu_percent', 'get_memory_percent', 'get_num_threads']
OUT_FIELDS = ['node_name', 'pid', 'cpu_percent', 'memory_percent', 'num_threads' ]
FORMAT_STRS = ['%s', '%s', '%0.2f', '%0.2f', '%s' ]
NODE_LABELS = ['Node', 'PID', 'CPU %', 'Mem %', 'Num Threads' ]
SORT_TYPE = [str, str, float, float, float ]
TOOLTIPS = {
0: ('cmdline', lambda x: '\n'.join(textwrap.wrap(' '.join(x)))),
3: ('memory_info', lambda x: ('Resident: %0.2f MiB, Virtual: %0.2f MiB' % (x[0]/2**20, x[1]/2**20)))
}
_node_info = NodeInfo()
name_filter = re.compile('')
def __init__(self, context):
super(Top, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('Top')
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
parser.add_argument("-q", "--quiet", action="store_true",
dest="quiet",
help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
# if not args.quiet:
# print 'arguments: ', args
# print 'unknowns: ', unknowns
self._selected_node = ''
self._selected_node_lock = RLock()
# Setup the toolbar
self._toolbar = QToolBar()
self._filter_box = QLineEdit()
self._regex_box = QCheckBox()
self._regex_box.setText('regex')
self._toolbar.addWidget(QLabel('Filter'))
self._toolbar.addWidget(self._filter_box)
self._toolbar.addWidget(self._regex_box)
self._filter_box.returnPressed.connect(self.update_filter)
self._regex_box.stateChanged.connect(self.update_filter)
# Create a container widget and give it a layout
self._container = QWidget()
self._container.setWindowTitle('Process Monitor')
self._layout = QVBoxLayout()
self._container.setLayout(self._layout)
self._layout.addWidget(self._toolbar)
# Create the table widget
self._table_widget = QTreeWidget()
self._table_widget.setObjectName('TopTable')
self._table_widget.setColumnCount(len(self.NODE_LABELS))
self._table_widget.setHeaderLabels(self.NODE_LABELS)
self._table_widget.itemClicked.connect(self._tableItemClicked)
self._table_widget.setSortingEnabled(True)
self._table_widget.setAlternatingRowColors(True)
self._layout.addWidget(self._table_widget)
context.add_widget(self._container)
# Add a button for killing nodes
self._kill_button = QPushButton('Kill Node')
self._layout.addWidget(self._kill_button)
self._kill_button.clicked.connect(self._kill_node)
# Update twice since the first cpu% lookup will always return 0
self.update_table()
self.update_table()
self._table_widget.resizeColumnToContents(0)
# Start a timer to trigger updates
self._update_timer = QTimer()
self._update_timer.setInterval(1000)
self._update_timer.timeout.connect(self.update_table)
self._update_timer.start()
def _tableItemClicked(self, item, column):
with self._selected_node_lock:
self._selected_node = item.text(0)
def update_filter(self, *args):
if self._regex_box.isChecked():
expr = self._filter_box.text()
else:
expr = re.escape(self._filter_box.text())
self.name_filter = re.compile(expr)
self.update_table()
def _kill_node(self):
self._node_info.kill_node(self._selected_node)
def update_one_item(self, row, info):
twi = TopWidgetItem()
for col, field in enumerate(self.OUT_FIELDS):
val = info[field]
twi.setText(col, self.FORMAT_STRS[col] % val)
self._table_widget.insertTopLevelItem(row, twi)
for col, (key, func) in self.TOOLTIPS.items():
twi.setToolTip(col, func(info[key]))
with self._selected_node_lock:
if twi.text(0) == self._selected_node:
twi.setSelected(True)
twi.setHidden(len(self.name_filter.findall(info['node_name'])) == 0)
def update_table(self):
self._table_widget.clear()
infos = self._node_info.get_all_node_fields(self.NODE_FIELDS)
for nx, info in enumerate(infos):
self.update_one_item(nx, info)
def shutdown_plugin(self):
self._update_timer.stop()
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('filter_text', self._filter_box.text())
instance_settings.set_value('is_regex', int(self._regex_box.checkState()))
def restore_settings(self, plugin_settings, instance_settings):
self._filter_box.setText(instance_settings.value('filter_text'))
is_regex_int = instance_settings.value('is_regex')
if is_regex_int:
self._regex_box.setCheckState(Qt.CheckState(is_regex_int))
else:
self._regex_box.setCheckState(Qt.CheckState(0))
self.update_filter()
class 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 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 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('pressure_controller_setup'), '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')
self.settings = self.get_settings()
self.init_sliders()
#self.init_config()
self._client = actionlib.SimpleActionClient(
'pressure_control', pressure_controller_ros.msg.CommandAction)
self._client_connected = self._client.wait_for_server(
timeout=rospy.rostime.Duration(1))
if not self._client_connected:
print("No command server avaiable... changes will not be sent")
self.set_graph_state(True)
self.send_channel_state(0, self.settings['channel_states'][0])
# 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 init_config(self):
# self._plot_type_index = 0
# self.plot_types = [
# {
# 'title': 'PyQtGraph',
# 'widget_class': PyQtGraphDataPlot,
# 'description': 'Based on PyQtGraph\n- installer: http://luke.campagnola.me/code/pyqtgraph',
# 'enabled': True,#PyQtGraphDataPlot is not None,
# },
# {
# 'title': 'MatPlot',
# 'widget_class': MatDataPlot,
# 'description': 'Based on MatPlotLib\n- needs most CPU\n- needs matplotlib >= 1.1.0\n- if using PySide: PySide > 1.1.0',
# 'enabled': True,#MatDataPlot is not None,
# },
# {
# 'title': 'QwtPlot',
# 'widget_class': QwtDataPlot,
# 'description': 'Based on QwtPlot\n- does not use timestamps\n- uses least CPU\n- needs Python Qwt bindings',
# 'enabled': True,#QwtDataPlot is not None,
# },
# ]
def init_sliders(self):
sliderbox = self._widget.findChild(QLayout, 'Sliders')
graph_button = QPushButton()
graph_button.setCheckable(True)
graph_button.setText("Graph Off")
graph_button.toggle()
graph_button.clicked.connect(self.set_graph_state)
self.graph_button = graph_button
firstCol = QVBoxLayout()
firstCol.addWidget(graph_button)
sliderbox.addLayout(firstCol)
self.sliders = []
all_rows_layout = QVBoxLayout()
chan_idx = 0
for num_channels_row in self.settings['num_channels']:
row_layout = QHBoxLayout()
for i in range(num_channels_row):
idx = chan_idx * 1
slider_group = {
'slider_p': None,
'number_p': None,
'slider_i': None,
'number_i': None,
'slider_d': None,
'number_d': None,
'on_off': None
}
layout_cluster = QVBoxLayout()
slider_cluster = QHBoxLayout()
label = QLabel()
label.setText("Chan. %d" % (idx + 1))
label.setAlignment(Qt.AlignCenter)
layout_cluster.addWidget(label)
for j in range(3):
layout = QVBoxLayout()
layout.setAlignment(Qt.AlignHCenter)
if j == 0:
maxrange = 1.0
elif j == 1:
maxrange = 10
elif j == 2:
maxrange = 10
slider = QSlider(Qt.Vertical)
slider.setMinimum(0)
slider.setMaximum(maxrange * 100)
slider.setValue(self.settings['pid_gains'][chan_idx][j] *
100)
slider.setTickPosition(QSlider.TicksRight)
slider.setTickInterval(maxrange / 100.0)
spinbox = QDoubleSpinBox()
spinbox.setMinimum(0)
spinbox.setMaximum(maxrange)
spinbox.setValue(self.settings['pid_gains'][chan_idx][j])
spinbox.setDecimals(2)
spinbox.setSingleStep(maxrange / 100.0)
cb_function_number = lambda value, idx=idx, gain_idx=j, slider=False: self.send_slider_value(
idx, gain_idx, value, slider)
cb_function_slider = lambda value, idx=idx, gain_idx=j, slider=True: self.send_slider_value(
idx, gain_idx, value, slider)
slider.valueChanged.connect(cb_function_slider)
spinbox.valueChanged.connect(cb_function_number)
label = QLabel()
label.setAlignment(Qt.AlignCenter)
if j == 0:
slider_group['slider_p'] = slider
slider_group['number_p'] = spinbox
label.setText("P")
elif j == 1:
slider_group['slider_i'] = slider
slider_group['number_i'] = spinbox
label.setText("I")
elif j == 2:
slider_group['slider_d'] = slider
slider_group['number_d'] = spinbox
label.setText("D")
labelmax = QLabel()
labelmax.setAlignment(Qt.AlignCenter)
labelmax.setText("%0.1f" % (maxrange))
labelmin = QLabel()
labelmin.setAlignment(Qt.AlignCenter)
labelmin.setText("0")
layout.addWidget(label)
layout.addWidget(labelmax)
layout.addWidget(slider, Qt.AlignHCenter)
layout.addWidget(labelmin)
layout.addWidget(spinbox, Qt.AlignHCenter)
layout.setAlignment(slider, Qt.AlignHCenter)
layout.setAlignment(spinbox, Qt.AlignHCenter)
slider_cluster.addLayout(layout)
on_button = QPushButton()
on_button.setCheckable(True)
on_button.setText("Off")
if self.settings['channel_states'][chan_idx]:
on_button.toggle()
on_button.setText("On")
on_button.clicked.connect(
lambda state, idx=idx: self.send_channel_state(idx, state))
slider_group['on_off'] = on_button
layout_cluster.addLayout(slider_cluster)
layout_cluster.addWidget(on_button)
row_layout.addLayout(layout_cluster)
row_layout.addSpacing(20)
self.sliders.append(slider_group)
chan_idx += 1
all_rows_layout.addLayout(row_layout)
sliderbox.addLayout(all_rows_layout)
#self._widget.setLayout(sliderbox)
def get_settings(self):
settings = {}
settings['channel_states'] = rospy.get_param(
'/config_node/channels/states', [1, 1, 1, 1, 1, 1, 1, 1, 1, 1])
settings['num_channels'] = rospy.get_param(
'/pressure_control/num_channels', [])
settings['num_channels_tot'] = sum(settings['num_channels'])
settings['pid_gains'] = rospy.get_param('/config_node/PID/values',
[[0, 0, 0]] *
settings['num_channels_tot'])
if isinstance(settings['pid_gains'][0], list):
settings['pid_gains'] = settings['pid_gains']
else:
settings['pid_gains'] = [settings['pid_gains']
] * settings['num_channels_tot']
print(settings['pid_gains'])
return settings
def send_slider_value(self, idx, gain_idx, value, slider):
if gain_idx == 0:
slider_str = 'slider_p'
number_str = 'number_p'
elif gain_idx == 1:
slider_str = 'slider_i'
number_str = 'number_i'
elif gain_idx == 2:
slider_str = 'slider_d'
number_str = 'number_d'
if slider:
cur_val = self.sliders[idx][slider_str].value() / 100.0
self.sliders[idx][number_str].setValue(cur_val)
else:
cur_val = self.sliders[idx][number_str].value()
self.sliders[idx][slider_str].setValue(cur_val * 100.0)
self.settings['pid_gains'][idx][gain_idx] = cur_val
self.send_command(command='pid',
args=[idx] + self.settings['pid_gains'][idx])
def send_channel_state(self, idx, state):
if state:
self.settings['channel_states'][idx] = 1
self.sliders[idx]['on_off'].setText("On")
else:
self.settings['channel_states'][idx] = 0
self.sliders[idx]['on_off'].setText("Off")
self.send_command(command='chan', args=self.settings['channel_states'])
rospy.set_param('/config_node/channels/states',
self.settings['channel_states'])
def set_graph_state(self, value):
if value:
on_off_str = 'on'
self.graph_button.setText("Graph ON")
else:
on_off_str = 'off'
self.graph_button.setText("Graph OFF")
self.send_command(command=on_off_str, args=[])
def send_command(self, command, args, wait_for_ack=False):
if self._client_connected:
# Send commands to the command server and wait for things to be taken care of
goal = pressure_controller_ros.msg.CommandGoal(command=command,
args=args,
wait_for_ack=False)
self._client.send_goal(goal)
self._client.wait_for_result()
if not self._client.get_result():
raise ('Something went wrong and a setting was not validated')
pass
else:
pass
else:
print(command, args)
def shutdown_sliders(self):
rospy.set_param('/config_node/PID/values', self.settings['pid_gains'])
print("")
print("Final PID Gains:")
print(self.settings['pid_gains'])
print("")
print("These settings were saved on the pressure controller")
print("")
print(
"To ensure these parameters are always recalled, for now you need to:"
)
print(
" - Copy these settings into 'pid/values' in all your control config files"
)
self.set_graph_state(False)
for slider_group in self.sliders:
for widget in slider_group:
if 'on_off' in widget:
slider_group[widget].clicked.disconnect()
else:
slider_group[widget].valueChanged.disconnect()
def save_mcu_settings(self):
self.send_command(command='save', args=[])
def shutdown_plugin(self):
self._client.cancel_all_goals()
self.shutdown_sliders()
self.save_mcu_settings()
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 MyPlugin(Plugin):
sig_sysmsg = Signal(str)
l_location=pyqtSignal(float,float)
r_location=pyqtSignal(float,float)
l_rssi = pyqtSignal(int)
r_rssi = pyqtSignal(int)
l_com = pyqtSignal(float)
r_com = pyqtSignal(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)
def change_l_rssi(rssi):
l_rssi_text.setText(str(rssi))
def change_r_rssi(rssi):
r_rssi_text.setText(str(rssi))
def l_rssi_callback(rssi):
self.l_rssi.emit(rssi.data)
#l_rssi_text.setText(str(rssi.data))
def r_rssi_callback(rssi):
self.r_rssi.emit(rssi.data)
#r_rssi_text.setText(str(rssi.data))
def move_l_compass(headidng):
l_com_text.setText("%.1f" % headidng)
time.sleep(0.01)
l_com_widget.setAngle(headidng)
time.sleep(0.01)
def move_r_compass(headidng):
r_com_text.setText("%.1f" % headidng)
time.sleep(0.01)
r_com_widget.setAngle(headidng)
time.sleep(0.01)
def l_com_callback(heading):
self.l_com.emit(heading.data)
#l_com_widget.setAngle(heading.data)
def r_com_callback(heading):
self.r_com.emit(heading.data)
#r_com_widget.setAngle(heading.data)
# Create QWidget
self._widget = QWidget()
gcv=QVBoxLayout(self._widget)
h = QVBoxLayout()
#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_com_widget=CompassWidget()
r_com_widget = CompassWidget()
l_rssi_lable=QLabel('local_rssi')
r_rssi_lable=QLabel('remote_rssi')
l_rssi_text=QLineEdit()
l_rssi_text.setMaximumWidth(70)
r_rssi_text=QLineEdit()
r_rssi_text.setMaximumWidth(70)
l_com_label=QLabel('LCOM')
r_com_label=QLabel('RCOM')
l_com_text=QLineEdit()
l_com_text.setMaximumWidth(80)
r_com_text=QLineEdit()
r_com_text.setMaximumWidth(80)
l_h_layout=QHBoxLayout()
r_h_layout = QHBoxLayout()
l_h_layout.addStretch(1)
l_h_layout.addWidget(l_rssi_lable)
l_h_layout.addWidget(l_rssi_text)
l_h_layout.addWidget(l_com_label)
l_h_layout.addWidget(l_com_text)
l_h_layout.addStretch(1)
r_h_layout.addStretch(1)
r_h_layout.addWidget(r_rssi_lable)
r_h_layout.addWidget(r_rssi_text)
r_h_layout.addWidget(r_com_label)
r_h_layout.addWidget(r_com_text)
r_h_layout.addStretch(1)
mid_layout=QHBoxLayout()
#mid_layout.setSpacing(20)
mid_layout.addLayout(l_h_layout)
mid_layout.addLayout(r_h_layout)
c_h_l=QHBoxLayout()
c_h_l.addWidget(l_com_widget)
c_h_l.addWidget(r_com_widget)
#c_h_l.SetMinimumSize(240)
gcv.addLayout(c_h_l)
gcv.addLayout(mid_layout)
gcv.addLayout(h)
gcv.setStretchFactor(c_h_l,3)
gcv.setStretchFactor(h,5)
# 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)
self.l_com.connect(move_l_compass)
self.r_com.connect(move_r_compass)
self.l_rssi.connect(change_l_rssi)
self.r_rssi.connect(change_r_rssi)
# 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'), '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)
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)
#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 Top(Plugin):
NODE_FIELDS = [
'pid', 'get_cpu_percent', 'get_memory_percent', 'get_num_threads'
]
OUT_FIELDS = [
'node_name', 'pid', 'cpu_percent', 'memory_percent', 'num_threads'
]
FORMAT_STRS = ['%s', '%s', '%0.2f', '%0.2f', '%s']
NODE_LABELS = ['Node', 'PID', 'CPU %', 'Mem %', 'Num Threads']
SORT_TYPE = [str, str, float, float, float]
TOOLTIPS = {
0: ('cmdline', lambda x: '\n'.join(textwrap.wrap(' '.join(x)))),
3: ('memory_info', lambda x:
('Resident: %0.2f MiB, Virtual: %0.2f MiB' %
(x[0] / 2**20, x[1] / 2**20)))
}
_node_info = NodeInfo()
name_filter = re.compile('')
def __init__(self, context):
super(Top, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('Top')
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
parser.add_argument("-q",
"--quiet",
action="store_true",
dest="quiet",
help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
# if not args.quiet:
# print 'arguments: ', args
# print 'unknowns: ', unknowns
self._selected_node = ''
self._selected_node_lock = RLock()
# Setup the toolbar
self._toolbar = QToolBar()
self._filter_box = QLineEdit()
self._regex_box = QCheckBox()
self._regex_box.setText('regex')
self._toolbar.addWidget(QLabel('Filter'))
self._toolbar.addWidget(self._filter_box)
self._toolbar.addWidget(self._regex_box)
self._filter_box.returnPressed.connect(self.update_filter)
self._regex_box.stateChanged.connect(self.update_filter)
# Create a container widget and give it a layout
self._container = QWidget()
self._container.setWindowTitle('Process Monitor')
self._layout = QVBoxLayout()
self._container.setLayout(self._layout)
self._layout.addWidget(self._toolbar)
# Create the table widget
self._table_widget = QTreeWidget()
self._table_widget.setObjectName('TopTable')
self._table_widget.setColumnCount(len(self.NODE_LABELS))
self._table_widget.setHeaderLabels(self.NODE_LABELS)
self._table_widget.itemClicked.connect(self._tableItemClicked)
self._table_widget.setSortingEnabled(True)
self._table_widget.setAlternatingRowColors(True)
self._layout.addWidget(self._table_widget)
context.add_widget(self._container)
# Add a button for killing nodes
self._kill_button = QPushButton('Kill Node')
self._layout.addWidget(self._kill_button)
self._kill_button.clicked.connect(self._kill_node)
# Update twice since the first cpu% lookup will always return 0
self.update_table()
self.update_table()
self._table_widget.resizeColumnToContents(0)
# Start a timer to trigger updates
self._update_timer = QTimer()
self._update_timer.setInterval(1000)
self._update_timer.timeout.connect(self.update_table)
self._update_timer.start()
def _tableItemClicked(self, item, column):
with self._selected_node_lock:
self._selected_node = item.text(0)
def update_filter(self, *args):
if self._regex_box.isChecked():
expr = self._filter_box.text()
else:
expr = re.escape(self._filter_box.text())
self.name_filter = re.compile(expr)
self.update_table()
def _kill_node(self):
self._node_info.kill_node(self._selected_node)
def update_one_item(self, row, info):
twi = TopWidgetItem()
for col, field in enumerate(self.OUT_FIELDS):
val = info[field]
twi.setText(col, self.FORMAT_STRS[col] % val)
self._table_widget.insertTopLevelItem(row, twi)
for col, (key, func) in self.TOOLTIPS.iteritems():
twi.setToolTip(col, func(info[key]))
with self._selected_node_lock:
if twi.text(0) == self._selected_node:
twi.setSelected(True)
twi.setHidden(len(self.name_filter.findall(info['node_name'])) == 0)
def update_table(self):
self._table_widget.clear()
infos = self._node_info.get_all_node_fields(self.NODE_FIELDS)
for nx, info in enumerate(infos):
self.update_one_item(nx, info)
def shutdown_plugin(self):
self._update_timer.stop()
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('filter_text', self._filter_box.text())
instance_settings.set_value('is_regex',
int(self._regex_box.checkState()))
def restore_settings(self, plugin_settings, instance_settings):
self._filter_box.setText(instance_settings.value('filter_text'))
is_regex_int = instance_settings.value('is_regex')
if is_regex_int:
self._regex_box.setCheckState(Qt.CheckState(is_regex_int))
else:
self._regex_box.setCheckState(Qt.CheckState(0))
self.update_filter()
class SchunkPlugin(Plugin):
def __init__(self, context):
super(SchunkPlugin, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('SchunkPlugin')
# 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('schunk_rqt_gui'),
'resource', 'ui', 'SchunkJoints.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('SchunkPluginUI')
# 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)
# Connect to ROS
# action clients
self.action_client = actionlib.SimpleActionClient(
'/gripper/sdh_controller/follow_joint_trajectory',
FollowJointTrajectoryAction)
# subscribers
self.sub_temp = rospy.Subscriber("/gripper/sdh_controller/temperature",
TemperatureArray, self.on_temp)
# Connect to UI
# service buttons
self._widget.button_init.clicked.connect(
lambda: self.call_service("init"))
self._widget.button_disconnect.clicked.connect(
lambda: self.call_service("shutdown"))
self._widget.button_estop.clicked.connect(
lambda: self.call_service("emergency_stop"))
self._widget.button_motor_on.clicked.connect(
lambda: self.call_service("motor_on"))
self._widget.button_motor_off.clicked.connect(
lambda: self.call_service("motor_off"))
# status text
self.status_message = self._widget.status_message
# joint sliders
self._widget.proximal_slider.valueChanged.connect(
lambda value: self.on_slider_update(self._widget.proximal_spinbox,
value))
self._widget.distal_slider.valueChanged.connect(
lambda value: self.on_slider_update(self._widget.distal_spinbox,
value))
# joint spinners
self._widget.proximal_spinbox.valueChanged.connect(
lambda value: self.on_spinner_update(self._widget.proximal_slider,
value))
self._widget.distal_spinbox.valueChanged.connect(
lambda value: self.on_spinner_update(self._widget.distal_slider,
value))
# set spinner boxes by default sliders values
self._widget.proximal_spinbox.setValue(
self._widget.proximal_slider.value() / 1000.0)
self._widget.distal_spinbox.setValue(
self._widget.distal_slider.value() / 1000.0)
# map temperature names to spinner boxes
self.tempspinners = dict()
self.tempspinners["root"] = self._widget.spin_root
self.tempspinners["controller"] = self._widget.spin_ctrl
self.tempspinners["pcb"] = self._widget.spin_pcb
self.is_initialised = False
self.is_motor_on = False
self.has_new_data = False
# start working thread
self.running = True
self.thread = threading.Thread(target=self.loop, args=())
self.thread.start()
def on_slider_update(self, spinner, value):
# just set spinner value, do not forward signal back to slider
spinner.blockSignals(True)
spinner.setValue(value / 1000.0)
spinner.blockSignals(False)
self.has_new_data = True
def on_spinner_update(self, slider, value):
# just set slider value, do not forward signal back to spinner
slider.blockSignals(True)
slider.setValue(value * 1000)
slider.blockSignals(False)
self.has_new_data = True
def call_service(self, name):
service_name = '/gripper/sdh_controller/' + name
try:
rospy.wait_for_service(service_name, timeout=0.5)
except rospy.exceptions.ROSException:
rospy.logerr("service '" + str(name) + "' is not available")
self.status_message.setText("service '" + str(name) +
"' is not available")
return False
service = rospy.ServiceProxy(service_name, Trigger)
try:
resp = service()
except rospy.service.ServiceException as e:
msg = "service call '" + str(name) + "' failed"
rospy.logerr(msg)
self.status_message.setText(msg)
return False
print("Called service:", name)
print("Response:")
print(resp)
self.status_message.setText(resp.message)
if name == "init":
self.is_initialised = resp.success
self.is_motor_on = resp.success
elif name == "shutdown":
self.is_initialised = not resp.success
self.is_motor_on = not resp.success
if name == "motor_on":
self.is_motor_on = resp.success
elif name == "motor_off":
self.is_motor_on = not resp.success
if resp.success and (name in ["init", "motor_on"]):
self.has_new_data = True
return resp.success
def loop(self):
self.running = True
while self.running:
if self.is_initialised and self.is_motor_on and self.has_new_data:
self.send_grasp_joint_positions()
self.has_new_data = False
time.sleep(0.1)
def send_grasp_joint_positions(self):
# values in range 0 ... 1
proximal_value = self._widget.proximal_spinbox.value()
distal_value = self._widget.distal_spinbox.value()
# define sets of joints
proximal_joints = [
"sdh_thumb_2_joint", "sdh_finger_12_joint", "sdh_finger_22_joint"
]
distal_joints = [
"sdh_thumb_3_joint", "sdh_finger_13_joint", "sdh_finger_23_joint"
]
static_joints = ["sdh_knuckle_joint"]
all_joints = static_joints + proximal_joints + distal_joints
# map joint ranges from [0..1] to individual set ranges
# proximal range: [-pi/2 .. 0]
# distal range: [0 .. pi/2]
proximal_range = [-math.pi / 2.0, 0.0]
distal_range = [0.0, math.pi / 2.0]
proximal_jpos = proximal_range[0] + proximal_value * (
proximal_range[1] - proximal_range[0])
distal_jpos = distal_range[0] + distal_value * (distal_range[1] -
distal_range[0])
trajectory_goal = FollowJointTrajectoryGoal()
# add single single joint point to trajectory
trajectory_goal.trajectory.points.append(JointTrajectoryPoint())
for jname in all_joints:
trajectory_goal.trajectory.joint_names.append(jname)
# select joint position from set
if jname in static_joints:
trajectory_goal.trajectory.points[0].positions.append(0)
elif jname in proximal_joints:
trajectory_goal.trajectory.points[0].positions.append(
proximal_jpos)
elif jname in distal_joints:
trajectory_goal.trajectory.points[0].positions.append(
distal_jpos)
else:
raise Exception("joint not in set")
# send trajectory and wait for response
self.action_client.send_goal(trajectory_goal)
if self.action_client.wait_for_result(timeout=rospy.Duration(5.0)):
trajectory_result = self.action_client.get_result()
print("set joints to " +
('%s' % trajectory_goal.trajectory.points[0].positions))
self.status_message.setText("set joints")
return trajectory_result.error_code == FollowJointTrajectoryResult.SUCCESSFUL
else:
rospy.logerr("timeout while waiting for response from grasp goal")
self.status_message.setText(
"timeout while waiting for response from grasp goal")
return False
def on_temp(self, temp_msg):
if self.is_initialised:
temps = dict(zip(temp_msg.name, temp_msg.temperature))
for name, spinner in self.tempspinners.iteritems():
try:
spinner.setValue(temps[name])
except KeyError:
rospy.logerr("temperature", name,
"is not provided by SDH driver node")
def shutdown_plugin(self):
# TODO unregister all publishers here
self.sub_temp.unregister()
self.action_client.cancel_all_goals()
self.running = False
self.thread.join()
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 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 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 HandEyeCalibration(Plugin):
PLUGIN_TITLE = ' Intel OTC Robotics: Hand-Eye Calibration'
def __init__(self, context):
super(HandEyeCalibration, self).__init__(context)
self.context = context
self.node = context.node
self.widget = QWidget()
self.widget.setObjectName(self.PLUGIN_TITLE)
self.widget.setWindowTitle(self.PLUGIN_TITLE)
# Data
self.Tsamples = []
# Toolbar
_, path_pkg = get_resource('packages', 'handeye_dashboard')
print("{}".format(path_pkg))
self.snapshot_action = QAction(QIcon.fromTheme('camera-photo'),
'Take a snapshot', self.widget)
path = path_pkg + '/share/handeye_dashboard/images/capture.png'
self.calibrate_action = QAction(QIcon(QPixmap.fromImage(QImage(path))),
'Get the camera/robot transform',
self.widget)
self.clear_action = QAction(QIcon.fromTheme('edit-clear'),
'Clear the record data.', self.widget)
path = path_pkg + '/share/handeye_dashboard/images/UR5.png'
self.execut_action = QAction(QIcon(QPixmap.fromImage(QImage(path))),
'EStart the publishing the TF.',
self.widget)
self.toolbar = QToolBar()
self.toolbar.addAction(self.snapshot_action)
self.toolbar.addAction(self.calibrate_action)
self.toolbar.addAction(self.clear_action)
self.toolbar.addAction(self.execut_action)
# Toolbar0
self.l0 = QLabel(self.widget)
self.l0.setText("Camera-Mount-Type: ")
self.l0.setFixedWidth(150)
self.l0.setAlignment(QtCore.Qt.AlignRight | QtCore.Qt.AlignVCenter)
self.combobox = QComboBox(self.widget)
self.combobox.addItem('attached on robot')
self.combobox.addItem('fixed beside robot')
self.toolbar0 = QToolBar()
self.toolbar0.addWidget(self.l0)
self.toolbar0.addWidget(self.combobox)
# Toolbar1
self.l1 = QLabel(self.widget)
self.l1.setText("Camera-Frame: ")
self.l1.setFixedWidth(150)
self.l1.setAlignment(QtCore.Qt.AlignRight | QtCore.Qt.AlignVCenter)
self.camera_frame = QLineEdit(self.widget)
self.camera_frame.setText("camera_link")
self.toolbar1 = QToolBar()
self.toolbar1.addWidget(self.l1)
self.toolbar1.addWidget(self.camera_frame)
# Toolbar2
self.l2 = QLabel(self.widget)
self.l2.setText("Object-Frame: ")
self.l2.setFixedWidth(150)
self.l2.setAlignment(QtCore.Qt.AlignRight | QtCore.Qt.AlignVCenter)
self.object_frame = QLineEdit(self.widget)
self.object_frame.setText("calib_board")
self.toolbar2 = QToolBar()
self.toolbar2.addWidget(self.l2)
self.toolbar2.addWidget(self.object_frame)
# Toolbar3
self.l3 = QLabel(self.widget)
self.l3.setText("Robot-Base-Frame: ")
self.l3.setFixedWidth(150)
self.l3.setAlignment(QtCore.Qt.AlignRight | QtCore.Qt.AlignVCenter)
self.base_frame = QLineEdit(self.widget)
self.base_frame.setText("base")
self.toolbar3 = QToolBar()
self.toolbar3.addWidget(self.l3)
self.toolbar3.addWidget(self.base_frame)
# Toolbar4
self.l4 = QLabel(self.widget)
self.l4.setText("End-Effector-Frame: ")
self.l4.setFixedWidth(150)
self.l4.setAlignment(QtCore.Qt.AlignRight | QtCore.Qt.AlignVCenter)
self.endeffector_frame = QLineEdit(self.widget)
self.endeffector_frame.setText("tool0")
self.toolbar4 = QToolBar()
self.toolbar4.addWidget(self.l4)
self.toolbar4.addWidget(self.endeffector_frame)
# Toolbar5
self.l5 = QLabel(self.widget)
self.l5.setText("Sample-Number: ")
self.l5.setFixedWidth(150)
self.l5.setAlignment(QtCore.Qt.AlignRight | QtCore.Qt.AlignVCenter)
self.le5 = QLineEdit(self.widget)
self.le5.setValidator(QIntValidator())
self.le5.setText('10')
self.le5.setReadOnly(True)
self.toolbar5 = QToolBar()
self.toolbar5.addWidget(self.l5)
self.toolbar5.addWidget(self.le5)
# TreeView
self.treeview = QTreeView()
self.treeview.setAlternatingRowColors(True)
self.model = QStandardItemModel(self.treeview)
self.treeview.setModel(self.model)
self.treeview.setHeaderHidden(True)
# TextEdit
self.textedit = QTextEdit(self.widget)
self.textedit.setReadOnly(True)
# Layout
self.layout = QVBoxLayout()
self.layout.addWidget(self.toolbar0)
self.layout.addWidget(self.toolbar1)
self.layout.addWidget(self.toolbar2)
self.layout.addWidget(self.toolbar3)
self.layout.addWidget(self.toolbar4)
self.layout.addWidget(self.toolbar5)
self.layout.addWidget(self.toolbar)
self.layoutH = QHBoxLayout()
self.layoutH.addWidget(self.treeview)
self.layoutH.addWidget(self.textedit)
self.layout.addLayout(self.layoutH)
self.widget.setLayout(self.layout)
# Add the widget to the user interface
if context.serial_number() > 1:
self.widget.setWindowTitle(self.widget.windowTitle() +
(' (%d)' % context.serial_number()))
context.add_widget(self.widget)
# Make the connections
self.snapshot_action.triggered.connect(self.take_snapshot)
self.calibrate_action.triggered.connect(self.calibration)
self.clear_action.triggered.connect(self.clear)
self.execut_action.triggered.connect(self.execution)
# Package path
self.path_pkg = path_pkg
# Set up TF
self.cli = self.node.create_client(HandeyeTF, 'handeye_tf_service')
while not self.cli.wait_for_service(timeout_sec=1.0):
self.node.get_logger().info(
'service not available, waiting again...')
self.req = HandeyeTF.Request()
def clear(self):
# >>> Clear the recorded samples
self.textedit.append('Clearing the recorded data ...')
self.textedit.clear()
self.Tsamples = []
self.model.clear()
def get_tf_transform(self, frame_id, child_frame_id):
self.req.transform.header.frame_id = frame_id
self.req.transform.child_frame_id = child_frame_id
self.req.publish.data = False
future = self.cli.call_async(self.req)
rclpy.spin_until_future_complete(self.node, future)
transform = TransformStamped()
try:
result = future.result()
except Exception as e:
self.node.get_logger().info('Service call failed %r' % (e, ))
else:
transform = result.tf_lookup_result
return transform
def publish_tf_transform(self, transform_to_publish):
self.req.publish.data = True
self.req.transform = transform_to_publish
future = self.cli.call_async(self.req)
rclpy.spin_until_future_complete(self.node, future)
try:
future.result()
except Exception as e:
self.node.get_logger().info('Service call failed %r' % (e, ))
else:
self.node.get_logger().info(
'Send the camera-robot transform :\n\tfrom `{}` to `{}`.'.
format(self.req.transform.header.frame_id,
self.req.transform.child_frame_id))
def take_snapshot(self):
# >>> Take the snapshot
self.textedit.append('Taking snapshot ...')
# Get the transform from `tool0` to `base_link`
T = self.get_tf_transform(self.base_frame.text(),
self.endeffector_frame.text())
bTe = np.zeros((4, 4))
q = [
T.transform.rotation.w, T.transform.rotation.x,
T.transform.rotation.y, T.transform.rotation.z
]
bTe = br.quaternion.to_transform(q)
bTe[:3, 3] = np.array([
T.transform.translation.x, T.transform.translation.y,
T.transform.translation.z
])
self.textedit.append('Lookup transform: from `{}` to `{}`.'.format(
self.base_frame.text(), self.endeffector_frame.text()))
self.node.get_logger().info(bcolors.OKGREEN + 'bTe:' + bcolors.ENDC +
'\n{}'.format(bTe))
# Get the transform from `calib_board` to `camera_link`
T = self.get_tf_transform(self.camera_frame.text(),
self.object_frame.text())
cTo = np.zeros((4, 4))
q = [
T.transform.rotation.w, T.transform.rotation.x,
T.transform.rotation.y, T.transform.rotation.z
]
cTo = br.quaternion.to_transform(q)
cTo[:3, 3] = np.array([
T.transform.translation.x, T.transform.translation.y,
T.transform.translation.z
])
self.textedit.append('Lookup transform: from `{}` to `{}`.'.format(
self.camera_frame.text(), self.object_frame.text()))
self.node.get_logger().info(bcolors.OKGREEN + 'cTo:' + bcolors.ENDC +
'\n{}'.format(cTo))
parent = QStandardItem('Snapshot {}'.format(len(self.Tsamples)))
child_1 = QStandardItem('bTe:\n{}\n{}\n{}\n{}'.format(
bTe[0, :], bTe[1, :], bTe[2, :], bTe[3, :]))
child_2 = QStandardItem('cTo:\n{}\n{}\n{}\n{}'.format(
cTo[0, :], cTo[1, :], cTo[2, :], cTo[3, :]))
parent.appendRow(child_1)
parent.appendRow(child_2)
self.model.appendRow(parent)
self.Tsamples.append((bTe, cTo))
self.le5.setText(str(len(self.Tsamples)))
def calibration(self):
# >>> Compute the calibration
self.textedit.append('Making the calibration ...')
if len(self.Tsamples) == 0:
self.textedit.append(
'No transform recorded, please take snapshots.')
return
# save samples to `dataset.json` file
save_samples_to_file(self.Tsamples)
import handeye
if self.combobox.currentIndex() == 0:
solver_cri = handeye.calibrator.HandEyeCalibrator(setup='Moving')
if self.combobox.currentIndex() == 1:
solver_cri = handeye.calibrator.HandEyeCalibrator(setup='Fixed')
for sample in self.Tsamples:
solver_cri.add_sample(sample[0], sample[1])
try:
bTc = solver_cri.solve(method=handeye.solver.Daniilidis1999)
# save the calibration result to 'camera-robot.json' file
file_output = '/tmp/' + 'camera-robot.json'
with open(file_output, 'w') as f:
json.dump(bTc.tolist(), f)
except Exception:
self.textedit.append("Failed to solve the hand-eye calibration.")
def execution(self):
# >>> Publish the camera-robot transform
self.textedit.append('Publishing the camera TF ...')
file_input = '/tmp/' + 'camera-robot.json'
with open(file_input, 'r') as f:
datastore = json.load(f)
to_frame = self.camera_frame.text()
if self.combobox.currentIndex() == 0:
from_frame = self.endeffector_frame.text()
if self.combobox.currentIndex() == 1:
from_frame = self.base_frame.text()
bTc = np.array(datastore)
static_transformStamped = TransformStamped()
static_transformStamped.header.stamp = ROSClock().now().to_msg()
static_transformStamped.header.frame_id = from_frame
static_transformStamped.child_frame_id = to_frame
static_transformStamped.transform.translation.x = bTc[0, 3]
static_transformStamped.transform.translation.y = bTc[1, 3]
static_transformStamped.transform.translation.z = bTc[2, 3]
q = br.transform.to_quaternion(bTc)
static_transformStamped.transform.rotation.x = q[1]
static_transformStamped.transform.rotation.y = q[2]
static_transformStamped.transform.rotation.z = q[3]
static_transformStamped.transform.rotation.w = q[0]
self.publish_tf_transform(static_transformStamped)
output_string = "camera-robot pose:\n"
output_string += " Translation: [{}, {}, {}]\n".format(
bTc[0, 3], bTc[1, 3], bTc[2, 3])
output_string += " Rotation: in Quaternion [{}, {}, {}, {}]".format(
q[0], q[1], q[2], q[3])
file_path = '/tmp/' + 'camera-robot.txt'
with open(file_path, 'w') as f:
f.write(output_string)
def shutdown_plugin(self):
"""
Unregister subscribers when the plugin shutdown
"""
pass
def save_settings(self, plugin_settings, instance_settings):
# Nothing to be done here
pass
def restore_settings(self, plugin_settings, instance_settings):
# Nothing to be done here
pass
class PidTuner(Plugin):
def __init__(self, context):
super(PidTuner, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('PidTuner')
# 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_pid_tuner'),
'resource', 'pid_tuner.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('PidTunerUI')
# 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.vals = {
Tuner.Pid1: {
PIDValue.Pl: 0.0,
PIDValue.Il: 0.0,
PIDValue.Dl: 0.0,
PIDValue.Sl: 0.0,
PIDValue.Pa: 0.0,
PIDValue.Ia: 0.0,
PIDValue.Da: 0.0,
PIDValue.Sa: 0.0,
"service": "",
"topic": ""
},
Tuner.Pid2: {
PIDValue.Pl: 0.0,
PIDValue.Il: 0.0,
PIDValue.Dl: 0.0,
PIDValue.Sl: 0.0,
PIDValue.Pa: 0.0,
PIDValue.Ia: 0.0,
PIDValue.Da: 0.0,
PIDValue.Sa: 0.0,
"service": "",
"topic": ""
},
Tuner.Inertial: {
InertialValue.Mass: 0.0,
InertialValue.Buoyancy: 0.0,
InertialValue.Ixx: 0.0,
InertialValue.Iyy: 0.0,
InertialValue.Izz: 0.0,
"service": "",
"topic": ""
}
}
self.cachedVals = {
Tuner.Pid1: {
PIDValue.Pl: 0.0,
PIDValue.Il: 0.0,
PIDValue.Dl: 0.0,
PIDValue.Sl: 0.0,
PIDValue.Pa: 0.0,
PIDValue.Ia: 0.0,
PIDValue.Da: 0.0,
PIDValue.Sa: 0.0
},
Tuner.Pid2: {
PIDValue.Pl: 0.0,
PIDValue.Il: 0.0,
PIDValue.Dl: 0.0,
PIDValue.Sl: 0.0,
PIDValue.Pa: 0.0,
PIDValue.Ia: 0.0,
PIDValue.Da: 0.0,
PIDValue.Sa: 0.0
},
Tuner.Inertial: {
InertialValue.Mass: 0.0,
InertialValue.Buoyancy: 0.0,
InertialValue.Ixx: 0.0,
InertialValue.Iyy: 0.0,
InertialValue.Izz: 0.0
}
}
self.sub_t1 = None
self.sub_t2 = None
self.sub_t3 = None
self._widget.t1_lp.textEdited.connect(
lambda t: self.on_pid_val_changed(t, Tuner.Pid1, PIDValue.Pl))
self._widget.t1_li.textEdited.connect(
lambda t: self.on_pid_val_changed(t, Tuner.Pid1, PIDValue.Il))
self._widget.t1_ld.textEdited.connect(
lambda t: self.on_pid_val_changed(t, Tuner.Pid1, PIDValue.Dl))
self._widget.t1_ls.textEdited.connect(
lambda t: self.on_pid_val_changed(t, Tuner.Pid1, PIDValue.Sl))
self._widget.t1_ap.textEdited.connect(
lambda t: self.on_pid_val_changed(t, Tuner.Pid1, PIDValue.Pa))
self._widget.t1_ai.textEdited.connect(
lambda t: self.on_pid_val_changed(t, Tuner.Pid1, PIDValue.Ia))
self._widget.t1_ad.textEdited.connect(
lambda t: self.on_pid_val_changed(t, Tuner.Pid1, PIDValue.Da))
self._widget.t1_as.textEdited.connect(
lambda t: self.on_pid_val_changed(t, Tuner.Pid1, PIDValue.Sa))
self._widget.t1_srv.editingFinished.connect(
lambda: self.on_srv_name_changed(self._widget.t1_srv, Tuner.Pid1))
self._widget.t1_tpc.editingFinished.connect(
lambda: self.on_topic_name_changed(self._widget.t1_tpc, Tuner.Pid1
))
self._widget.t1_read.pressed.connect(
lambda: self.on_reload_values(Tuner.Pid1))
self._widget.t1_send.pressed.connect(
lambda: self.on_send_values(Tuner.Pid1))
self._widget.t2_lp.textEdited.connect(
lambda t: self.on_pid_val_changed(t, Tuner.Pid2, PIDValue.Pl))
self._widget.t2_li.textEdited.connect(
lambda t: self.on_pid_val_changed(t, Tuner.Pid2, PIDValue.Il))
self._widget.t2_ld.textEdited.connect(
lambda t: self.on_pid_val_changed(t, Tuner.Pid2, PIDValue.Dl))
self._widget.t2_ls.textEdited.connect(
lambda t: self.on_pid_val_changed(t, Tuner.Pid2, PIDValue.Sl))
self._widget.t2_ap.textEdited.connect(
lambda t: self.on_pid_val_changed(t, Tuner.Pid2, PIDValue.Pa))
self._widget.t2_ai.textEdited.connect(
lambda t: self.on_pid_val_changed(t, Tuner.Pid2, PIDValue.Ia))
self._widget.t2_ad.textEdited.connect(
lambda t: self.on_pid_val_changed(t, Tuner.Pid2, PIDValue.Da))
self._widget.t2_as.textEdited.connect(
lambda t: self.on_pid_val_changed(t, Tuner.Pid2, PIDValue.Sa))
self._widget.t2_srv.editingFinished.connect(
lambda: self.on_srv_name_changed(self._widget.t2_srv, Tuner.Pid2))
self._widget.t2_tpc.editingFinished.connect(
lambda: self.on_topic_name_changed(self._widget.t2_tpc, Tuner.Pid2
))
self._widget.t2_read.pressed.connect(
lambda: self.on_reload_values(Tuner.Pid2))
self._widget.t2_send.pressed.connect(
lambda: self.on_send_values(Tuner.Pid2))
self._widget.t3_m.textEdited.connect(lambda t: self.on_pid_val_changed(
t, Tuner.Inertial, InertialValue.Mass))
self._widget.t3_b.textEdited.connect(lambda t: self.on_pid_val_changed(
t, Tuner.Inertial, InertialValue.Buoyancy))
self._widget.t3_ixx.textEdited.connect(
lambda t: self.on_pid_val_changed(t, Tuner.Inertial, InertialValue.
Ixx))
self._widget.t3_iyy.textEdited.connect(
lambda t: self.on_pid_val_changed(t, Tuner.Inertial, InertialValue.
Iyy))
self._widget.t3_izz.textEdited.connect(
lambda t: self.on_pid_val_changed(t, Tuner.Inertial, InertialValue.
Izz))
self._widget.t3_srv.editingFinished.connect(
lambda: self.on_srv_name_changed(self._widget.t3_srv, Tuner.
Inertial))
self._widget.t3_tpc.editingFinished.connect(
lambda: self.on_topic_name_changed(self._widget.t3_tpc, Tuner.
Inertial))
self._widget.t3_read.pressed.connect(
lambda: self.on_reload_values(Tuner.Inertial))
self._widget.t3_send.pressed.connect(
lambda: self.on_send_values(Tuner.Inertial))
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 on_pid_val_changed(self, text, tuner, pidvaluetype):
try:
self.vals[tuner][pidvaluetype] = float(text)
except ValueError as ve:
pass
self.update_text()
def on_srv_name_changed(self, source, tuner):
self.vals[tuner]["service"] = source.text()
def on_topic_name_changed(self, source, tuner):
self.vals[tuner]["topic"] = source.text()
self.resub()
def update_text(self):
self._widget.t1_lp.setText(str(self.vals[Tuner.Pid1][PIDValue.Pl]))
self._widget.t1_li.setText(str(self.vals[Tuner.Pid1][PIDValue.Il]))
self._widget.t1_ld.setText(str(self.vals[Tuner.Pid1][PIDValue.Dl]))
self._widget.t1_ls.setText(str(self.vals[Tuner.Pid1][PIDValue.Sl]))
self._widget.t1_ap.setText(str(self.vals[Tuner.Pid1][PIDValue.Pa]))
self._widget.t1_ai.setText(str(self.vals[Tuner.Pid1][PIDValue.Ia]))
self._widget.t1_ad.setText(str(self.vals[Tuner.Pid1][PIDValue.Da]))
self._widget.t1_as.setText(str(self.vals[Tuner.Pid1][PIDValue.Sa]))
self._widget.t1_srv.setText(str(self.vals[Tuner.Pid1]["service"]))
self._widget.t1_tpc.setText(str(self.vals[Tuner.Pid1]["topic"]))
self._widget.t2_lp.setText(str(self.vals[Tuner.Pid2][PIDValue.Pl]))
self._widget.t2_li.setText(str(self.vals[Tuner.Pid2][PIDValue.Il]))
self._widget.t2_ld.setText(str(self.vals[Tuner.Pid2][PIDValue.Dl]))
self._widget.t2_ls.setText(str(self.vals[Tuner.Pid2][PIDValue.Sl]))
self._widget.t2_ap.setText(str(self.vals[Tuner.Pid2][PIDValue.Pa]))
self._widget.t2_ai.setText(str(self.vals[Tuner.Pid2][PIDValue.Ia]))
self._widget.t2_ad.setText(str(self.vals[Tuner.Pid2][PIDValue.Da]))
self._widget.t2_as.setText(str(self.vals[Tuner.Pid2][PIDValue.Sa]))
self._widget.t2_srv.setText(str(self.vals[Tuner.Pid2]["service"]))
self._widget.t2_tpc.setText(str(self.vals[Tuner.Pid2]["topic"]))
self._widget.t3_m.setText(
str(self.vals[Tuner.Inertial][InertialValue.Mass]))
self._widget.t3_b.setText(
str(self.vals[Tuner.Inertial][InertialValue.Buoyancy]))
self._widget.t3_ixx.setText(
str(self.vals[Tuner.Inertial][InertialValue.Ixx]))
self._widget.t3_iyy.setText(
str(self.vals[Tuner.Inertial][InertialValue.Iyy]))
self._widget.t3_izz.setText(
str(self.vals[Tuner.Inertial][InertialValue.Izz]))
self._widget.t3_srv.setText(str(self.vals[Tuner.Inertial]["service"]))
self._widget.t3_tpc.setText(str(self.vals[Tuner.Inertial]["topic"]))
def on_reload_values(self, tuner):
if tuner == Tuner.Pid1:
self.on_topic_name_changed(self._widget.t1_tpc, tuner)
if tuner == Tuner.Pid2:
self.on_topic_name_changed(self._widget.t2_tpc, tuner)
if tuner == Tuner.Inertial:
self.on_topic_name_changed(self._widget.t3_tpc, tuner)
if tuner != Tuner.Inertial:
self.vals[tuner][PIDValue.Pl] = self.cachedVals[tuner][PIDValue.Pl]
self.vals[tuner][PIDValue.Il] = self.cachedVals[tuner][PIDValue.Il]
self.vals[tuner][PIDValue.Dl] = self.cachedVals[tuner][PIDValue.Dl]
self.vals[tuner][PIDValue.Sl] = self.cachedVals[tuner][PIDValue.Sl]
self.vals[tuner][PIDValue.Pa] = self.cachedVals[tuner][PIDValue.Pa]
self.vals[tuner][PIDValue.Ia] = self.cachedVals[tuner][PIDValue.Ia]
self.vals[tuner][PIDValue.Da] = self.cachedVals[tuner][PIDValue.Da]
self.vals[tuner][PIDValue.Sa] = self.cachedVals[tuner][PIDValue.Sa]
else:
self.vals[tuner][InertialValue.Mass] = self.cachedVals[tuner][
InertialValue.Mass]
self.vals[tuner][InertialValue.Buoyancy] = self.cachedVals[tuner][
InertialValue.Buoyancy]
self.vals[tuner][InertialValue.Iyy] = self.cachedVals[tuner][
InertialValue.Ixx]
self.vals[tuner][InertialValue.Ixx] = self.cachedVals[tuner][
InertialValue.Iyy]
self.vals[tuner][InertialValue.Izz] = self.cachedVals[tuner][
InertialValue.Izz]
self.update_text()
def on_send_values(self, tuner):
if tuner == Tuner.Pid1:
self.on_srv_name_changed(self._widget.t1_srv, tuner)
if tuner == Tuner.Pid2:
self.on_srv_name_changed(self._widget.t2_srv, tuner)
if tuner == Tuner.Inertial:
self.on_srv_name_changed(self._widget.t3_srv, tuner)
if tuner != Tuner.Inertial:
try:
tunesrv = rospy.ServiceProxy(self.vals[tuner]["service"],
TunePid)
tp = TunePid()
tp.l_p = self.vals[tuner][PIDValue.Pl]
tp.l_i = self.vals[tuner][PIDValue.Il]
tp.l_d = self.vals[tuner][PIDValue.Dl]
tp.l_sat = self.vals[tuner][PIDValue.Sl]
tp.a_p = self.vals[tuner][PIDValue.Pa]
tp.a_i = self.vals[tuner][PIDValue.Ia]
tp.a_d = self.vals[tuner][PIDValue.Da]
tp.a_sat = self.vals[tuner][PIDValue.Sa]
tunesrv(tp)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
else:
class CarlaControlPlugin(Plugin):
"""
RQT Plugin to control CARLA
"""
def __init__(self, context):
"""
Constructor
"""
super(CarlaControlPlugin, self).__init__(context)
self.setObjectName('CARLA Control')
self._widget = QWidget()
self._node = CompatibleNode('rqt_carla_control_node')
package_share_dir = roscomp.get_package_share_directory(
'rqt_carla_control')
ui_file = os.path.join(package_share_dir, 'resource',
'CarlaControl.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('CarlaControl')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
self.pause_icon = QIcon(
QPixmap(os.path.join(package_share_dir, 'resource', 'pause.png')))
self.play_icon = QIcon(
QPixmap(os.path.join(package_share_dir, 'resource', 'play.png')))
self._widget.pushButtonStepOnce.setIcon(
QIcon(
QPixmap(
os.path.join(package_share_dir, 'resource',
'step_once.png'))))
self.carla_status = None
self.carla_status_subscriber = self._node.new_subscription(
CarlaStatus,
"/carla/status",
self.carla_status_changed,
qos_profile=10)
self.carla_control_publisher = self._node.new_publisher(
CarlaControl, "/carla/control", qos_profile=10)
self._widget.pushButtonPlayPause.setDisabled(True)
self._widget.pushButtonStepOnce.setDisabled(True)
self._widget.pushButtonPlayPause.setIcon(self.play_icon)
self._widget.pushButtonPlayPause.clicked.connect(
self.toggle_play_pause)
self._widget.pushButtonStepOnce.clicked.connect(self.step_once)
context.add_widget(self._widget)
if roscomp.get_ros_version() == 2:
spin_thread = threading.Thread(target=self._node.spin, daemon=True)
spin_thread.start()
def toggle_play_pause(self):
"""
toggle play/pause
"""
if self.carla_status.synchronous_mode:
if self.carla_status.synchronous_mode_running:
self.carla_control_publisher.publish(
CarlaControl(command=CarlaControl.PAUSE))
else:
self.carla_control_publisher.publish(
CarlaControl(command=CarlaControl.PLAY))
def step_once(self):
"""
execute one step
"""
self.carla_control_publisher.publish(
CarlaControl(command=CarlaControl.STEP_ONCE))
def carla_status_changed(self, status):
"""
callback whenever carla status changes
"""
self.carla_status = status
if status.synchronous_mode:
self._widget.pushButtonPlayPause.setDisabled(False)
self._widget.pushButtonStepOnce.setDisabled(False)
if status.synchronous_mode_running:
self._widget.pushButtonPlayPause.setIcon(self.pause_icon)
else:
self._widget.pushButtonPlayPause.setIcon(self.play_icon)
else:
self._widget.pushButtonPlayPause.setDisabled(True)
self._widget.pushButtonStepOnce.setDisabled(True)
def shutdown_plugin(self):
"""
shutdown plugin
"""
self._node.destroy_subscription(self.carla_control_publisher)
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 QuaternionView(Plugin):
_draw = Signal()
def __init__(self, context):
super(QuaternionView, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('QuaternionView')
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_quaternion_view'), 'resource', 'QuaternionView.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('QuaternionView')
# 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.sub = None
self.topic_name = ""
self.topic_type = ""
self.topic_content = ""
self.refresh_rate = 5.0
self.refresh_period = rospy.Duration(0)
self.time_last = rospy.Time(0)
self.manual_mode = True
self.val = Quaternion(0.0,0.0,0.0,1.0)
self.update_refresh_period()
self.set_manual_mode(self.manual_mode)
self.plot_3d_figure = Figure()
self.plot_3d_figure.patch.set_facecolor('white')
self.plot_3d_canvas = FigureCanvas(self.plot_3d_figure)
self.plot_3d_toolbar = NavigationToolbar(self.plot_3d_canvas, self._widget.widget_plot_3d)
self.plot_3d_ax = self.plot_3d_figure.add_subplot(1,1,1, projection='3d')
self.set_plot_layout(self._widget.widget_plot_3d, self.plot_3d_toolbar, self.plot_3d_canvas)
self.plot_3d_ax.set_title(self.topic_name)
self.plot_3d_ax.set_xlabel('X')
self.plot_3d_ax.set_ylabel('Y')
self.plot_3d_ax.set_zlabel('Z')
self._draw.connect(self.update_display)
self.update_display()
def shutdown_plugin(self):
if self.sub is not None:
self.sub.unregister()
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value("topic_name", self.topic_name)
instance_settings.set_value("topic_type", self.topic_type)
instance_settings.set_value("topic_content", self.topic_content)
instance_settings.set_value("refresh_rate", self.refresh_rate)
instance_settings.set_value("manual_mode", str(self.manual_mode))
def restore_settings(self, plugin_settings, instance_settings):
topic_name = instance_settings.value("topic_name")
topic_type = instance_settings.value("topic_type")
topic_content = instance_settings.value("topic_content")
manual_mode = instance_settings.value("manual_mode")
refresh_rate = instance_settings.value("refresh_rate")
if (topic_name is not None) and (topic_type is not None) and (topic_content is not None) and (manual_mode is not None) and (refresh_rate is not None):
self.topic_name = str(topic_name)
self.topic_type = str(topic_type)
self.topic_content = str(topic_content)
self.manual_mode = (str(manual_mode).lower() == "true")
self.refresh_rate = self.parse_float(refresh_rate, default=5.0)
self.update_refresh_period()
self.set_manual_mode(self.manual_mode)
if (not self.manual_mode) and self.topic_name and self.topic_type and self.topic_content:
self.sub = rospy.Subscriber(self.topic_name, self.get_topic_class_from_type(self.topic_type), self.sub_callback)
def trigger_configuration(self):
self.open_settings_dialog()
def set_plot_layout(self, widget, toolbar, canvas):
layout = QVBoxLayout()
layout.addWidget(toolbar)
layout.addWidget(canvas)
widget.setLayout(layout)
def clear_plot(self):
# Discards the old graph data
artists = self.plot_3d_ax.lines + self.plot_3d_ax.collections
for artist in artists:
artist.remove()
def getKey(self,item):
return item[0]
def get_topic_class_from_type(self, msg_type):
connection_header = msg_type.split("/")
ros_pkg = connection_header[0] + ".msg"
msg_type = connection_header[1]
msg_class = getattr(import_module(ros_pkg), msg_type)
return msg_class
def get_topic_type(self, name):
topics = sorted(rospy.get_published_topics(), key=self.getKey)
topic_names, topic_types = zip(*topics)
topic_type = topic_types[topic_names.index(name)]
msg_class = self.get_topic_class_from_type(topic_type)
return topic_type, msg_class
def recursive_topic_content(self, msg_in, content):
attr = None
subcontent = content.split('/',1)
if len(subcontent) > 1:
attr = self.recursive_topic_content(getattr(msg_in, subcontent[0]), subcontent[1])
else:
attr = getattr(msg_in, content)
return attr
def sub_callback(self, msg_in):
now = rospy.Time.now()
if now > self.time_last:
# Rate-limit refreshes
if now - self.time_last > self.refresh_period:
self.val = Quaternion(0.0,0.0,0.0,1.0)
try:
new_val = self.recursive_topic_content(msg_in, self.topic_content)
if type(new_val) is Quaternion:
val = new_val
else:
raise TypeError
except AttributeError as e:
rospy.logwarn("AttributeError: " + str(e))
self.sub.unregister()
except TypeError as e:
rospy.logwarn("Unable to display " + str(getattr(msg_in, self.topic_content).__class__.__name__) + " as a quaternion")
self.sub.unregister()
self.val = val
self.time_last = now
self._draw.emit()
else:
# Timestep backwards, reset
self.time_last = rospy.Time(0)
def normalize_tf_quaternion(self,q):
d = math.sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3])
if d > 0.0:
q[0] = q[0]/d
q[1] = q[1]/d
q[2] = q[2]/d
q[3] = q[3]/d
else:
#Invalid quaternion, reset
q[0] = 0.0
q[1] = 0.0
q[2] = 0.0
q[3] = 1.0
return q
def update_display(self):
self.clear_plot()
x = [1.0,0.0,0.0,0.0]
x2 = [0.8,0.0,0.0,0.0]
z = [0.8,0.0,0.1,0.0]
q = self.normalize_tf_quaternion([self.val.x, self.val.y, self.val.z,self.val.w])
xr = tft.quaternion_multiply(tft.quaternion_multiply(q,x),tft.quaternion_inverse(q))
x2r = tft.quaternion_multiply(tft.quaternion_multiply(q,x2),tft.quaternion_inverse(q))
zr = tft.quaternion_multiply(tft.quaternion_multiply(q,z),tft.quaternion_inverse(q))
self.plot_3d_ax.plot([0.0,0.5], [0.0,0.0], [0.0,0.0], 'r-')
self.plot_3d_ax.plot([0.0,0.0], [0.0,0.5], [0.0,0.0], 'g-')
self.plot_3d_ax.plot([0.0,0.0], [0.0,0.0], [0.0,0.5], 'b-')
self.plot_3d_ax.plot([0.0,xr[0]], [0.0,xr[1]], [0.0,xr[2]], 'k-', linewidth=2)
self.plot_3d_ax.plot([zr[0],xr[0]], [zr[1],xr[1]], [zr[2],xr[2]], 'k-', linewidth=2)
self.plot_3d_ax.plot([zr[0],x2r[0]], [zr[1],x2r[1]], [zr[2],x2r[2]], 'k-', linewidth=2)
self.plot_3d_ax.set_aspect('equal', 'box')
self.plot_3d_ax.set_xlim(-1.0, 1.0)
self.plot_3d_ax.set_ylim(-1.0, 1.0)
self.plot_3d_ax.set_zlim(-1.0, 1.0)
self.plot_3d_canvas.draw()
if not self.manual_mode:
self.update_normalized_displays()
def parse_float(self, text, default=0.0):
val = default
try:
val = float(text)
except (ValueError, TypeError):
pass
return val
def manual_update(self, _=None, new_val=None, update_euler=True):
if type(new_val) is Quaternion:
self.val = new_val
else:
self.val = Quaternion(self.parse_float(self._widget.input_q_x.text()),
self.parse_float(self._widget.input_q_y.text()),
self.parse_float(self._widget.input_q_z.text()),
self.parse_float(self._widget.input_q_w.text()))
if update_euler:
q = [self.val.x, self.val.y, self.val.z,self.val.w]
e = tft.euler_from_quaternion(q, axes='rzyx')
self._widget.input_e_r.setText('%.5f' % e[0])
self._widget.input_e_p.setText('%.5f' % e[0])
self._widget.input_e_y.setText('%.5f' % e[0])
self._widget.input_e_r_deg.setText('%.5f' % math.degrees(e[2]))
self._widget.input_e_p_deg.setText('%.5f' % math.degrees(e[1]))
self._widget.input_e_y_deg.setText('%.5f' % math.degrees(e[0]))
self._draw.emit()
def manual_update_rpy(self, _=None, new_val=None, update_euler_degrees=True):
yaw = 0.0
pitch = 0.0
roll = 0.0
if new_val is None:
yaw = self.parse_float(self._widget.input_e_y.text())
pitch = self.parse_float(self._widget.input_e_p.text())
roll = self.parse_float(self._widget.input_e_r.text())
else:
yaw = new_val[0]
pitch = new_val[1]
roll = new_val[2]
q = tft.quaternion_from_euler(yaw, pitch, roll, axes='rzyx')
self._widget.input_q_w.setText('%.5f' % q[3])
self._widget.input_q_x.setText('%.5f' % q[0])
self._widget.input_q_y.setText('%.5f' % q[1])
self._widget.input_q_z.setText('%.5f' % q[2])
if update_euler_degrees:
self._widget.input_e_r_deg.setText('%.5f' % math.degrees(roll))
self._widget.input_e_p_deg.setText('%.5f' % math.degrees(pitch))
self._widget.input_e_y_deg.setText('%.5f' % math.degrees(yaw))
self.manual_update(new_val=Quaternion(q[0], q[1], q[2], q[3]), update_euler=False)
def manual_update_rpy_deg(self, _=None):
yaw = math.radians(self.parse_float(self._widget.input_e_y_deg.text()))
pitch = math.radians(self.parse_float(self._widget.input_e_p_deg.text()))
roll = math.radians(self.parse_float(self._widget.input_e_r_deg.text()))
self.manual_update_rpy(new_val=[yaw,pitch,roll], update_euler_degrees=False)
def update_normalized_displays(self):
q = self.normalize_tf_quaternion([self.val.x, self.val.y, self.val.z,self.val.w])
e = tft.euler_from_quaternion(q, axes='szyx')
self._widget.input_q_w.setText('%.5f' % q[3])
self._widget.input_q_x.setText('%.5f' % q[0])
self._widget.input_q_y.setText('%.5f' % q[1])
self._widget.input_q_z.setText('%.5f' % q[2])
self._widget.input_e_r.setText('%.5f' % e[2])
self._widget.input_e_p.setText('%.5f' % e[1])
self._widget.input_e_y.setText('%.5f' % e[0])
self._widget.input_e_r_deg.setText('%.5f' % math.degrees(e[2]))
self._widget.input_e_p_deg.setText('%.5f' % math.degrees(e[1]))
self._widget.input_e_y_deg.setText('%.5f' % math.degrees(e[0]))
def set_manual_mode(self, manual):
if manual:
print("Manual mode")
if self.sub is not None:
self.sub.unregister()
self._widget.input_q_w.textEdited.connect(self.manual_update)
self._widget.input_q_x.textEdited.connect(self.manual_update)
self._widget.input_q_y.textEdited.connect(self.manual_update)
self._widget.input_q_z.textEdited.connect(self.manual_update)
self._widget.input_e_r.textEdited.connect(self.manual_update_rpy)
self._widget.input_e_p.textEdited.connect(self.manual_update_rpy)
self._widget.input_e_y.textEdited.connect(self.manual_update_rpy)
self._widget.input_e_r_deg.textEdited.connect(self.manual_update_rpy_deg)
self._widget.input_e_p_deg.textEdited.connect(self.manual_update_rpy_deg)
self._widget.input_e_y_deg.textEdited.connect(self.manual_update_rpy_deg)
self._widget.input_q_w.returnPressed.connect(self.update_normalized_displays)
self._widget.input_q_x.returnPressed.connect(self.update_normalized_displays)
self._widget.input_q_y.returnPressed.connect(self.update_normalized_displays)
self._widget.input_q_z.returnPressed.connect(self.update_normalized_displays)
self._widget.input_e_r.returnPressed.connect(self.update_normalized_displays)
self._widget.input_e_p.returnPressed.connect(self.update_normalized_displays)
self._widget.input_e_y.returnPressed.connect(self.update_normalized_displays)
self._widget.input_e_r_deg.returnPressed.connect(self.update_normalized_displays)
self._widget.input_e_p_deg.returnPressed.connect(self.update_normalized_displays)
self._widget.input_e_y_deg.returnPressed.connect(self.update_normalized_displays)
else:
print("Subscriber mode")
try:
self._widget.input_q_w.textEdited.disconnect()
self._widget.input_q_x.textEdited.disconnect()
self._widget.input_q_y.textEdited.disconnect()
self._widget.input_q_z.textEdited.disconnect()
self._widget.input_e_r.textEdited.disconnect()
self._widget.input_e_p.textEdited.disconnect()
self._widget.input_e_y.textEdited.disconnect()
self._widget.input_e_r_deg.textEdited.disconnect()
self._widget.input_e_p_deg.textEdited.disconnect()
self._widget.input_e_y_deg.textEdited.disconnect()
self._widget.input_q_w.returnPressed.disconnect()
self._widget.input_q_x.returnPressed.disconnect()
self._widget.input_q_y.returnPressed.disconnect()
self._widget.input_q_z.returnPressed.disconnect()
self._widget.input_e_r.returnPressed.disconnect()
self._widget.input_e_p.returnPressed.disconnect()
self._widget.input_e_y.returnPressed.disconnect()
self._widget.input_e_r_deg.returnPressed.disconnect()
self._widget.input_e_p_deg.returnPressed.disconnect()
self._widget.input_e_y_deg.returnPressed.disconnect()
except TypeError:
pass
def update_refresh_period(self):
self.refresh_period = rospy.Duration( 1.0 / self.refresh_rate )
def open_settings_dialog(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_topic_list("topic_list", str(self.topic_name), "Topics")
dialog.add_combobox_empty("content_list", "Contents", str(self.topic_content))
dialog.add_lineedit("refresh_rate", str(self.refresh_rate), "Refresh Rate")
dialog.add_checkbox("manual_mode", bool(self.manual_mode), "Manual Mode")
settings = dialog.get_settings();
if settings is not None:
for s in settings:
if s[0] == "topic_list":
self.topic_name = str(s[1])
elif s[0] == "content_list":
self.topic_content = str(s[1])
elif s[0] == "refresh_rate":
self.refresh_rate = self.parse_float(s[1])
elif s[0] == "manual_mode":
self.manual_mode = bool(s[1])
self.update_refresh_period()
if self.manual_mode:
self.set_manual_mode(True)
elif self.topic_name and self.topic_content:
self.topic_type, msg_class = self.get_topic_type(self.topic_name)
self.sub = rospy.Subscriber(self.topic_name, msg_class, self.sub_callback)
self._draw.emit()
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 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 RqtInjerrobot(Plugin):
def __init__(self, context):
super(RqtInjerrobot, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('RqtInjerrobot')
# 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_injerrobot'),
'resource', 'RqtInjerrobot.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('RqtInjerrobotUi')
# add signals/slots
self._widget.selectYamlPushButton.pressed.connect(
self.press_select_yaml)
self._widget.loadYamlPushButton.pressed.connect(self.press_load_yaml)
self._widget.saveYamlPushButton.pressed.connect(self.press_save_yaml)
self._widget.armBox.currentIndexChanged.connect(self.arm_selected)
self._widget.armBox.highlighted.connect(self.arm_activated)
self._widget.editStepPushButton.pressed.connect(self.press_edit_step)
self._widget.actionNSEdit.editingFinished.connect(self.edited_actionns)
self._widget.groupNameEdit.editingFinished.connect(
self.edited_groupname)
self._widget.baseLinkEdit.editingFinished.connect(self.edited_baselink)
self._widget.endEffectorEdit.editingFinished.connect(
self.edited_endeffector)
# 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._yaml_file = ""
self._params = dict()
self._name = "RqtInjerrobot"
self._keys_not_steps = [
'arm_ip', 'arm_port', 'joint_names', 'group_name', 'action_ns'
]
self._goto_dialog = RqtGoto()
# controller
def edited_actionns(self):
self.set_current_actionns(self._widget.actionNSEdit.text())
def edited_groupname(self):
self.set_current_movegroup_name(self._widget.groupNameEdit.text())
def edited_baselink(self):
self.set_current_base_link(self._widget.baseLinkEdit.text())
def edited_endeffector(self):
self.set_current_end_effector(self._widget.endEffectorEdit.text())
def arm_activated(self, val):
print 'arm_activated', val
def get_arm_names(self):
return self._params.keys()
def get_current_arm_name(self):
current_arm_name = self._widget.armBox.currentText()
if current_arm_name in self.get_arm_names():
return current_arm_name
return None
def get_steps(self):
steps = None
try:
steps = [
k for k in self._params[self.get_current_arm_name()].keys()
if k not in self._keys_not_steps
]
except KeyError, e:
rospy.logerr('%s::%s: : cannot get steps for %s. %s' %
(self._name, self.get_function_name(),
self.get_current_arm_name(), e))
return steps
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):
# If the current value of sliders is zero directly send stop twist msg
if self._widget.x_linear_slider.value() == 0 and \
self._widget.z_angular_slider.value() == 0:
self.zero_cmd_sent = False
self._on_parameter_changed()
else:
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 MyPlugin(Plugin):
def __init__(self, context):
self.cmd_pub = rospy.Publisher("/welder/cmd", String, queue_size=1)
self.welder_status_sub = rospy.Subscriber('/welder/status',
String,
self.status_callback,
queue_size=10)
self.welder_time_sub = rospy.Subscriber('/welder/time',
String,
self.time_callback,
queue_size=10)
self.welder_progress_sub = rospy.Subscriber('/welder/progress',
String,
self.progress_callback,
queue_size=10)
self.image_sub = rospy.Subscriber("/laser_ctrl/img_feed/compressed",
CompressedImage,
self.image_callback,
queue_size=10)
self.laser_state_sub = rospy.Subscriber('/laser_ctrl/cmd',
String,
self.laser_state_clbk,
queue_size=10)
super(MyPlugin, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('welder_node_gui')
# 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(f"arguments: {args}")
print(f"unknowns: {unknowns}")
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('welder_node_gui'),
'resource', '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('Welder Robot GUI')
# 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()))
context.add_widget(self._widget)
self._widget.startButton.clicked.connect(self.start_cnc)
self._widget.stopButton.clicked.connect(self.stop_cnc)
self._widget.timeLabel.setText('None')
def start_cnc(self):
self.cmd_pub.publish('1')
def progress_slot(self, data):
blade_count = int(
ros_data.data) # this will not work, ros_data not defined
progress = int(float(blade_count) / 256 * 100)
grids = int((blade_count / 16))
self._widget.progressBar.setValue(progress)
self._widget.bladeNumber.display(blade_count)
self._widget.gridNumber.display(grids)
def stop_cnc(self):
self.cmd_pub.publish('0')
self._widget.timeLabel.setText('None')
def image_callback(self, ros_data):
np_arr = np.fromstring(ros_data.data, np.uint8)
image = cv.imdecode(np_arr, cv.IMREAD_COLOR)
height, width, _ = image.shape
bytes_per_line = 3 * width
qImage = QImage(image.data, width, height, bytes_per_line,
QImage.Format_RGB888).rgbSwapped()
pix = QPixmap(qImage)
h_label = self._widget.videoDisplay.height()
w_label = self._widget.videoDisplay.width()
self._widget.videoDisplay.setPixmap(
pix.scaled(w_label, h_label, Qt.KeepAspectRatio))
def time_callback(self, ros_data):
time_text = ros_data.data + ' s'
self._widget.timeLabel.setText(time_text)
def progress_callback(self, ros_data):
self.emit(Signal("changeUI(PyQt_PyObject)"), ros_data.data)
def laser_state_clbk(self, ros_data):
if ros_data.data == 's':
self._widget.laserStatusLabel.setText('OFF')
elif ros_data.data == 'f':
self._widget.laserStatusLabel.setText('ON')
def status_callback(self, ros_data):
if ros_data.data != '0':
self._widget.startButton.setEnabled(False)
self._widget.stopButton.setEnabled(True)
else:
self._widget.startButton.setEnabled(True)
self._widget.stopButton.setEnabled(False)
self._widget.statusLabel.setText(WELDER_STATES[int(ros_data.data)])
def shutdown_plugin(self):
# TODO (Pablo): unregister all publishers here
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO (Pablo): save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO (Pablo): restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
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 CarlaControlPlugin(Plugin):
"""
RQT Plugin to control CARLA
"""
def __init__(self, context):
"""
Constructor
"""
super(CarlaControlPlugin, self).__init__(context)
self.setObjectName('CARLA Control')
self._widget = QWidget()
ui_file = os.path.join(rospkg.RosPack().get_path('rqt_carla_control'),
'resource', 'CarlaControl.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('CarlaControl')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
self.pause_icon = QIcon(
QPixmap(
os.path.join(rospkg.RosPack().get_path('rqt_carla_control'),
'resource', 'pause.png')))
self.play_icon = QIcon(
QPixmap(
os.path.join(rospkg.RosPack().get_path('rqt_carla_control'),
'resource', 'play.png')))
self._widget.pushButtonStepOnce.setIcon(
QIcon(
QPixmap(
os.path.join(
rospkg.RosPack().get_path('rqt_carla_control'),
'resource', 'step_once.png'))))
self.carla_status = None
self.carla_status_subscriber = rospy.Subscriber(
"/carla/status", CarlaStatus, self.carla_status_changed)
self.carla_control_publisher = rospy.Publisher("/carla/control",
CarlaControl,
queue_size=10)
self._widget.pushButtonPlayPause.setDisabled(True)
self._widget.pushButtonStepOnce.setDisabled(True)
self._widget.pushButtonPlayPause.setIcon(self.play_icon)
self._widget.pushButtonPlayPause.clicked.connect(
self.toggle_play_pause)
self._widget.pushButtonStepOnce.clicked.connect(self.step_once)
context.add_widget(self._widget)
def toggle_play_pause(self):
"""
toggle play/pause
"""
if self.carla_status.synchronous_mode:
if self.carla_status.synchronous_mode_running:
self.carla_control_publisher.publish(
CarlaControl(command=CarlaControl.PAUSE))
else:
self.carla_control_publisher.publish(
CarlaControl(command=CarlaControl.PLAY))
def step_once(self):
"""
execute one step
"""
self.carla_control_publisher.publish(
CarlaControl(command=CarlaControl.STEP_ONCE))
def carla_status_changed(self, status):
"""
callback whenever carla status changes
"""
self.carla_status = status
if status.synchronous_mode:
self._widget.pushButtonPlayPause.setDisabled(False)
self._widget.pushButtonStepOnce.setDisabled(False)
if status.synchronous_mode_running:
self._widget.pushButtonPlayPause.setIcon(self.pause_icon)
else:
self._widget.pushButtonPlayPause.setIcon(self.play_icon)
else:
self._widget.pushButtonPlayPause.setDisabled(True)
self._widget.pushButtonStepOnce.setDisabled(True)
def shutdown_plugin(self):
"""
shutdown plugin
"""
self.carla_control_publisher.unregister()
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(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)
# These directions are matched to the Arduino code
self.directions = {
0: "Forward",
1: "Back",
2: "Left",
3: "Right",
4: "ForwardLeft",
5: "BackRight",
6: "ForwardRight",
7: "BackLeft",
8: "CW",
9: "CCW"
}
self.directionsinv = {
"Forward": 0,
"Back": 1,
"Left": 2,
"Right": 3,
"ForwardLeft": 4,
"BackRight": 5,
"ForwardRight": 6,
"BackLeft": 7,
"CW": 8,
"CCW": 9
}
self.states = {
0: "Status : Stopped",
1: "Status : Moving",
2: "Status : Calibration Mode",
3: "Status : Mapping",
4: "Status : Measuring"
}
self.robotstate = 1
self._widget.Forward.pressed.connect(self.on_direction_press)
self._widget.Back.pressed.connect(self.on_direction_press)
self._widget.Left.pressed.connect(self.on_direction_press)
self._widget.Right.pressed.connect(self.on_direction_press)
self._widget.ForwardLeft.pressed.connect(self.on_direction_press)
self._widget.ForwardRight.pressed.connect(self.on_direction_press)
self._widget.BackLeft.pressed.connect(self.on_direction_press)
self._widget.BackRight.pressed.connect(self.on_direction_press)
self._widget.CW.pressed.connect(self.on_direction_press)
self._widget.CCW.pressed.connect(self.on_direction_press)
self._widget.Go.pressed.connect(self.on_go_pressed)
self._widget.Stop.pressed.connect(self.on_stop_pressed)
#
# self._widget.EncoderLinearSpinBox.valueChanged.connect(self.on_encoder_linear_spinbox_pressed)
# self._widget.EncoderLinearSlider.valueChanged.connect(self.on_encoder_linear_slider_pressed)
#
# self.on_encoder_linear_spinbox_pressed()
#
# self._widget.EncoderRotationSpinBox.valueChanged.connect(self.on_encoder_rotary_spinbox_pressed)
# self._widget.EncoderRotationSlider.valueChanged.connect(self.on_encoder_rotary_slider_pressed)
#
# self.on_encoder_rotary_spinbox_pressed()
#
self._widget.ServoSpinBox.valueChanged.connect(
self.on_servo_spinbox_pressed)
self._widget.ServoSlider.valueChanged.connect(
self.on_servo_slider_pressed)
self._widget.ServoButton.pressed.connect(self.on_servo_button_pressed)
self.on_servo_spinbox_pressed()
self._widget.MicSpinBox.valueChanged.connect(
self.on_mic_spinbox_pressed)
self._widget.MicSlider.valueChanged.connect(self.on_mic_slider_pressed)
self._widget.MicButton.pressed.connect(self.on_mic_button_pressed)
self.on_mic_spinbox_pressed()
self._widget.MicCalibrateButton.pressed.connect(
self.on_mic_calibrate_button_pressed)
self._widget.MicDirectionButton.pressed.connect(
self.on_mic_direction_button_pressed)
self.micdirection = 0
self._widget.CameraButton.pressed.connect(
self.on_camera_button_pressed)
self._widget.SweepButton.pressed.connect(self.on_sweep_button_pressed)
self._widget.ImpulseButton.pressed.connect(
self.on_impulse_button_pressed)
self._widget.CalibrateEncoderButton.pressed.connect(
self.on_calibrate_encoder_button_pressed)
self._widget.FrequencySlider.valueChanged.connect(
self.on_frequency_slider_pressed)
self._widget.FrequencySpinBox.valueChanged.connect(
self.on_frequency_spinbox_pressed)
# Camera image selection button groups
self.bg1 = QButtonGroup()
self.bg2 = QButtonGroup()
self.bg1.addButton(self._widget.LeftImageCheckBox)
self.bg1.addButton(self._widget.RightImageCheckBox)
self.bg1.addButton(self._widget.DisparityImageCheckBox)
self.bg2.addButton(self._widget.NormalImageCheckBox)
self.bg2.addButton(self._widget.EdgeImageCheckBox)
self.bg2.addButton(self._widget.BWImageCheckBox)
self.bg2.addButton(self._widget.KeypointsImageCheckBox)
self._widget.LeftImageCheckBox.pressed.connect(
self.on_camera_type_changed)
self._widget.RightImageCheckBox.pressed.connect(
self.on_camera_type_changed)
self._widget.DisparityImageCheckBox.pressed.connect(
self.on_camera_type_changed)
self._widget.EdgeImageCheckBox.pressed.connect(
self.on_image_type_changed)
self._widget.BWImageCheckBox.pressed.connect(
self.on_image_type_changed)
self._widget.KeypointsImageCheckBox.pressed.connect(
self.on_image_type_changed)
self._widget.NormalImageCheckBox.pressed.connect(
self.on_image_type_changed)
self.bg1.buttonClicked.connect(self.on_camera_type_changed)
self.bg2.buttonClicked.connect(self.on_image_type_changed)
self.on_camera_type_changed()
self.on_image_type_changed()
self.camera_type = 2
self.image_type = 2
self.micHeight = 15
self.encoderCalibMode = False
# Connect to Arduino's subscriber to control it
self.pub2ard = rospy.Publisher('arduinoSub',
Arduinostate,
queue_size=10)
self.pub2pi = rospy.Publisher('imagetrigger', Int8, queue_size=10)
self.pub2spkr = rospy.Publisher('speakerListener', Int8, queue_size=10)
self.pub2mic = rospy.Publisher('audiotrigger', String, queue_size=10)
self.r = rospy.Rate(5)
self.arduinomsg = Arduinostate()
#self.pimsg = Bool()
self.pimsg = Int8()
self.spkrmsg = Int8()
self.micmsg = String()
self.mapSub = rospy.Subscriber("mapPub", String, self.plotCallback)
self.mapPub = rospy.Publisher("mapSub", String, queue_size=10)
self.mapmsg = String()
self.mapUtility = mapping.Map()
self.mapUtility.z = 5
self.freqPlot = pg.plot(title="Frequency Spectrum")
self.distFreqPlot = pg.plot(title="Frequency vs. Distance")
self.contourPlot = pg.ImageView(name="2D Power - Single Frequency")
S = audiopath + "AAsaw.wav"
fs, audio = read(S)
self.freqPlot.plot(abs(fft(audio)))
self.lastaudio = ""
self.audiofiles = []
def plotCallback(self, data):
S = data.data
if (S != self.lastaudio):
fs, audio = read(audiopath + S + ".wav")
self.audiofiles.append(abs(fft(audio)))
self.freqPlot.clear()
self.freqPlot.plot(abs(fft(audio)))
self.lastaudio = S
print("Showing plot of " + self.lastaudio)
x = linspace(-100, 100, 201)
xx, yy = meshgrid(x, x)
# Go through audio files at the given height
# Get power at the given frequency for all x,y
#self.contourPlot.setImage(z)
# Need separate function for Freq, amplitude distance in 1D and 2D
# Might want one for updating files and another for updating frequency
def distancePlot(self, axis):
if (axis == 0): # x
# Search for all files in path that have different x values
g = os.listdir(audiopath)
A = []
x = []
for k in range(len(g)):
if "wav" in g:
fs, audio = read(g[k])
x.append(int(g[0]))
A.append(20 * log10(abs(fft(audio))))
self.distancePlot.plot(x, A)
def talker(self, msgtype):
if (msgtype == 0):
self.arduinomsg.toggleVelocity = True
self.arduinomsg.toggleServo = False
self.arduinomsg.toggleFunction = False
self.arduinomsg.vel_x = 0
self.arduinomsg.vel_y = 0
self.arduinomsg.vel_theta = 0
elif (msgtype == 1): # Go in a direction for some time
self.arduinomsg.toggleFunction = True
self.arduinomsg.toggleVelocity = False
self.arduinomsg.toggleServo = False
direction = self.directionsinv[
self._widget.DirectionTextBox.text()]
time = float(self._widget.TimeLineEdit.text())
self.arduinomsg.functionSelect = 1
self.arduinomsg.functionIntParam = direction
self.arduinomsg.functionFloatParam = time
self.mapUtility.updatePosition(direction, time)
self.mapmsg = "%d,%d,%d,%d,0" % (
self.mapUtility.x, self.mapUtility.y, self.mapUtility.t,
self.mapUtility.z)
self.mapPub.publish(self.mapmsg)
# Need additional part of message to tell map to make mic marker
self.r.sleep()
S = self.mapmsg.split(",")
self.arduinomsg.x = int(S[0])
self.arduinomsg.y = int(S[1])
self.arduinomsg.theta = int(S[2])
self.arduinomsg.micPosition = int(S[3])
# For mapping, need to update coordinates here+
elif (msgtype == 2): # Velocity control
self.arduinomsg.toggleVelocity = True
self.arduinomsg.toggleFunction = False
self.arduinomsg.toggleServo = False
elif (msgtype == 3): # Servo
self.arduinomsg.toggleServo = True
self.arduinomsg.toggleVelocity = False
self.arduinomsg.toggleFunction = False
self.arduinomsg.servoangle = int8(
self._widget.ServoSpinBox.value())
elif (msgtype == 4): # Mic
self.arduinomsg.toggleFunction = True
self.arduinomsg.toggleServo = False
self.arduinomsg.toggleVelocity = False
self.arduinomsg.functionSelect = 3
self.arduinomsg.functionIntParam = int8(self.micdirection)
self.arduinomsg.functionFloatParam = self._widget.MicSpinBox.value(
)
self.mapUtility.z += (-1)**(self.micdirection)
self.mapmsg = "%d,%d,%d,%d,0" % (
self.mapUtility.x, self.mapUtility.y, self.mapUtility.t,
self.mapUtility.z)
self.mapPub.publish(self.mapmsg)
self.r.sleep()
S = self.mapmsg.split(",")
self.arduinomsg.x = int(S[0])
self.arduinomsg.y = int(S[1])
self.arduinomsg.theta = int(S[2])
self.arduinomsg.micPosition = int(S[3])
elif (msgtype == 5): # Mic calibrate
self.arduinomsg.toggleFunction = True
self.arduinomsg.toggleServo = False
self.arduinomsg.toggleVelocity = False
self.arduinomsg.functionSelect = 4
self.arduinomsg.functionIntParam = int8(
self._widget.MicCalibrateSpinBox.value())
elif (msgtype == 6): # Measure with mic
pass
# Get position of robot and mic height
# Send trigger then start recording
# Place marker on the map at the location and save WAV file
elif (msgtype == 7): # Encoder calibrate
self.arduinomsg.toggleFunction = True
self.arduinomsg.toggleServo = False
self.arduinomsg.toggleVelocity = False
direction = self.directionsinv[
self._widget.DirectionTextBox.text()]
time = float(self._widget.TimeLineEdit.text())
self.arduinomsg.functionSelect = 2
self.arduinomsg.functionIntParam = direction
self.arduinomsg.functionFloatParam = time
numTrials = 10
for k in range(numTrials):
x1 = int16(
subprocess.check_output("rostopic echo /arduinoPub/x -n1",
shell=True).split('\n')[0])
y1 = int16(
subprocess.check_output("rostopic echo /arduinoPub/y -n1",
shell=True).split('\n')[0])
z1 = int16(
subprocess.check_output(
"rostopic echo /arduinoPub/theta -n1",
shell=True).split('\n')[0])
self.pub2ard.publish(self.arduinomsg)
x2 = int16(
subprocess.check_output("rostopic echo /arduinoPub/x -n1",
shell=True).split('\n')[0])
y2 = int16(
subprocess.check_output("rostopic echo /arduinoPub/y -n1",
shell=True).split('\n')[0])
z2 = int16(
subprocess.check_output(
"rostopic echo /arduinoPub/theta -n1",
shell=True).split('\n')[0])
dx = abs(x2) - abs(x1)
dy = abs(y2) - abs(y1)
dz = abs(z2) - abs(z1)
# Error is larger with long times, not reading output correctly
#print("Trial %d : %d " %(k,dy - dz))
# Reverse to return to original position
if (direction == 0):
print("Trial %d: Error : %d , Total y: %d, theta: %d" %
(k, dy - dz, dy, dz))
self.arduinomsg.functionIntParam = 1
self.pub2ard.publish(self.arduinomsg)
#time.sleep(time)
self.arduinomsg.functionIntParam = 0
# Right back minus left back
# Positive error means increase left wheel speed or decrease right wheel speed
if (direction == 1):
print("Trial %d: Error : %d , Total y: %d, theta: %d" %
(k, dy - dz, dy, dz))
self.arduinomsg.functionIntParam = 0
self.pub2ard.publish(self.arduinomsg)
#time.sleep(time)
self.arduinomsg.functionIntParam = 1
# Left and right might be tricky...
if (direction == 8 or direction == 9):
print("Trial %d : %d, %d, %d " % (k, dx, dy, dz))
rospy.loginfo(self.arduinomsg) # Logging sent message, no received!
self.pub2ard.publish(self.arduinomsg)
self.r.sleep()
def shutdown_plugin(self):
sys.exit()
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 updateState(self, newstate):
self.robotstate = self.states[newstate]
self._widget.StatusTextBox.setText(self.robotstate)
def on_direction_press(self):
self._widget.DirectionTextBox.setText(self.sender().objectName())
def on_stop_pressed(self):
self.updateState(0)
self.talker(0)
def on_go_pressed(self):
self.updateState(1)
self.talker(1)
def on_frequency_spinbox_pressed(self):
self._widget.FrequencySlider.setValue(
self._widget.FrequencySpinBox.value())
def on_frequency_slider_pressed(self):
self._widget.FrequencySpinBox.setValue(
self._widget.FrequencySlider.value())
def on_servo_spinbox_pressed(self):
self._widget.ServoSlider.setValue(self._widget.ServoSpinBox.value())
def on_servo_slider_pressed(self):
self._widget.ServoSpinBox.setValue(self._widget.ServoSlider.value())
def on_mic_spinbox_pressed(self):
self._widget.MicSlider.setValue(self._widget.MicSpinBox.value())
def on_mic_slider_pressed(self):
self._widget.MicSpinBox.setValue(self._widget.MicSlider.value())
def on_mic_button_pressed(self):
self.talker(4)
def on_mic_calibrate_button_pressed(self):
self._widget.MicCalibrateButton.setText(
"Calibrate %i" % self._widget.MicCalibrateSpinBox.value())
self.talker(5)
def on_mic_direction_button_pressed(self):
if (self.micdirection): # If true, set to down
self._widget.MicDirectionButton.setText("Mic Up")
self.micdirection = 0
else:
self._widget.MicDirectionButton.setText("Mic Down")
self.micdirection = 1
def on_servo_button_pressed(self):
self.talker(3)
# Loop it here instead of in arduino
def on_calibrate_encoder_button_pressed(self):
if (self.encoderCalibMode):
self.encoderCalibMode = False
self._widget.CalibrateMicButton.setText("Calibrate: Off")
else:
self.encoderCalibMode = True
self.talker(6)
self._widget.CalibrateEncoderButton.setText("Calibrate: On")
def on_camera_button_pressed(self):
if (self.camera_type == 2):
if (self.image_type == 2):
self.pimsg = 1
elif (self.image_type == 3):
self.pimsg = 2
elif (self.image_type == 4):
self.pimsg = 3
elif (self.image_type == 5):
self.pimsg = 4
elif (self.camera_type == 3): # Right
if (self.image_type == 2):
self.pimsg = 5
elif (self.image_type == 3):
self.pimsg = 6
elif (self.image_type == 4):
self.pimsg = 7
elif (self.image_type == 5):
self.pimsg = 8
elif (self.camera_type == 4):
self.pimsg = 7 # Disparity map
self.pub2pi.publish(self.pimsg)
def on_camera_type_changed(self):
self.camera_type = -1 * self.bg1.checkedId()
#self._widget.CameraButton.setText("Camera %i" % self.camera_type )
def on_image_type_changed(self):
self.image_type = -1 * self.bg2.checkedId()
#self._widget.CameraButton.setText("Camera %i" % self.image_type )
'''
When measurement button is triggered, get the current position to save as a marker
'''
def on_sweep_button_pressed(self):
self.spkrmsg = 0
self.mapmsg = "%d,%d,%d,%d,1" % (self.mapUtility.x, self.mapUtility.y,
self.mapUtility.t, self.mapUtility.z)
self.micmsg = self.mapmsg
self.pub2mic.publish(self.micmsg)
self.pub2spkr.publish(self.spkrmsg)
self.r.sleep() # Wait until recording is done to add marker
self.mapPub.publish(self.mapmsg)
S = self.mapmsg.split(",")
self.arduinomsg.x = int(S[0])
self.arduinomsg.y = int(S[1])
self.arduinomsg.theta = int(S[2])
self.arduinomsg.micPosition = int(S[3])
def on_impulse_button_pressed(self):
self.spkrmsg = 2
self.pub2spkr.publish(self.spkrmsg)
# Calibrate cameras individually and in stereo to get camera parameters
def calibrateCameras(self):
self.updateState(2)
def mappingMode(self):
self.updateState(3)
def measurementMode(self):
self.updateState(4)
class Packml(Plugin):
def __init__(self, context):
super(Packml, self).__init__(context)
self.setObjectName('Packml')
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())
if not args.quiet:
print 'arguments: ', args
print 'unknowns: ', unknowns
# Create QWidget
self._widget = QWidget()
ui_file = os.path.join(rospkg.RosPack().get_path('packml_gui'), 'resource', 'packml.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('Packml')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
context.add_widget(self._widget)
# Custom code begins here
self._widget.reset_button.clicked[bool].connect(self.__handle_reset_clicked)
self._widget.start_button.clicked[bool].connect(self.__handle_start_clicked)
self._widget.stop_button.clicked[bool].connect(self.__handle_stop_clicked)
self._widget.clear_button.clicked[bool].connect(self.__handle_clear_clicked)
self._widget.hold_button.clicked[bool].connect(self.__handle_hold_clicked)
self._widget.unhold_button.clicked[bool].connect(self.__handle_unhold_clicked)
self._widget.suspend_button.clicked[bool].connect(self.__handle_suspend_clicked)
self._widget.unsuspend_button.clicked[bool].connect(self.__handle_unsuspend_clicked)
self._widget.abort_button.clicked[bool].connect(self.__handle_abort_clicked)
self._service_thread = Thread(target=self.wait_for_services, args=())
self._service_thread.start()
self._status_sub = rospy.Subscriber('packml/status', Status, self.status_callback)
self.threadpool = QThreadPool()
def disable_all_buttons(self):
self._widget.clear_button.setEnabled(False)
self._widget.reset_button.setEnabled(False)
self._widget.start_button.setEnabled(False)
self._widget.stop_button.setEnabled(False)
self._widget.hold_button.setEnabled(False)
self._widget.suspend_button.setEnabled(False)
self._widget.unhold_button.setEnabled(False)
self._widget.unsuspend_button.setEnabled(False)
self._widget.abort_button.setEnabled(False)
def set_message_text(self, text):
self._widget.message_box.setText("Message: " + text)
def status_callback(self, msg):
self.update_button_states(msg.state.val)
self.update_status_fields(msg)
def update_button_states(self, state):
self.disable_all_buttons()
if state == State.ABORTED:
self._widget.clear_button.setEnabled(True)
elif state == State.STOPPED:
self._widget.reset_button.setEnabled(True)
elif state == State.IDLE:
self._widget.start_button.setEnabled(True)
elif state == State.EXECUTE:
self._widget.hold_button.setEnabled(True)
self._widget.suspend_button.setEnabled(True)
elif state == State.HELD:
self._widget.unhold_button.setEnabled(True)
elif state == State.SUSPENDED:
self._widget.unsuspend_button.setEnabled(True)
elif state == State.COMPLETE:
self._widget.reset_button.setEnabled(True)
if state != State.STOPPED and \
state != State.STOPPING and \
state != State.ABORTED and \
state != State.ABORTING and \
state != State.CLEARING:
self._widget.stop_button.setEnabled(True)
if state != State.ABORTED and \
state != State.ABORTING:
self._widget.abort_button.setEnabled(True)
def update_status_fields(self, msg):
self.update_state_field(msg.state.val)
self._widget.substate.setText(str(msg.sub_state))
self.update_mode_field(msg.mode.val)
self._widget.error_code.setText(str(msg.error))
self._widget.suberror_code.setText(str(msg.sub_error))
def update_state_field(self, state):
if state == State.UNDEFINED:
self._widget.state_name.setText("UNDEFINED")
elif state == State.OFF:
self._widget.state_name.setText("OFF")
elif state == State.STOPPED:
self._widget.state_name.setText("STOPPED")
elif state == State.STARTING:
self._widget.state_name.setText("STARTING")
elif state == State.IDLE:
self._widget.state_name.setText("IDLE")
elif state == State.SUSPENDED:
self._widget.state_name.setText("SUSPENDED")
elif state == State.EXECUTE:
self._widget.state_name.setText("EXECUTE")
elif state == State.STOPPING:
self._widget.state_name.setText("STOPPING")
elif state == State.ABORTING:
self._widget.state_name.setText("ABORTING")
elif state == State.ABORTED:
self._widget.state_name.setText("ABORTED")
elif state == State.HOLDING:
self._widget.state_name.setText("HOLDING")
elif state == State.HELD:
self._widget.state_name.setText("HELD")
elif state == State.RESETTING:
self._widget.state_name.setText("RESETTING")
elif state == State.SUSPENDING:
self._widget.state_name.setText("SUSPENDING")
elif state == State.UNSUSPENDING:
self._widget.state_name.setText("UNSUSPENDING")
elif state == State.CLEARING:
self._widget.state_name.setText("CLEARING")
elif state == State.UNHOLDING:
self._widget.state_name.setText("UNHOLDING")
elif state == State.COMPLETING:
self._widget.state_name.setText("COMPLETING")
elif state == State.COMPLETE:
self._widget.state_name.setText("COMPLETE")
else:
self._widget.state_name.setTest("UNKNOWN")
def update_mode_field(self, mode):
if mode == Mode.UNDEFINED:
self._widget.mode_name.setText("UNDEFINED")
elif mode == Mode.AUTOMATIC:
self._widget.mode_name.setText("AUTOMATIC")
elif mode == Mode.SEMI_AUTOMATIC:
self._widget.mode_name.setText("SEMI-AUTOMATIC")
elif mode == Mode.MANUAL:
self._widget.mode_name.setText("MANUAL")
elif mode == Mode.IDLE:
self._widget.mode_name.setText("IDLE")
elif mode == Mode.SETUP:
self._widget.mode_name.setText("SETUP")
else:
self._widget.mode_name.setText("UNKNOWN")
def wait_for_services(self):
self._widget.setEnabled(False)
transition_service_name = 'packml/transition'
rospy.wait_for_service(transition_service_name, 30)
self.transition_service = rospy.ServiceProxy(transition_service_name, Transition)
self._widget.setEnabled(True)
def shutdown_plugin(self):
self._status_sub.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 handle_click_thread(self, request):
try:
service_thread = WorkerThread(self.transition_service, request, self.set_message_text)
if self.threadpool.activeThreadCount() >= 1:
return
else:
self.threadpool.start(service_thread)
except rospy.ServiceException as exc:
rospy.logerror("Service did not process request: " + str(exc))
def __handle_start_clicked(self, checked):
rospy.loginfo("Start button press")
self.handle_click_thread(TransitionRequest.START)
def __handle_stop_clicked(self, checked):
rospy.loginfo("Stop button press")
self.handle_click_thread(TransitionRequest.STOP)
def __handle_reset_clicked(self, checked):
rospy.loginfo("Reset button press")
self.handle_click_thread(TransitionRequest.RESET)
def __handle_clear_clicked(self, checked):
rospy.loginfo("Clear button press")
self.handle_click_thread(TransitionRequest.CLEAR)
def __handle_hold_clicked(self, checked):
rospy.loginfo("Hold button press")
self.handle_click_thread(TransitionRequest.HOLD)
def __handle_unhold_clicked(self, checked):
rospy.loginfo("Unhold button press")
self.handle_click_thread(TransitionRequest.UNHOLD)
def __handle_suspend_clicked(self, checked):
rospy.loginfo("Suspend button press")
self.handle_click_thread(TransitionRequest.SUSPEND)
def __handle_unsuspend_clicked(self, checked):
rospy.loginfo("Unsuspend button press")
self.handle_click_thread(TransitionRequest.UNSUSPEND)
def __handle_abort_clicked(self, checked):
rospy.loginfo("Abort button press")
self.handle_click_thread(TransitionRequest.ABORT)
@staticmethod
def add_arguments(parser):
rospy.loginfo("Add arguments callback")
group = parser.add_argument_group('Options for PackML plugin')
group.add_argument('--arg1', action='store_true', help='arg1 help')
class SchunkPlugin(Plugin):
def __init__(self, context):
super(SchunkPlugin, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('SchunkPlugin')
# 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('schunk_ui'),
'resource', 'ui', 'SchunkJoints.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('SchunkPluginUI')
# 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)
# Connect to ROS
# action clients
self.action_client = actionlib.SimpleActionClient(
'/gripper/sdh_controller/follow_joint_trajectory',
FollowJointTrajectoryAction)
# subscribers
self.sub_temp = rospy.Subscriber("/gripper/sdh_controller/temperature",
TemperatureArray, self.on_temp)
self.sub_tactile = rospy.Subscriber("/gripper/sdh_controller/pressure",
PressureArrayList, self.on_tactile)
# maximum measurable pressure
# 4096 * calib_pressure / calib_voltage
# 2^12 * 0.000473 / 592.1 = 0.0032720959297415976 (unit: N/(mm*mm) )
self.max_pressure = 2**12 * 0.000473 / 592.1
self.max_pressure_readings = 0
# Connect to UI
# service buttons
self._widget.button_init.clicked.connect(
lambda: self.call_service("init"))
self._widget.button_disconnect.clicked.connect(
lambda: self.call_service("shutdown"))
self._widget.button_estop.clicked.connect(
lambda: self.call_service("emergency_stop"))
self._widget.button_motor_on.clicked.connect(
lambda: self.call_service("motor_on"))
self._widget.button_motor_off.clicked.connect(
lambda: self.call_service("motor_off"))
self._widget.button_reset_max_pressure.clicked.connect(
lambda: self.reset_max_pressure_readings())
# status text
self.status_message = self._widget.status_message
# joint sliders
self._widget.proximal_slider.valueChanged.connect(
lambda value: self.on_slider_update(self._widget.proximal_spinbox,
value))
self._widget.distal_slider.valueChanged.connect(
lambda value: self.on_slider_update(self._widget.distal_spinbox,
value))
# joint spinners
self._widget.proximal_spinbox.valueChanged.connect(
lambda value: self.on_spinner_update(self._widget.proximal_slider,
value))
self._widget.distal_spinbox.valueChanged.connect(
lambda value: self.on_spinner_update(self._widget.distal_slider,
value))
# set spinner boxes by default sliders values
self._widget.proximal_spinbox.setValue(
self._widget.proximal_slider.value() / 1000.0)
self._widget.distal_spinbox.setValue(
self._widget.distal_slider.value() / 1000.0)
# map temperature names to spinner boxes
self.tempspinners = dict()
self.tempspinners["root"] = self._widget.spin_root
self.tempspinners["controller"] = self._widget.spin_ctrl
self.tempspinners["pcb"] = self._widget.spin_pcb
self.is_initialised = False
self.is_motor_on = False
self.has_new_data = False
# start working thread
self.running = True
self.thread = threading.Thread(target=self.loop, args=())
self.thread.start()
def on_slider_update(self, spinner, value):
# just set spinner value, do not forward signal back to slider
spinner.blockSignals(True)
spinner.setValue(value / 1000.0)
spinner.blockSignals(False)
self.has_new_data = True
def on_spinner_update(self, slider, value):
# just set slider value, do not forward signal back to spinner
slider.blockSignals(True)
slider.setValue(value * 1000)
slider.blockSignals(False)
self.has_new_data = True
def call_service(self, name):
service_name = '/gripper/sdh_controller/' + name
try:
rospy.wait_for_service(service_name, timeout=0.5)
except rospy.exceptions.ROSException:
rospy.logerr("service '" + str(name) + "' is not available")
self.status_message.setText("service '" + str(name) +
"' is not available")
return False
service = rospy.ServiceProxy(service_name, Trigger)
resp = service()
print("Called service:", name)
print("Response:")
print(resp)
self.status_message.setText(resp.message)
if name == "init":
self.is_initialised = resp.success
self.is_motor_on = resp.success
elif name == "shutdown":
self.is_initialised = not resp.success
self.is_motor_on = not resp.success
if name == "motor_on":
self.is_motor_on = resp.success
elif name == "motor_off":
self.is_motor_on = not resp.success
if resp.success and (name in ["init", "motor_on"]):
self.has_new_data = True
return resp.success
def loop(self):
self.running = True
while self.running:
if self.is_initialised and self.is_motor_on and self.has_new_data:
self.send_grasp_joint_positions()
self.has_new_data = False
time.sleep(0.1)
def send_grasp_joint_positions(self):
# values in range 0 ... 1
proximal_value = self._widget.proximal_spinbox.value()
distal_value = self._widget.distal_spinbox.value()
# define sets of joints
proximal_joints = [
"sdh_thumb_2_joint", "sdh_finger_12_joint", "sdh_finger_22_joint"
]
distal_joints = [
"sdh_thumb_3_joint", "sdh_finger_13_joint", "sdh_finger_23_joint"
]
static_joints = ["sdh_knuckle_joint"]
all_joints = static_joints + proximal_joints + distal_joints
# map joint ranges from [0..1] to individual set ranges
# proximal range: [-pi/2 .. 0]
# distal range: [0 .. pi/2]
proximal_range = [-math.pi / 2.0, 0.0]
distal_range = [0.0, math.pi / 2.0]
proximal_jpos = proximal_range[0] + proximal_value * (
proximal_range[1] - proximal_range[0])
distal_jpos = distal_range[0] + distal_value * (distal_range[1] -
distal_range[0])
trajectory_goal = FollowJointTrajectoryGoal()
# add single single joint point to trajectory
trajectory_goal.trajectory.points.append(JointTrajectoryPoint())
for jname in all_joints:
trajectory_goal.trajectory.joint_names.append(jname)
# select joint position from set
if jname in static_joints:
trajectory_goal.trajectory.points[0].positions.append(0)
elif jname in proximal_joints:
trajectory_goal.trajectory.points[0].positions.append(
proximal_jpos)
elif jname in distal_joints:
trajectory_goal.trajectory.points[0].positions.append(
distal_jpos)
else:
raise Exception("joint not in set")
# send trajectory and wait for response
self.action_client.send_goal(trajectory_goal)
if self.action_client.wait_for_result(timeout=rospy.Duration(5.0)):
trajectory_result = self.action_client.get_result()
print("set joints to " +
('%s' % trajectory_goal.trajectory.points[0].positions))
self.status_message.setText("set joints")
return trajectory_result.error_code == FollowJointTrajectoryResult.SUCCESSFUL
else:
rospy.logerr("timeout while waiting for response from grasp goal")
self.status_message.setText(
"timeout while waiting for response from grasp goal")
return False
def on_temp(self, temp_msg):
if self.is_initialised:
temps = dict(zip(temp_msg.name, temp_msg.temperature))
for name, spinner in self.tempspinners.iteritems():
try:
spinner.setValue(temps[name])
except KeyError:
rospy.logerr("temperature", name,
"is not provided by SDH driver node")
@staticmethod
def jet(m):
# clip values to range [0,1]
m = np.clip(m, 0.0, 1.0)
r = np.clip(np.minimum(4 * m - 1.5, -4 * m + 4.5), 0.0, 1.0)
g = np.clip(np.minimum(4 * m - 0.5, -4 * m + 3.5), 0.0, 1.0)
b = np.clip(np.minimum(4 * m + 0.5, -4 * m + 2.5), 0.0, 1.0)
return (np.dstack((r, g, b)) * 255).astype(np.uint8)
def on_tactile(self, msg_tactile):
for m in msg_tactile.pressure_list:
p = np.array(m.pressure, dtype=np.float64).reshape(
(m.cells_y, m.cells_x))
# apply colour map to scaled values
im = SchunkPlugin.jet(p / self.max_pressure)
getattr(self._widget,
"lbl_" + m.sensor_name).setText(m.sensor_name + ":")
getattr(self._widget,
"max_" + m.sensor_name).setText("{:.9f}".format(np.max(p)))
if np.max(p) > self.max_pressure_readings:
self.max_pressure_readings = np.max(p)
self._widget.max_pressure.setText(
str(self.max_pressure_readings))
# label for tactile content
lbl = getattr(self._widget, "tactile_" + m.sensor_name)
qimg = QImage(im.data, im.shape[1], im.shape[0], 3 * im.shape[1],
QImage.Format_RGB888)
T = QTransform()
T.rotate(90)
if self._widget.checkBox_show_img.isChecked():
qpix = QPixmap.fromImage(qimg).transformed(T)
qpix = qpix.scaledToWidth(qpix.width() * 20)
lbl.setPixmap(qpix)
else:
lbl.setText("image placeholder")
def reset_max_pressure_readings(self):
self.max_pressure_readings = 0
self._widget.max_pressure.setText(str(self.max_pressure_readings))
def shutdown_plugin(self):
# TODO unregister all publishers here
self.sub_temp.unregister()
self.sub_tactile.unregister()
self.action_client.cancel_all_goals()
self.running = False
self.thread.join()
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 Planner(Plugin):
def __init__(self, context):
super(Planner, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('ContrailPlanner')
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_contrail_planner'), 'resource', 'ContrailPlanner.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('ContrailPlannerUi')
# 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)
# Interface Functionality
# Plot Tabs
self._widget.tabgroup_plots.currentChanged.connect(self.draw_focused_plot)
# Management
self._widget.button_load.clicked.connect(self.button_load_pressed)
self._widget.button_save.clicked.connect(self.button_save_pressed)
self._widget.button_save_as.clicked.connect(self.button_save_as_pressed)
self._widget.button_reset.clicked.connect(self.button_reset_pressed)
self._widget.button_yaw_smooth.clicked.connect(self.button_yaw_smooth_pressed)
# Flight Plan
self._widget.combobox_mode.currentIndexChanged.connect(self.mode_changed)
self._widget.input_duration.textEdited.connect(self.set_flight_plan_duration)
self._widget.input_nom_vel.textEdited.connect(self.set_flight_plan_nom_vel)
self._widget.input_nom_rate.textEdited.connect(self.set_flight_plan_nom_rate)
# Waypoint
self._widget.table_waypoints.currentItemChanged.connect(self.list_item_changed)
#self._widget.table_waypoints.itemChanged.connect(self.waypoint_item_changed)
self._widget.input_x.returnPressed.connect(self.button_overwrite_pressed)
self._widget.input_y.returnPressed.connect(self.button_overwrite_pressed)
self._widget.input_z.returnPressed.connect(self.button_overwrite_pressed)
self._widget.input_psi.returnPressed.connect(self.button_overwrite_pressed)
self._widget.button_insert.clicked.connect(self.button_insert_pressed)
self._widget.button_append.clicked.connect(self.button_append_pressed)
self._widget.button_overwrite.clicked.connect(self.button_overwrite_pressed)
self._widget.button_move_up.clicked.connect(self.button_move_up_pressed)
self._widget.button_move_down.clicked.connect(self.button_move_down_pressed)
self._widget.button_remove.clicked.connect(self.button_remove_pressed)
# Class Variales
self.loaded_movement = Movement()
self.num_interp = 25
#Create plot figures
self.plot_3d_figure = Figure()
self.plot_3d_figure.patch.set_facecolor('white')
self.plot_3d_canvas = FigureCanvas(self.plot_3d_figure)
self.plot_3d_toolbar = NavigationToolbar(self.plot_3d_canvas, self._widget.widget_plot_3d)
self.plot_3d_ax = self.plot_3d_figure.add_subplot(1,1,1, projection='3d')
self.set_plot_layout(self._widget.widget_plot_3d, self.plot_3d_toolbar, self.plot_3d_canvas)
self.plot_3d_ax.set_title('3D Projection')
self.plot_3d_ax.set_xlabel('Position (X)')
self.plot_3d_ax.set_ylabel('Position (Y)')
self.plot_3d_ax.set_zlabel('Position (Z)')
self.plot_3d_ax.set_aspect('equal', 'box')
self.plot_x_figure = Figure()
self.plot_x_figure.patch.set_facecolor('white')
self.plot_x_canvas = FigureCanvas(self.plot_x_figure)
self.plot_x_toolbar = NavigationToolbar(self.plot_x_canvas, self._widget.widget_plot_x)
(self.plot_x_ax_pos, self.plot_x_ax_vel, self.plot_x_ax_acc) = self.set_subplots(self.plot_x_figure, 'X', 'm')
self.set_plot_layout(self._widget.widget_plot_x, self.plot_x_toolbar, self.plot_x_canvas)
self.plot_y_figure = Figure()
self.plot_y_figure.patch.set_facecolor('white')
self.plot_y_canvas = FigureCanvas(self.plot_y_figure)
self.plot_y_toolbar = NavigationToolbar(self.plot_y_canvas, self._widget.widget_plot_y)
(self.plot_y_ax_pos, self.plot_y_ax_vel, self.plot_y_ax_acc) = self.set_subplots(self.plot_y_figure, 'Y', 'm')
self.set_plot_layout(self._widget.widget_plot_y, self.plot_y_toolbar, self.plot_y_canvas)
self.plot_z_figure = Figure()
self.plot_z_figure.patch.set_facecolor('white')
self.plot_z_canvas = FigureCanvas(self.plot_z_figure)
self.plot_z_toolbar = NavigationToolbar(self.plot_z_canvas, self._widget.widget_plot_z)
(self.plot_z_ax_pos, self.plot_z_ax_vel, self.plot_z_ax_acc) = self.set_subplots(self.plot_z_figure, 'Z', 'm')
self.set_plot_layout(self._widget.widget_plot_z, self.plot_z_toolbar, self.plot_z_canvas)
self.plot_psi_figure = Figure()
self.plot_psi_figure.patch.set_facecolor('white')
self.plot_psi_canvas = FigureCanvas(self.plot_psi_figure)
self.plot_psi_toolbar = NavigationToolbar(self.plot_psi_canvas, self._widget.widget_plot_psi)
(self.plot_psi_ax_pos, self.plot_psi_ax_vel, self.plot_psi_ax_acc) = self.set_subplots(self.plot_psi_figure, 'Yaw', 'rad')
self.set_plot_layout(self._widget.widget_plot_psi, self.plot_psi_toolbar, self.plot_psi_canvas)
self.reset_flight_plan()
def set_subplots(self, figure, title, unit):
ax_pos = figure.add_subplot(3,1,1)
ax_vel = figure.add_subplot(3,1,2)
ax_acc = figure.add_subplot(3,1,3)
ax_pos.grid(b=True)
ax_vel.grid(b=True)
ax_acc.grid(b=True)
ax_pos.set_title(title + ' Axis')
ax_pos.set_ylabel('Pos. (' + unit + ')')
ax_vel.set_ylabel('Vel. (' + unit + '/s)')
ax_acc.set_ylabel('Acc. (' + unit + '/s/s)')
ax_acc.set_xlabel('Time (s)')
return (ax_pos, ax_vel, ax_acc)
def set_plot_layout(self, widget, toolbar, canvas):
layout = QVBoxLayout()
layout.addWidget(toolbar)
layout.addWidget(canvas)
widget.setLayout(layout)
def shutdown_plugin(self):
pass
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value("num_interp", self.num_interp)
def restore_settings(self, plugin_settings, instance_settings):
try:
self.num_interp = int( instance_settings.value("num_interp") )
except (AttributeError,TypeError) as e:
self.num_interp = 25
#def trigger_configuration(self):
#TODO: allow user to set 'self.num_interp'
def button_save_pressed(self):
if not self.loaded_movement.filename:
self.button_save_as_pressed()
if self.loaded_movement.filename:
self.set_flight_plan()
yaml_move = self.loaded_movement.encode_yaml()
with open(self.loaded_movement.filename, 'w') as outfile:
yaml.dump(yaml_move, outfile,default_flow_style = False)
rospy.loginfo("Movements saved: %s" % self.loaded_movement.filename)
else:
rospy.logerr("Can't save, no filename set")
def button_save_as_pressed(self):
(name,filt) = QFileDialog.getSaveFileName(caption='Save Movement',filter="YAML (*.yaml)")
self.loaded_movement.filename = name
if self.loaded_movement.filename:
self.button_save_pressed()
else:
rospy.logerr("No filename specified")
def button_load_pressed(self):
(name,filt) = QFileDialog.getOpenFileName(caption='Open Movement')
if name:
stream = open(name, 'r')
try:
with stream:
self.loaded_movement = Movement(filename=name, movement=yaml.safe_load(stream))
except yaml.YAMLError as e:
rospy.logerr(e)
self.update_display()
def button_reset_pressed(self):
self.reset_flight_plan()
def make_yaw_continuous(self, psi_c, psi_p):
while math.fabs(psi_c - psi_p) > math.pi:
if psi_c > psi_p:
psi_c -= 2*math.pi
else:
psi_c += 2*math.pi
return psi_c
def button_yaw_smooth_pressed(self):
for i in xrange(len(self.loaded_movement.waypoints)):
if i > 1:
self.loaded_movement.waypoints[i].yaw = self.make_yaw_continuous(self.loaded_movement.waypoints[i].yaw,self.loaded_movement.waypoints[i-1].yaw)
self.update_display()
def reset_flight_plan(self):
self.plot_3d_ax.view_init(azim=-135)
self.loaded_movement = Movement()
self.update_display()
def update_display(self):
self.clear_display()
self.update_flight_plan()
for i in xrange(len(self.loaded_movement.waypoints)):
self._widget.table_waypoints.insertRow(i)
item_x = QTableWidgetItem(str(self.loaded_movement.waypoints[i].x))
item_y = QTableWidgetItem(str(self.loaded_movement.waypoints[i].y))
item_z = QTableWidgetItem(str(self.loaded_movement.waypoints[i].z))
item_yaw = QTableWidgetItem(str(self.loaded_movement.waypoints[i].yaw))
item_x.setTextAlignment(QtCore.Qt.AlignVCenter | QtCore.Qt.AlignRight)
item_y.setTextAlignment(QtCore.Qt.AlignVCenter | QtCore.Qt.AlignRight)
item_z.setTextAlignment(QtCore.Qt.AlignVCenter | QtCore.Qt.AlignRight)
item_yaw.setTextAlignment(QtCore.Qt.AlignVCenter | QtCore.Qt.AlignRight)
self._widget.table_waypoints.setItem(i, 0, item_x)
self._widget.table_waypoints.setItem(i, 1, item_y)
self._widget.table_waypoints.setItem(i, 2, item_z)
self._widget.table_waypoints.setItem(i, 3, item_yaw)
self.update_plot()
def quiver_dir_from_yaw(self,psi):
n = len(psi)
u = [0.0]*n
v = [0.0]*n
w = [0.0]*n
for i in xrange(n):
u[i] = math.cos(psi[i])
v[i] = math.sin(psi[i])
#w[i] = 0.0
return (u,v,w)
def update_plot(self):
print("plot update")
self.clear_plot()
if len(self.loaded_movement.waypoints):
plot_data = [[],[],[],[]]
for i in xrange(len(self.loaded_movement.waypoints)):
plot_data[0].append(self.loaded_movement.waypoints[i].x)
plot_data[1].append(self.loaded_movement.waypoints[i].y)
plot_data[2].append(self.loaded_movement.waypoints[i].z)
plot_data[3].append(self.loaded_movement.waypoints[i].yaw)
d = float(self.loaded_movement.duration)
d2 = d*d
# Plot data
n = len(plot_data[0])
ni = n
t = [0.0]*n
x = plot_data[0]
xd = [0.0]*n
xdd = [0.0]*n
y = plot_data[1]
yd = [0.0]*n
ydd = [0.0]*n
z = plot_data[2]
zd = [0.0]*n
zdd = [0.0]*n
psi = plot_data[3]
psid = [0.0]*n
psidd = [0.0]*n
ti = [0.0]*ni
xi = x
xdi = [0.0]*ni
xddi = [0.0]*ni
yi = y
ydi = [0.0]*ni
yddi = [0.0]*ni
zi = z
zdi = [0.0]*ni
zddi = [0.0]*ni
psii = psi
psidi = [0.0]*ni
psiddi = [0.0]*ni
if (n > 1) and (d > 0.0) and not self.loaded_movement.is_discrete:
dt = d / (n-1)
t = [dt * i for i in range(n)]
# Plot spline display if in contiuous mode
iqs_x = InterpolatedQuinticSpline()
iqs_y = InterpolatedQuinticSpline()
iqs_z = InterpolatedQuinticSpline()
iqs_psi = InterpolatedQuinticSpline()
if iqs_x.interpolate(x) and iqs_y.interpolate(y) and iqs_z.interpolate(z) and iqs_psi.interpolate(psi):
xd = [i/d for i in iqs_x.get_dvias()]
xdd = [i/d2 for i in iqs_x.get_ddvias()]
yd = [i/d for i in iqs_y.get_dvias()]
ydd = [i/d2 for i in iqs_y.get_ddvias()]
zd = [i/d for i in iqs_z.get_dvias()]
zdd = [i/d2 for i in iqs_z.get_ddvias()]
psid = [i/d for i in iqs_psi.get_dvias()]
psidd = [i/d2 for i in iqs_psi.get_ddvias()]
ni = n*self.num_interp
ui = [float(i) / (ni-1) for i in range(ni)]
xvi = [iqs_x.lookup(u) for u in ui]
yvi = [iqs_y.lookup(u) for u in ui]
zvi = [iqs_z.lookup(u) for u in ui]
psivi = [iqs_psi.lookup(u) for u in ui]
xi = [i[0] for i in xvi]
xdi = [i[1]/d for i in xvi]
xddi = [i[2]/d2 for i in xvi]
yi = [i[0] for i in yvi]
ydi = [i[1]/d for i in yvi]
yddi = [i[2]/d2 for i in yvi]
zi = [i[0] for i in zvi]
zdi = [i[1]/d for i in zvi]
zddi = [i[2]/d2 for i in zvi]
psii = [i[0] for i in psivi]
psidi = [i[1]/d for i in psivi]
psiddi = [i[2]/d2 for i in psivi]
dti = d / (ni-1)
ti = [dti * i for i in range(ni)]
else:
rospy.logerr("Could not interpolate spline!")
self.plot_3d_ax.plot(x, y, z, 'b--')
self.plot_3d_ax.plot(xi, yi, zi, 'g-')
self.plot_x_ax_pos.plot(ti, xi, 'g-')
self.plot_x_ax_vel.plot(ti, xdi, 'g-')
self.plot_x_ax_acc.plot(ti, xddi, 'g-')
self.plot_y_ax_pos.plot(ti, yi, 'g-')
self.plot_y_ax_vel.plot(ti, ydi, 'g-')
self.plot_y_ax_acc.plot(ti, yddi, 'g-')
self.plot_z_ax_pos.plot(ti, zi, 'g-')
self.plot_z_ax_vel.plot(ti, zdi, 'g-')
self.plot_z_ax_acc.plot(ti, zddi, 'g-')
self.plot_psi_ax_pos.plot(ti, psii, 'g-')
self.plot_psi_ax_vel.plot(ti, psidi, 'g-')
self.plot_psi_ax_acc.plot(ti, psiddi, 'g-')
# Visual waypoint data
elif self.loaded_movement.is_discrete:
# Generate fake t data for some form of display
t = xrange(len(x))
self.plot_3d_ax.plot(x, y, z, 'g-')
self.plot_x_ax_pos.plot(t, x, 'g-')
self.plot_x_ax_vel.plot(t, xd, 'g-')
self.plot_x_ax_acc.plot(t, xdd, 'g-')
self.plot_y_ax_pos.plot(t, y, 'g-')
self.plot_y_ax_vel.plot(t, yd, 'g-')
self.plot_y_ax_acc.plot(t, ydd, 'g-')
self.plot_z_ax_pos.plot(t, z, 'g-')
self.plot_z_ax_vel.plot(t, zd, 'g-')
self.plot_z_ax_acc.plot(t, zdd, 'g-')
self.plot_psi_ax_pos.plot(t, psi, 'g-')
self.plot_psi_ax_vel.plot(t, psid, 'g-')
self.plot_psi_ax_acc.plot(t, psidd, 'g-')
self.plot_3d_ax.plot(x, y, z, 'bo')
(qu,qv,qw) = self.quiver_dir_from_yaw(psi)
self.plot_3d_ax.quiver(x, y, z, qu, qv, qw, length=0.25, pivot='tail')#, cmap='Reds', lw=2)
self.plot_x_ax_pos.plot(t, x, 'bo')
self.plot_x_ax_vel.plot(t, xd, 'bo')
self.plot_x_ax_acc.plot(t, xdd, 'bo')
self.plot_y_ax_pos.plot(t, y, 'bo')
self.plot_y_ax_vel.plot(t, yd, 'bo')
self.plot_y_ax_acc.plot(t, ydd, 'bo')
self.plot_z_ax_pos.plot(t, z, 'bo')
self.plot_z_ax_vel.plot(t, zd, 'bo')
self.plot_z_ax_acc.plot(t, zdd, 'bo')
self.plot_psi_ax_pos.plot(t, psi, 'bo')
self.plot_psi_ax_vel.plot(t, psid, 'bo')
self.plot_psi_ax_acc.plot(t, psidd, 'bo')
sel_ind = self._widget.table_waypoints.currentRow()
if sel_ind >= 0:
self.plot_3d_ax.plot([x[sel_ind]], [y[sel_ind]], [z[sel_ind]], 'ro')
self.plot_3d_ax.quiver([x[sel_ind]], [y[sel_ind]], [z[sel_ind]], [qu[sel_ind]], [qv[sel_ind]], [qw[sel_ind]], length=0.25, pivot='tail', colors=[[1.0,0.0,0.0]], lw=3)
self.plot_x_ax_pos.plot([t[sel_ind]], [x[sel_ind]], 'ro')
self.plot_x_ax_vel.plot([t[sel_ind]], [xd[sel_ind]], 'ro')
self.plot_x_ax_acc.plot([t[sel_ind]], [xdd[sel_ind]], 'ro')
self.plot_y_ax_pos.plot([t[sel_ind]], [y[sel_ind]], 'ro')
self.plot_y_ax_vel.plot([t[sel_ind]], [yd[sel_ind]], 'ro')
self.plot_y_ax_acc.plot([t[sel_ind]], [ydd[sel_ind]], 'ro')
self.plot_z_ax_pos.plot([t[sel_ind]], [z[sel_ind]], 'ro')
self.plot_z_ax_vel.plot([t[sel_ind]], [zd[sel_ind]], 'ro')
self.plot_z_ax_acc.plot([t[sel_ind]], [zdd[sel_ind]], 'ro')
self.plot_psi_ax_pos.plot([t[sel_ind]], [psi[sel_ind]], 'ro')
self.plot_psi_ax_vel.plot([t[sel_ind]], [psid[sel_ind]], 'ro')
self.plot_psi_ax_acc.plot([t[sel_ind]], [psidd[sel_ind]], 'ro')
# Calculate nice limits for 3D
minx = min([min(x),min(xi)])
miny = min([min(y),min(yi)])
minz = min([min(z),min(zi)])
maxx = max([max(x),max(xi)])
maxy = max([max(y),max(yi)])
maxz = max([max(z),max(zi)])
max_range = 1.25*max([maxx-minx, maxy-miny, maxz-minz]) / 2.0
mid_x = (maxx+minx) / 2.0
mid_y = (maxy+miny) / 2.0
mid_z = (maxz+minz) / 2.0
if max_range:
self.plot_3d_ax.set_xlim(mid_x - max_range, mid_x + max_range)
self.plot_3d_ax.set_ylim(mid_y - max_range, mid_y + max_range)
self.plot_3d_ax.set_zlim(mid_z - max_range, mid_z + max_range)
self.set_nice_limits_2d(self.plot_x_ax_pos, 0, t[-1], min([min(x),min(xi)]), max([max(x),max(xi)]))
self.set_nice_limits_2d(self.plot_x_ax_vel, 0, t[-1], min([min(xd),min(xdi)]), max([max(xd),max(xdi)]))
self.set_nice_limits_2d(self.plot_x_ax_acc, 0, t[-1], min([min(xdd),min(xddi)]), max([max(xdd),max(xddi)]))
self.set_nice_limits_2d(self.plot_y_ax_pos, 0, t[-1], min([min(y),min(yi)]), max([max(y),max(yi)]))
self.set_nice_limits_2d(self.plot_y_ax_vel, 0, t[-1], min([min(yd),min(ydi)]), max([max(yd),max(ydi)]))
self.set_nice_limits_2d(self.plot_y_ax_acc, 0, t[-1], min([min(ydd),min(yddi)]), max([max(ydd),max(yddi)]))
self.set_nice_limits_2d(self.plot_z_ax_pos, 0, t[-1], min([min(z),min(zi)]), max([max(z),max(zi)]))
self.set_nice_limits_2d(self.plot_z_ax_vel, 0, t[-1], min([min(zd),min(zdi)]), max([max(zd),max(zdi)]))
self.set_nice_limits_2d(self.plot_z_ax_acc, 0, t[-1], min([min(zdd),min(zddi)]), max([max(zdd),max(zddi)]))
self.set_nice_limits_2d(self.plot_psi_ax_pos, 0, t[-1], min([min(psi),min(psii)]), max([max(psi),max(psii)]))
self.set_nice_limits_2d(self.plot_psi_ax_vel, 0, t[-1], min([min(psid),min(psidi)]), max([max(psid),max(psidi)]))
self.set_nice_limits_2d(self.plot_psi_ax_acc, 0, t[-1], min([min(psidd),min(psiddi)]), max([max(psidd),max(psiddi)]))
# Refresh current canvas
self.draw_focused_plot(self._widget.tabgroup_plots.currentIndex())
def set_nice_limits_2d(self, ax, x_min, x_max, y_min, y_max):
mr = 1.25*(y_max-y_min) / 2.0
y_mid = (y_max+y_min) / 2.0
ax.set_xlim(x_min, x_max)
ax.set_ylim(y_mid - mr, y_mid + mr)
def draw_focused_plot(self,index):
if(index == 0):
self.plot_3d_canvas.draw()
elif(index == 1):
self.plot_x_ax_pos.relim()
self.plot_x_ax_vel.relim()
self.plot_x_ax_acc.relim()
self.plot_x_canvas.draw()
elif(index == 2):
self.plot_y_ax_pos.relim()
self.plot_y_ax_vel.relim()
self.plot_y_ax_acc.relim()
self.plot_y_canvas.draw()
elif(index == 3):
self.plot_z_ax_pos.relim()
self.plot_z_ax_vel.relim()
self.plot_z_ax_acc.relim()
self.plot_z_canvas.draw()
elif(index == 4):
self.plot_psi_ax_pos.relim()
self.plot_psi_ax_vel.relim()
self.plot_psi_ax_acc.relim()
self.plot_psi_canvas.draw()
def clear_plot(self):
# Discards the old graph data
artists = self.plot_3d_ax.lines + self.plot_3d_ax.collections \
+ self.plot_x_ax_pos.lines + self.plot_x_ax_pos.collections \
+ self.plot_x_ax_vel.lines + self.plot_x_ax_vel.collections \
+ self.plot_x_ax_acc.lines + self.plot_x_ax_acc.collections \
+ self.plot_y_ax_pos.lines + self.plot_y_ax_pos.collections \
+ self.plot_y_ax_vel.lines + self.plot_y_ax_vel.collections \
+ self.plot_y_ax_acc.lines + self.plot_y_ax_acc.collections \
+ self.plot_z_ax_pos.lines + self.plot_z_ax_pos.collections \
+ self.plot_z_ax_vel.lines + self.plot_z_ax_vel.collections \
+ self.plot_z_ax_acc.lines + self.plot_z_ax_acc.collections \
+ self.plot_psi_ax_pos.lines + self.plot_psi_ax_pos.collections \
+ self.plot_psi_ax_vel.lines + self.plot_psi_ax_vel.collections \
+ self.plot_psi_ax_acc.lines + self.plot_psi_ax_acc.collections
for artist in artists:
artist.remove()
def clear_inputs(self):
self._widget.input_x.setText("0.0")
self._widget.input_y.setText("0.0")
self._widget.input_z.setText("0.0")
self._widget.input_psi.setText("0.0")
def clear_display(self):
self._widget.table_waypoints.setRowCount(0)
self.clear_inputs()
self.clear_plot()
def mode_changed(self):
mode = self._widget.combobox_mode.currentText()
if mode == 'Continuous':
self.loaded_movement.is_discrete = False
self._widget.label_duration.setEnabled(True)
self._widget.input_duration.setEnabled(True)
self._widget.label_nom_vel.setEnabled(False)
self._widget.input_nom_vel.setEnabled(False)
self._widget.label_nom_rate.setEnabled(False)
self._widget.input_nom_rate.setEnabled(False)
else: # Discrete
self.loaded_movement.is_discrete = True
self._widget.label_duration.setEnabled(False)
self._widget.input_duration.setEnabled(False)
self._widget.label_nom_vel.setEnabled(True)
self._widget.input_nom_vel.setEnabled(True)
self._widget.label_nom_rate.setEnabled(True)
self._widget.input_nom_rate.setEnabled(True)
self.update_plot()
#def waypoint_item_changed(self, item):
# if item:
# ind = item.row()
# dind = item.column()
# data = float(item.text())
# if dind == 0:
# self.loaded_movement.waypoints[ind].x = data
# elif dind == 1:
# self.loaded_movement.waypoints[ind].y = data
# elif dind == 2:
# self.loaded_movement.waypoints[ind].z = data
# elif dind == 3:
# self.loaded_movement.waypoints[ind].yaw = data
#
# self.list_item_changed()
# self.update_plot()
def list_item_changed(self):
ind = self._widget.table_waypoints.currentRow()
if ind >= 0:
wp = self.loaded_movement.waypoints[ind]
self._widget.input_x.setText(str(wp.x))
self._widget.input_y.setText(str(wp.y))
self._widget.input_z.setText(str(wp.z))
self._widget.input_psi.setText(str(wp.yaw))
else:
self.clear_inputs()
self.update_plot()
def set_flight_plan(self):
mode = self._widget.combobox_mode.currentText()
if mode == 'Discrete':
self.loaded_movement.is_discrete = True
else:
self.loaded_movement.is_discrete = False
self.loaded_movement.duration = float(self._widget.input_duration.text())
self.loaded_movement.nom_vel = float(self._widget.input_nom_vel.text())
self.loaded_movement.nom_rate = float(self._widget.input_nom_rate.text())
def set_flight_plan_duration(self, text):
self.loaded_movement.duration = float(text)
self.update_plot()
def set_flight_plan_nom_vel(self, text):
self.loaded_movement.nom_vel = float(text)
self.update_plot()
def set_flight_plan_nom_rate(self, text):
self.loaded_movement.nom_rate = float(text)
self.update_plot()
def update_flight_plan(self):
self._widget.combobox_mode.currentText()
if self.loaded_movement.is_discrete:
self._widget.combobox_mode.setCurrentIndex(1)
else:
self._widget.combobox_mode.setCurrentIndex(0)
self._widget.input_duration.setText(str(self.loaded_movement.duration))
self._widget.input_nom_vel.setText(str(self.loaded_movement.nom_vel))
self._widget.input_nom_rate.setText(str(self.loaded_movement.nom_rate))
def prepare_waypoint(self):
return Waypoint(x=self._widget.input_x.text(),
y=self._widget.input_y.text(),
z=self._widget.input_z.text(),
yaw=self._widget.input_psi.text())
def button_insert_pressed(self):
ind = self._widget.table_waypoints.currentRow()
self.loaded_movement.waypoints.insert(ind, self.prepare_waypoint())
self.update_display()
self._widget.table_waypoints.selectRow(ind)
def button_append_pressed(self):
ind = self._widget.table_waypoints.currentRow() + 1
self.loaded_movement.waypoints.insert(ind, self.prepare_waypoint())
self.update_display()
self._widget.table_waypoints.selectRow(ind)
def button_overwrite_pressed(self):
ind = self._widget.table_waypoints.currentRow()
if (len(self.loaded_movement.waypoints) > ind) and (ind >= 0):
self.loaded_movement.waypoints[ind] = self.prepare_waypoint()
self.update_display()
self._widget.table_waypoints.selectRow(ind)
def button_move_up_pressed(self):
ind = self._widget.table_waypoints.currentRow()
if (len(self.loaded_movement.waypoints) > ind) and (ind > 0):
self.loaded_movement.waypoints.insert(ind-1, self.loaded_movement.waypoints.pop(ind))
self.update_display()
self._widget.table_waypoints.selectRow(ind-1)
def button_move_down_pressed(self):
ind = self._widget.table_waypoints.currentRow()
if (len(self.loaded_movement.waypoints)-1 > ind) and (ind >= 0):
self.loaded_movement.waypoints.insert(ind+1, self.loaded_movement.waypoints.pop(ind))
self.update_display()
self._widget.table_waypoints.selectRow(ind+1)
def button_remove_pressed(self):
ind = self._widget.table_waypoints.currentRow()
if (len(self.loaded_movement.waypoints) > ind) and (ind >= 0):
self.loaded_movement.waypoints.pop(ind)
self.update_display()
self._widget.table_waypoints.selectRow(ind-1)
class TrackAnnotationPlugin(Plugin):
def __init__(self, context):
super(TrackAnnotationPlugin, self).__init__(context)
self.setObjectName('TrackAnnotationPlugin')
self.shutdownRequested = False
# 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()
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'gui.ui')
loadUi(ui_file, self.widget)
# Give QObjects reasonable names
self.widget.setObjectName('TrackAnnotationPluginUi')
if context.serial_number() > 1:
self.widget.setWindowTitle(self.widget.windowTitle() + (' (%d)' % context.serial_number()))
# Setup backend components
self.setupBackend()
# Populate inputs
self.widget.genderCombo.addItem("")
self.widget.genderCombo.addItem(CategoricalAttribute.GENDER_MALE)
self.widget.genderCombo.addItem(CategoricalAttribute.GENDER_FEMALE)
self.widget.ageGroupCombo.addItem("")
self.widget.ageGroupCombo.addItem(CategoricalAttribute.AGE_GROUP_0_TO_2)
self.widget.ageGroupCombo.addItem(CategoricalAttribute.AGE_GROUP_3_TO_7)
self.widget.ageGroupCombo.addItem(CategoricalAttribute.AGE_GROUP_8_TO_12)
self.widget.ageGroupCombo.addItem(CategoricalAttribute.AGE_GROUP_13_TO_19)
self.widget.ageGroupCombo.addItem(CategoricalAttribute.AGE_GROUP_20_TO_36)
self.widget.ageGroupCombo.addItem(CategoricalAttribute.AGE_GROUP_37_TO_65)
self.widget.ageGroupCombo.addItem(CategoricalAttribute.AGE_GROUP_66_TO_99)
# Register event handlers
uiActions = UiActions(self.widget, self.database, self.editor, self.simulator, self.visualizer)
self.uiActions = uiActions
self.widget.loadDatabaseButton.clicked[bool].connect(uiActions.loadDatabase)
self.widget.saveDatabaseButton.clicked[bool].connect(uiActions.saveDatabase)
self.widget.saveDatabaseAsButton.clicked[bool].connect(uiActions.saveDatabaseAs)
self.widget.newTrackButton.clicked[bool].connect(uiActions.newTrack)
self.widget.previousTrackButton.clicked[bool].connect(uiActions.previousTrack)
self.widget.trackIdText.returnPressed.connect(uiActions.trackIdTextChanged)
self.widget.nextTrackButton.clicked[bool].connect(uiActions.nextTrack)
self.widget.deleteTrackButton.clicked[bool].connect(uiActions.deleteTrack)
self.widget.mergeTracksButton.clicked[bool].connect(uiActions.mergeTracks)
self.widget.waypointFrameCombo.editTextChanged[str].connect(uiActions.waypointFrameChanged)
self.widget.previousWaypointButton.clicked[bool].connect(uiActions.previousWaypoint)
self.widget.waypointIndexText.returnPressed.connect(uiActions.waypointIndexTextChanged)
self.widget.nextWaypointButton.clicked[bool].connect(uiActions.nextWaypoint)
self.widget.deleteWaypointButton.clicked[bool].connect(uiActions.deleteWaypoint)
self.widget.genderCombo.editTextChanged[str].connect(uiActions.genderChanged)
self.widget.ageGroupCombo.editTextChanged[str].connect(uiActions.ageGroupChanged)
self.widget.syncTracksWithDetectionsCheckbox.stateChanged[int].connect(uiActions.syncTracksWithDetectionsChanged)
self.widget.publishClockCheckbox.stateChanged[int].connect(uiActions.publishClockChanged)
self.widget.hideInactiveTrajectoriesCheckbox.stateChanged[int].connect(uiActions.hideInactiveTrajectoriesChanged)
self.widget.pauseCheckbox.stateChanged[int].connect(uiActions.pauseChanged)
self.widget.simulationLengthInput.valueChanged[float].connect(uiActions.simulationLengthChanged)
self.widget.simulationRateInput.valueChanged[float].connect(uiActions.simulationRateChanged)
# Set default values
self.widget.syncTracksWithDetectionsCheckbox.setCheckState(0)
self.widget.hideInactiveTrajectoriesCheckbox.setCheckState(0)
self.widget.waypointFrameCombo.setEditText("laser_front_link")
self.widget.publishClockCheckbox.setCheckState(0)
# Add widget to the user interface
context.add_widget(self.widget)
# Initialization done, start update thread
rospy.loginfo("Track annotation editor has finished loading!")
self.startUpdateThread()
# Load database if specified
databaseToLoad = str(rospy.get_param("~filename", ""))
if databaseToLoad:
uiActions.loadGivenDatabase(databaseToLoad)
def setupBackend(self):
# Create backend components
self.transformer = track_annotation_tool.Transformer()
self.database = track_annotation_tool.Database()
self.editor = track_annotation_tool.Editor(self.database, self.transformer)
self.simulator = track_annotation_tool.Simulator(self.database, self.editor, self.transformer)
self.visualizer = track_annotation_tool.Visualizer(self.database, self.editor, self.transformer)
# Create ROS subscribers for communication with RViz plugin
self.waypointSubscriber = WaypointSubscriber(self.editor)
# Start clock generator (must happen after all calls to rospy.get_param() due to thread synchronization issues)
self.simulator.clockGenerator.start()
def startUpdateThread(self):
# Start update loop
self.thread = threading.Thread(target=self.updateLoop)
self.thread.start()
def updateLoop(self):
# Run simulation
lastTime = rospy.Time.now()
rate = track_annotation_tool.WallRate(30) # in hertz
while not self.shutdownRequested:
currentTime = rospy.Time.now()
if currentTime.to_sec() < lastTime.to_sec():
rospy.logwarn("*** TIME MOVED BACKWARDS! RESETTING TRACK POSITIONS! ***")
lastTime = currentTime
self.simulator.update()
self.visualizer.update()
self.updateAvailableFrames()
try:
rate.sleep()
except rospy.exceptions.ROSInterruptException as e:
rospy.logwarn(e)
def updateAvailableFrames(self):
selectedFrame = self.editor.activeFrameId
existingFrames = [self.widget.waypointFrameCombo.itemText(i) for i in range(self.widget.waypointFrameCombo.count())]
for frameId in sorted(self.simulator.transformer.tfListener.getFrameStrings()):
if not frameId in existingFrames:
self.widget.waypointFrameCombo.addItem(frameId)
#rospy.loginfo("New TF frame available: " + frameId)
self.widget.waypointFrameCombo.setEditText(selectedFrame)
def shutdown_plugin(self):
self.shutdownRequested = True
if self.database.hasUnsavedChanges:
reply = QMessageBox.warning(self.widget, "Close track annotation tool", "You have unsaved changes. Do you want to save these changes before closing?", QMessageBox.Yes | QMessageBox.No)
if reply == QMessageBox.Yes:
self.uiActions.saveDatabase()
def save_settings(self, plugin_settings, instance_settings):
plugin_settings.set_value('default_waypoint_frame', self.editor.activeFrameId)
plugin_settings.set_value('hide_inactive_trajectories', self.visualizer.hideInactiveTrajectories)
def restore_settings(self, plugin_settings, instance_settings):
self.widget.hideInactiveTrajectoriesCheckbox.setCheckState(2 if plugin_settings.value('hide_inactive_trajectories') else 0)
defaultWaypointFrame = plugin_settings.value('default_waypoint_frame')
if defaultWaypointFrame:
self.widget.waypointFrameCombo.setEditText(defaultWaypointFrame)
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 Dashboard(Plugin):
autoStateSignal = QtCore.pyqtSignal(int)
nBallsSignal = QtCore.pyqtSignal(int)
shooterInRangeSignal = QtCore.pyqtSignal(bool)
turretInRangeSignal = QtCore.pyqtSignal(bool)
msg_data = "default"
def __init__(self, context):
#Start client -rosbridge
self.client = roslibpy.Ros(host='localhost', port=5803)
self.client.run()
super(Dashboard, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('Dashboard')
# 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_dashboard'), 'resource', 'Dashboard.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('DashboardUi')
# Set up signal-slot connections
self._widget.set_imu_angle_button.clicked.connect(self.setImuAngle)
self._widget.imu_angle.valueChanged.connect(self.imuAngleChanged)
self._widget.auto_wall_dist_button.clicked.connect(self.setAutoWallDist)
self._widget.auto_wall_dist.valueChanged.connect(self.autoWallDistChanged)
self._widget.ball_reset_button.clicked.connect(self.resetBallCount)
self._widget.ball_reset_count.valueChanged.connect(self.resetBallChanged)
# Add buttons for auto modes
v_layout = self._widget.auto_mode_v_layout #vertical layout storing the buttons
self.auto_mode_button_group = QButtonGroup(self._widget) # needs to be a member variable so the publisher can access it to see which auto mode was selected
# Search for auto_mode config items
for i in range(1,100): # loop will exit when can't find the next auto mode, so really only a while loop needed, but exiting at 100 will prevent infinite looping
if rospy.has_param("/auto/auto_mode_" + str(i)):
auto_sequence = rospy.get_param("/auto/auto_mode_" + str(i))
new_auto_mode = QWidget()
new_h_layout = QHBoxLayout()
new_h_layout.setContentsMargins(0,0,0,0)
new_button = QRadioButton("Mode " + str(i))
new_button.setStyleSheet("font-weight: bold")
self.auto_mode_button_group.addButton(new_button, i) #second arg is the button's id
new_h_layout.addWidget( new_button )
new_h_layout.addWidget( QLabel(", ".join(auto_sequence)) )
hSpacer = QSpacerItem(0, 0, QSizePolicy.Expanding, QSizePolicy.Minimum)
new_h_layout.addItem(hSpacer)
new_auto_mode.setLayout( new_h_layout )
v_layout.addWidget(new_auto_mode)
else:
print(str(i-1) + " auto modes found.")
# if no auto modes found, inform the user with a label
if (i-1) == 0:
v_layout.addWidget( QLabel("No auto modes found") )
break #break out of for loop searching for auto modes
# auto state stuff
self.autoState = 0
self.displayAutoState() #display initial auto state
# publish thread
publish_thread = Thread(target=self.publish_thread) #args=(self,))
publish_thread.start()
# number balls display
self.zero_balls = QPixmap(":/images/0_balls.png")
self.one_ball = QPixmap(":/images/1_ball.png")
self.two_balls = QPixmap(":/images/2_balls.png")
self.three_balls = QPixmap(":/images/3_balls.png")
self.four_balls = QPixmap(":/images/4_balls.png")
self.five_balls = QPixmap(":/images/5_balls.png")
self.more_than_five_balls = QPixmap(":/images/more_than_5_balls.png")
self.n_balls = -1 #don't know n balls at first
#in range stuff
self.shooter_in_range = False
self.turret_in_range = False
self.in_range_pixmap = QPixmap(":/images/GreenTarget.png")
self.not_in_range_pixmap = QPixmap(":/images/RedTarget.png")
self._widget.in_range_display.setPixmap(self.not_in_range_pixmap)
# 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 subscribers last, so that any callbacks they execute won't interfere with init
auto_state_listener = roslibpy.Topic(self.client, '/auto/auto_state', 'behavior_actions/AutoState')
self.auto_state_sub = auto_state_listener.subscribe(self.autoStateCallback)
n_balls_listener = roslibpy.Topic(self.client,'/num_powercells','std_msgs/UInt8')
self.n_balls_sub = n_balls_listener.subscribe(self.nBallsCallback)
shooter_in_range_listener = roslibpy.Topic(self.client, '/shooter/shooter_in_range', std_msgs.msg.Bool)
self.shooter_in_range_sub = shooter_in_range_listener.subscribe(self.shooterInRangeCallback)
turret_in_range_listener = roslibpy.Topic(self.client, '/align_to_shoot/turret_in_range', std_msgs.msg.Bool)
self.turret_in_range_sub = turret_in_range_listener.subscribe(self.turretInRangeCallback)
self.autoStateSignal.connect(self.autoStateSlot)
self.nBallsSignal.connect(self.nBallsSlot)
self.shooterInRangeSignal.connect(self.shooterInRangeSlot)
self.turretInRangeSignal.connect(self.turretInRangeSlot)
def autoStateCallback(self, msg):
self.autoStateSignal.emit(int(msg.id));
def autoStateSlot(self, state):
#self.lock.acquire()
if(self.autoState != state):
self.autoState = state
self.displayAutoState()
def displayAutoState(self):
if self.autoState == 0:
self._widget.auto_state_display.setText("Not ready")
self._widget.auto_state_display.setStyleSheet("background-color:#ff5555;")
elif self.autoState == 1:
self._widget.auto_state_display.setText("Ready, waiting for auto period")
self._widget.auto_state_display.setStyleSheet("background-color:#ffffff;")
elif self.autoState == 2:
self._widget.auto_state_display.setText("Running")
self._widget.auto_state_display.setStyleSheet("background-color:#ffff00")
elif self.autoState == 3:
self._widget.auto_state_display.setText("Finished")
self._widget.auto_state_display.setStyleSheet("background-color:#00ff00;")
elif self.autoState == 4:
self._widget.auto_state_display.setText("Error")
self._widget.auto_state_display.setStyleSheet("background-color:#ff5555;")
def nBallsCallback(self, msg):
self.nBallsSignal.emit(int(msg.data))
def nBallsSlot(self, state):
if(self.n_balls != state):
self.n_balls = state
display = self._widget.n_balls_display
if state == 0:
display.setPixmap(self.zero_balls)
elif state == 1:
display.setPixmap(self.one_ball)
elif state == 2:
display.setPixmap(self.two_balls)
elif state == 3:
display.setPixmap(self.three_balls)
elif state == 4:
display.setPixmap(self.four_balls)
elif state == 5:
display.setPixmap(self.five_balls)
elif state > 5:
display.setPixmap(self.more_than_five_balls)
else:
display.setText("Couldn't read # balls")
def shooterInRangeCallback(self, msg):
self.shooterInRangeSignal.emit(bool(msg.data))
def shooterInRangeSlot(self, in_range):
self.shooter_in_range = in_range #set here so that it's set synchronously with the other slots
def turretInRangeCallback(self, msg):
self.turretInRangeSignal.emit(bool(msg.data))
self.updateInRange()
def turretInRangeSlot(self, in_range):
self.turret_in_range = in_range #set here so that it's set synchronously with the other slots
self.updateInRange()
def updateInRange(self):
display = self._widget.in_range_display
if(self.shooter_in_range and self.turret_in_range):
display.setPixmap(self.in_range_pixmap)
else:
display.setPixmap(self.not_in_range_pixmap)
def setImuAngle(self):
print("setting imu")
#self.lock.acquire()
angle = self._widget.imu_angle.value() # imu_angle is the text field (doublespinbox) that the user can edit to change the navx angle, defaulting to zero
# call the service
try:
service = roslibpy.Service(self.client,'/imu/set_imu_zero',ImuZeroAngle)
# rospy.wait_for_service("/imu/set_imu_zero", 1) # timeout in sec, TODO maybe put in config file?
#TODO remove print
#Service Request-rosbridge
request = roslibpy.ServiceRequest()
result = service.call(request)
#result(angle)
# change button to green color to indicate that the service call went through
self._widget.set_imu_angle_button.setStyleSheet("background-color:#5eff00;")
except (rospy.ServiceException, rospy.ROSException) as e: # the second exception happens if the wait for service times out
self.errorPopup("Imu Set Angle Error", e)
#self.lock.release()
print("finished setting imu")
def imuAngleChanged(self):
#self.lock.acquire()
# change button to red color if someone fiddled with the angle input, to indicate that input wasn't set yet
self._widget.set_imu_angle_button
self._widget.set_imu_angle_button.setStyleSheet("background-color:#ff0000;")
#self.lock.release()
def setAutoWallDist(self):
print("setting auto wall distance")
#self.lock.acquire()
distance = self._widget.auto_wall_dist.value()
self._widget.auto_wall_dist_button.setStyleSheet("background-color:#5eff00;")
print("finished setting auto wall distance")
def autoWallDistChanged(self):
self._widget.auto_wall_dist_button.setStyleSheet("background-color:#ff0000;")
def resetBallCount(self):
print("manually reset ball count")
#self.lock.acquire()
nballs = self._widget.ball_reset_count.value()
# call the service
try:
rospy.wait_for_service("/reset_ball", 1) #timeout in sec, TODO maybe put in config file?
caller = rospy.ServiceProxy("/reset_ball", resetBallSrv)
caller(nballs)
# change button to green color to indicate that the service call went through
self._widget.set_imu_angle_button.setStyleSheet("background-color:##5eff00;")
except (rospy.ServiceException, rospy.ROSException) as e: # the second exception happens if the wait for service times out
self.errorPopup("Reset ball count Error", e)
#self.lock.release()
print("finished resetting ball count")
def resetBallChanged(self):
self._widget.ball_reset_button.setStyleSheet("background-color:#ff0000;")
def errorPopup(self, title, e):
msg_box = QMessageBox()
msg_box.setWindowTitle(title)
msg_box.setIcon(QMessageBox.Warning)
msg_box.setText("%s"%e)
msg_box.exec_()
#Publisher -> fake Auto States
def publish_thread(self):
pub = roslibpy.Topic(self.client, '/auto/auto_mode', 'behavior_actions/AutoMode')
r = rospy.Rate(10) # 10hz
pub.advertise()
while self.client.is_connected:
pub.publish(roslibpy.Message(
{
'auto_mode': self.auto_mode_button_group.checkedId()
}
))
r.sleep()
def shutdown_plugin(self):
self.auto_state_sub.unregister()
self.n_balls_sub.unregister()
self.shooter_in_range_sub.unregister()
self.turret_in_range_sub.unregister()
self.client.close()
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
pass
class CameraViewer(Plugin):
def __init__(self, context):
super(CameraViewer, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('CameraViewer')
# 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_drone_teleop'),
'resource', 'CamViewer.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('CamViewerUi')
# 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.bridge = CvBridge()
# Add Subscibers
cam_frontal_topic = rospy.get_param('cam_frontal_topic',
'/iris/cam_frontal/image_raw')
cam_ventral_topic = rospy.get_param('cam_ventral_topic',
'/iris/cam_ventral/image_raw')
rospy.Subscriber(cam_frontal_topic, Image, self.cam_frontal_cb)
rospy.Subscriber(cam_ventral_topic, Image, self.cam_ventral_cb)
rospy.Subscriber('interface/filtered_img', Image, self.filtered_img_cb)
rospy.Subscriber('interface/threshed_img', Image, self.threshed_img_cb)
def msg_to_pixmap(self, msg):
cv_img = self.bridge.imgmsg_to_cv2(msg)
shape = cv_img.shape
if len(shape) == 3 and shape[2] == 3: # RGB888
h, w, _ = shape
bytesPerLine = 3 * w
img_format = QImage.Format_RGB888
else: # Grayscale8
h, w = shape[0], shape[1]
bytesPerLine = 1 * w
img_format = QImage.Format_Grayscale8
q_img = QImage(cv_img.data, w, h, bytesPerLine, img_format)
return QPixmap.fromImage(q_img).scaled(320, 240)
def cam_frontal_cb(self, msg):
self._widget.img_frontal.setPixmap(self.msg_to_pixmap(msg))
def cam_ventral_cb(self, msg):
self._widget.img_ventral.setPixmap(self.msg_to_pixmap(msg))
def threshed_img_cb(self, msg):
self._widget.img_threshed.setPixmap(self.msg_to_pixmap(msg))
def filtered_img_cb(self, msg):
self._widget.img_filtered.setPixmap(self.msg_to_pixmap(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
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.clear_buffer_push_button.setIcon(
QIcon.fromTheme('edit-delete'))
self._widget.clear_buffer_push_button.pressed.connect(
self._clear_buffer)
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 _clear_buffer(self):
self.tf2_buffer_.clear()
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.values():
self._scene.addItem(node_item)
for edge_items in edges.values():
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)
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')
ns = rospy.get_namespace()[1:-1]
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(int(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(
int(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(
int(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(
int(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 Camera_is1500_Widget(Plugin):
def __init__(self, context):
super(Camera_is1500_Widget, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('Camera setting')
# 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 "resources" folder of this package
ui_file = os.path.join(rospkg.RosPack().get_path('rqt_camera_is1500'),
'resources', 'Camera_is1500_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('Camera_is1500_WidgetUI')
# 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)
"""
Start nodes
"""
self.target_x = 0.0
self.target_y = 0.0
self.distance_x = 0.0
"""
MAP path for is1500 package
"""
self.map_path_name = ''
self.map_file_name = ''
self.destination_map_file_name = ''
self.destination_map_config_file_name = ''
self.cameramap = []
self.current_wp = None
"""
RVIZ-APPs
"""
self.map_forRviz_file_name = self._widget.map_to_rviz_path_edit.text()
self.utm_x_value = float(self._widget.utm_x_edit.text())
self.utm_y_value = float(self._widget.utm_y_edit.text())
self.utm_phi_value = float(self._widget.utm_phi_edit.text())
self.joao_origin_utm_x_value = float(
self._widget.joao_origin_utm_x_edit.text())
self.joao_origin_utm_y_value = float(
self._widget.joao_origin_utm_y_edit.text())
self.fiducials_map = [
] # contain the position of the fiducial in camera frame
self.indoor = self._widget.indoor_checkBox_edit.isChecked()
self.init_transf = False # True if map already drew
self.origin_cam_x = 0.0
self.origin_cam_y = 0.0
self.last_robot_x_edit = 0.0 #float(self._widget.get_pos_robot_x_edit.text())
self.last_robot_y_edit = 0.0 #float(self._widget.get_pos_robot_y_edit.text())
self.last_robot_yaw_edit = 0.0 #float(self._widget.get_pos_robot_yaw_edit.text())
self.angle_btwXcam_East = 0.0
"""
Connect stuff here
"""
# self.model = QStandardItemModel(self._widget.arc_list)
# self._widget.name_edit.setValidator(QDoubleValidator())
# self._widget.path_edit.setValidator(QDoubleValidator())
# self._widget.in_case_edit.setValidator(QDoubleValidator())
# self._widget.y_edit.setValidator(QDoubleValidator())
# self._widget.map_name_edit.setValidator()
"""
Start nodes
"""
self._widget.launch_camera_start_button.released.connect(
self.start_camera_is1500_launchFile)
self._widget.color_launch_camera_label.setStyleSheet(
"background-color:#ff0000;")
self._widget.launch_supervisor_start_button.released.connect(
self.start_supervisor_launchFile)
self._widget.launch_supervisor_color_label.setStyleSheet(
"background-color:#ff0000;")
self._widget.rviz_start_button.released.connect(self.start_rviz)
self._widget.color_rviz_label.setStyleSheet(
"background-color:#ff0000;")
self._widget.target_start_button.released.connect(
self.target_waypontPy_run)
self._widget.color_target_label.setStyleSheet(
"background-color:#ff0000;")
self._widget.distance_start_button.released.connect(
self.distance_throughDeadZone_run)
self._widget.color_distance_label.setStyleSheet(
"background-color:#ff0000;")
self._widget.test_button.released.connect(self.test_button_function)
"""
Set map
"""
self._widget.send_map_param_button.released.connect(
self.set_map_rosparam)
self._widget.map_param_edit.setValidator(QDoubleValidator())
"""
Add/modify map
"""
self._widget.file_name_button.released.connect(self.get_file)
self._widget.desination_file_name_button.released.connect(
self.get_folder)
self._widget.desination_save_file_button.released.connect(
self.save_file)
self._widget.config_map_file_name_button.released.connect(
self.get_file_yaml)
self._widget.config_map_save_file_button.released.connect(
self.config_save_file)
self.model = QStandardItemModel(self._widget.map_from_mapYaml_list)
self.model.itemChanged.connect(self.on_change_mapList)
# Screen explaination msg
# self._widget.name_edit.setToolTip("Set the name of the folder which will contain the map ")
# self._widget.path_edit.setToolTip("Path of th map")
# self._widget.file_name_label.setToolTipe("Map file which be added to the map folder of the robot")
# Doesn't work
# self._widget.name_edit.description = "This is label name_edit"
# self._widget.path_edit.description = "This is label path_edit"
# self._widget.file_name_button.description = "This is the OK button"
#
# for widget in (self._widget.name_edit, self._widget.path_edit,
# self._widget.file_name_button):
# widget.bind("<Enter>", self.on_enter)
# widget.bind("<Leave>", self.on_leave)
self._widget.map_name_edit.textChanged.connect(self.map_name_change)
"""
Rviz part
"""
self._widget.utm_x_edit.setValidator(QDoubleValidator())
self._widget.utm_y_edit.setValidator(QDoubleValidator())
self._widget.utm_phi_edit.setValidator(QDoubleValidator())
self._widget.utm_x_edit.textChanged.connect(self.utm_x_change)
self._widget.utm_y_edit.textChanged.connect(self.utm_y_change)
self._widget.utm_phi_edit.textChanged.connect(self.utm_phi_change)
self._widget.joao_origin_utm_x_edit.setValidator(QDoubleValidator())
self._widget.joao_origin_utm_y_edit.setValidator(QDoubleValidator())
self._widget.joao_origin_utm_x_edit.textChanged.connect(
self.joao_origin_utm_x_change)
self._widget.joao_origin_utm_y_edit.textChanged.connect(
self.joao_origin_utm_y_change)
self._widget.indoor_checkBox_edit.stateChanged.connect(
self.indoor_isCheck)
self._widget.reset_init_map_file_button.released.connect(
self.reset_init_change)
self._widget.get_position_button.released.connect(self.get_pos_change)
self._widget.get_pos_robot_x_edit.setValidator(QDoubleValidator())
self._widget.get_pos_robot_y_edit.setValidator(QDoubleValidator())
self._widget.get_pos_robot_yaw_edit.setValidator(QDoubleValidator())
# Buttons
self._widget.map_to_rviz_send_file_button.released.connect(
self.visualize_fiducials)
self._widget.map_to_rviz_name_button.released.connect(
self.get_file_map_to_rviz)
"""
ROS
"""
self.marker_pub = rospy.Publisher(
'/fiducials_position',
visualization_msgs.msg.MarkerArray,
queue_size=10) # publish fiducials for a chosen map
self.currentMap_marker_pub = rospy.Publisher(
'/fiducials_position_current',
visualization_msgs.msg.MarkerArray,
queue_size=10) #publish currend used fiducials
self.camera_pos_sub = rospy.Subscriber("/base_link_odom_camera_is1500",
Odometry,
self.publish_transform_pos)
# self.camera_pos_sub = rospy.Subscriber("/position_camera_is1500", Odometry, self.publish_transform_pos)
self.transform_cameraPos_pub = rospy.Publisher(
'/transform_cameraPos_pub', Odometry, queue_size=1)
self.tf_buffer = tf2_ros.Buffer()
self.tf_listner = tf2_ros.TransformListener(self.tf_buffer)
self.fromMetric_sub = rospy.Subscriber('/pointsFromMetric',
GetPointsFromMetric,
self.publish_metric_to_rviz)
self.transform_metric_pub = rospy.Publisher(
'/rviz_pointsFromMetric',
visualization_msgs.msg.Marker,
queue_size=10)
# rospy.spin()
# empty yet
"""
Start nodes
"""
def start_camera_is1500_launchFile(self):
# os.spawnl(os.P_NOWAIT, 'sudo shutdown && 123456')
# subprocess.call('/home/jonathan/catkin_ws_kyb/src/rqt_camera_is1500/script/camera_is1500.bash', shell=True)
subprocess.call(
'/home/ew/catkin_ws/src/rqt_camera_is1500/script/camera_is1500.bash',
shell=True)
# os.spawnl(os.P_NOWAIT, 'cd && cd /home/jonathan/catkin_ws_kyb/ && source devel/setup.bash && cd src/camera_is1500/launch/ && roslaunch camera_is1500 cameraTry.launch')
# print 'Node: Camera_is1500 launched'
self._widget.color_launch_camera_label.setStyleSheet(
"background-color:#228B22;")
def start_supervisor_launchFile(self):
# cmd = "gnome-terminal -x sh -c 'cd && cd /home/jonathan/catkin_ws_kyb/ && source devel/setup.bash && roslaunch supervisor supervisor.launch"
# cmd = "gnome-terminal -x sh -c 'cd && cd /home/ew/catkin_ws/ && source devel/setup.bash && roslaunch supervisor supervisor.launch'"
# cmd = '/home/ew/catkin_ws/src/rqt_camera_is1500/script/supervisor.bash'
subprocess.call(
'/home/ew/catkin_ws/src/rqt_camera_is1500/script/supervisor.bash',
shell=True)
self._widget.launch_supervisor_color_label.setStyleSheet(
"background-color:#228B22;")
def start_rviz(self):
self._widget.color_rviz_label.setStyleSheet(
"background-color:#ff0000;")
# subprocess.call('/home/jonathan/catkin_ws_kyb/src/rqt_camera_is1500/script/rviz.bash', shell=True)
subprocess.call(
'/home/ew/catkin_ws/src/rqt_camera_is1500/script/rviz.bash',
shell=True)
# print 'RViz launched'
self._widget.color_rviz_label.setStyleSheet(
"background-color:#228B22;")
def target_waypontPy_run(self):
self.target_x = float(self._widget.target_x_edit.text())
self.target_y = float(self._widget.target_y_edit.text())
# cmd = ""
# gnome-terminal -x sh -c 'rosparam set /camera_is1500_node/mapNumber 1'
cmd = "gnome-terminal -x sh -c \'rosrun camera_is1500 waypoint_node.py %d %d \' " % (
float(self.target_x), float(self.target_y))
subprocess.call(cmd, shell=True)
self._widget.color_target_label.setStyleSheet(
"background-color:#228B22;")
def distance_throughDeadZone_run(self):
self.distance_x = float(self._widget.distance_x_edit.text())
cmd = "gnome-terminal -x sh -c \'rosrun camera_is1500 throughDeadZone.py %d \' " % (
float(self.distance_x))
# cmd = "rosrun camera_is1500 throughDeadZone.py %d" % (float(self.distance_x))
subprocess.call(str(cmd), shell=True)
self._widget.color_distance_label.setStyleSheet(
"background-color:#228B22;")
def test_button_function(self):
print('Checkbox: ', self.indoor)
# def toggleLeftWidget(self):
# self.map_import_layout.setHidden(not self.map_import_layout.isHidden())
"""
Node param settings
"""
def set_map_rosparam(self):
cmd = "rosparam set /camera_is1500_node/mapNumber %d" % (float(
self._widget.map_param_edit.text()))
subprocess.call(cmd, shell=True)
"""
Map selection function
"""
def map_name_change(self):
self.map_file_name = self._widget.map_name_edit.text()
"""
"""
def read_map(self):
self.cameramap = []
with open(self.destination_map_config_file_name, 'r') as stream:
map_list = yaml.load(stream)
for map in map_list:
new_map = CameraMap()
new_map.from_yaml(map)
self.cameramap.append(new_map)
# self.populate_combobox()
# rospy.loginfo('Read: %s'%self.cameramap)
# print('Read: %s'%self.cameramap)
print('Load data')
for i in range(0, len(self.cameramap)):
print(self.cameramap[i].name)
print(self.cameramap[i].path)
# self.wait_timer = rospy.Timer(rospy.Duration(2.), self.visualize_waypoints, oneshot=True)
"""
"""
def config_save_file(self):
map_list = []
for map in self.cameramap:
new_map = {} #CameraMap()#{}
new_map['name'] = map.name
new_map['path'] = map.path
map_list.append(new_map)
new_map_to_add = {}
new_map_to_add['name'] = self.map_file_name
new_map_to_add['path'] = self.destination_map_config_file_name
map_list.append(new_map_to_add)
with open(self.destination_map_config_file_name, 'w') as stream:
stream.write(
yaml.safe_dump(map_list,
encoding='utf-8',
allow_unicode=False,
default_flow_style=False))
print 'Saved elements in file' + self.destination_map_config_file_name
print yaml.safe_dump(map_list,
encoding='utf-8',
allow_unicode=False,
default_flow_style=False)
"""
"""
def save_file(self):
print('Hi, you are triing to save something with the name of : ',
map_name_edit)
"""
"""
def get_file(self):
file_dlg = QFileDialog()
file_dlg.setFileMode(QFileDialog.AnyFile)
if file_dlg.exec_():
map_file_name = file_dlg.selectedFiles()
rospy.loginfo('Selected files: %s' % map_file_name)
if len(map_file_name) > 0:
self.map_path_name = map_file_name[0]
self._widget.file_path_edit.setText(self.map_path_name)
# self.read_waypoints()
"""
"""
def get_folder(self):
file_dlg = QFileDialog()
file_dlg.setFileMode(QFileDialog.Directory)
if file_dlg.exec_():
destination_map_file_name = file_dlg.selectedFiles()
rospy.loginfo('Selected files: %s' % destination_map_file_name)
if len(destination_map_file_name) > 0:
self.destination_map_file_name = destination_map_file_name[0]
self._widget.desination_file_name_edit.setText(
self.destination_map_file_name)
"""
"""
def get_file_yaml(self):
file_dlg = QFileDialog()
file_dlg.setFileMode(QFileDialog.AnyFile)
# file_dlg.setFilter("Yaml files (*.yaml)")
# print('Hi, you try to get a file')
# print(self.map_file_name)
if file_dlg.exec_():
destination_map_config_file_name = file_dlg.selectedFiles()
rospy.loginfo('Selected files: %s' %
destination_map_config_file_name)
if len(destination_map_config_file_name) > 0:
self.destination_map_config_file_name = destination_map_config_file_name[
0]
self._widget.config_map_file_name_edit.setText(
self.destination_map_config_file_name)
self.read_map()
"""
"""
# TODO : ........
def on_change_mapList(self):
print(hahaa)
"""
RViz group
"""
def utm_x_change(self):
self.utm_x_value = float(self._widget.utm_x_edit.text())
# print('utm_x: ', self.utm_x_value, ', type: ', type(self.utm_x_value))
"""
"""
def utm_y_change(self):
self.utm_y_value = float(self._widget.utm_y_edit.text())
# print('utm_y: ', self.utm_y_value, ', type: ', type(self.utm_y_value))
"""
"""
def utm_phi_change(self):
self.utm_phi_value = float(self._widget.utm_phi_edit.text())
# print('utm_phi: ', self.utm_phi_value, ', type: ', type(self.utm_phi_value))
"""
"""
def joao_origin_utm_x_change(self):
self.joao_origin_utm_x_value = float(
self._widget.joao_origin_utm_x_edit.text())
"""
"""
def joao_origin_utm_y_change(self):
self.joao_origin_utm_y_value = float(
self._widget.joao_origin_utm_y_edit.text())
"""
"""
def get_file_map_to_rviz(self):
file_dlg = QFileDialog()
file_dlg.setFileMode(QFileDialog.AnyFile)
# file_dlg.setFilter("Yaml files (*.yaml)")
# print('Hi, you try to get a file')
if file_dlg.exec_():
map_file_name = file_dlg.selectedFiles()
rospy.loginfo(
'Selected files to extract position of the fiducials: %s' %
map_file_name)
if len(map_file_name) > 0:
self.map_forRviz_file_name = map_file_name[0]
self._widget.map_to_rviz_path_edit.setText(
self.map_forRviz_file_name)
"""
"""
# init the map changement
def reset_init_change(self):
self.init_transf = False
# delete all fiducial
marker_array = visualization_msgs.msg.MarkerArray()
new_marker = visualization_msgs.msg.Marker()
new_marker.header.stamp = rospy.Time.now()
# Set frame id
new_marker.header.frame_id = 'odom'
new_marker.ns = 'fiducials_pos'
new_marker.action = visualization_msgs.msg.Marker.DELETEALL
marker_array.markers.append(new_marker)
self.currentMap_marker_pub.publish(marker_array)
"""
"""
# Screen in RViz a selected map in chosen position when button is preshed
def visualize_fiducials(self, event=None):
self.generalizedDrawingMap(
self.map_forRviz_file_name, radians(self.utm_phi_value),
[float(self.utm_x_value),
float(self.utm_y_value)],
[self.joao_origin_utm_x_value, self.joao_origin_utm_y_value],
self.marker_pub)
"""
"""
def get_pos_change(self):
trans = self.tf_buffer.lookup_transform('odom', 'base_link',
rospy.Time())
self._widget.get_pos_robot_x_edit.setText(
str(trans.transform.translation.x))
self._widget.get_pos_robot_y_edit.setText(
str(trans.transform.translation.y))
self.last_robot_x_edit = float(
self._widget.get_pos_robot_x_edit.text())
self.last_robot_y_edit = float(
self._widget.get_pos_robot_y_edit.text())
quat = trans.transform.rotation
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion(
(quat.x, quat.y, quat.z, quat.w))
self._widget.get_pos_robot_yaw_edit.setText(str(yaw / 2 / 3.14 * 360))
self.last_robot_yaw_edit = float(
self._widget.get_pos_robot_yaw_edit.text())
"""
"""
# Screen in RViz a map
# Input: data_path: path of the map from camera is 1500
# angleMap: compare to the east for x-axis in RAD
# originMapInUTM: Origin of the camera (usually #100) in UTM coordinates
# originGeneralUTM: Joao's origin
# Output: screen map on RViz
def generalizedDrawingMap(self, data_path, angleMap, originMapInUTM,
originGeneralUTM, publisher):
data = pd.read_csv(data_path,
header=None,
sep=' ',
skipinitialspace=True,
low_memory=False,
skiprows=3)
fiducial_pos = data.values[:,
1], data.values[:,
14], -data.values[:,
15], data.values[:,
16] #concatanate the vector
self.fiducials_map = fiducial_pos
# delete all fiducial
marker_array = visualization_msgs.msg.MarkerArray()
new_marker = visualization_msgs.msg.Marker()
new_marker.header.stamp = rospy.Time.now()
# Set frame id
new_marker.header.frame_id = 'odom'
new_marker.ns = 'fiducials_pos'
new_marker.action = visualization_msgs.msg.Marker.DELETEALL
marker_array.markers.append(new_marker)
publisher.publish(marker_array)
joao_x = originGeneralUTM[0] #self.joao_origin_utm_x_value#468655
joao_y = originGeneralUTM[1] #self.joao_origin_utm_y_value#5264080
gps_origin_map_x = originMapInUTM[
0] #float(self.utm_x_value)#468598.24
gps_origin_map_y = originMapInUTM[
1] #float(self.utm_y_value)#5264012.01
# Calculate the size of the fiducials matrix
length = len(fiducial_pos[0]) - 1
# Fiducial points to RVIZ
marker_array = visualization_msgs.msg.MarkerArray()
for i in range(0, length, 3):
new_marker = visualization_msgs.msg.Marker()
new_marker.header.stamp = rospy.Time.now()
new_marker.header.frame_id = 'odom'
new_marker.id = i #int(fiducial_pos[i][0])
new_marker.ns = 'fiducials position'
new_marker.type = visualization_msgs.msg.Marker.SPHERE
new_marker.action = visualization_msgs.msg.Marker.ADD
x = fiducial_pos[1][i]
y = fiducial_pos[2][i]
new_marker.pose.position.x = x * np.cos(angleMap) - y * np.sin(
angleMap) + gps_origin_map_x - joao_x
new_marker.pose.position.y = x * np.sin(angleMap) + y * np.cos(
angleMap) + gps_origin_map_y - joao_y
print('( ', new_marker.pose.position.x, ', ',
new_marker.pose.position.y, ') ')
new_marker.pose.position.z = -fiducial_pos[3][i]
quat = tf.transformations.quaternion_from_euler(
0, 0, 0) # TODO set the angle
new_marker.pose.orientation.x = quat[0]
new_marker.pose.orientation.y = quat[1]
new_marker.pose.orientation.z = quat[2]
new_marker.pose.orientation.w = quat[3]
new_marker.scale.x = 0.05
new_marker.scale.y = 0.05
new_marker.scale.z = 0.05
new_marker.color.a = 1.0
new_marker.color.r = 1.0
new_marker.color.g = 0.0
new_marker.color.b = 0.0
marker_array.markers.append(new_marker)
publisher.publish(marker_array)
# Fiducial names
marker_array = visualization_msgs.msg.MarkerArray()
for i in range(0, length, 3):
new_marker = visualization_msgs.msg.Marker()
new_marker.header.stamp = rospy.Time.now()
new_marker.header.frame_id = 'odom'
new_marker.id = i #int(fiducial_pos[i][0])
new_marker.ns = 'fiducials_names'
new_marker.type = visualization_msgs.msg.Marker.TEXT_VIEW_FACING
new_marker.action = visualization_msgs.msg.Marker.ADD
new_marker.text = str(fiducial_pos[0][i])
x = fiducial_pos[1][i]
y = fiducial_pos[2][i]
new_marker.pose.position.x = x * np.cos(angleMap) - y * np.sin(
angleMap) + gps_origin_map_x - joao_x
new_marker.pose.position.y = x * np.sin(angleMap) + y * np.cos(
angleMap) + gps_origin_map_y - joao_y
new_marker.pose.position.z = -fiducial_pos[3][i] + 0.1
new_marker.scale.z = 0.1
new_marker.color.a = 1.0
new_marker.color.r = 1.0
new_marker.color.g = 1.0
new_marker.color.b = 1.0
marker_array.markers.append(new_marker)
publisher.publish(marker_array)
"""
"""
def transformFromLastGPS(self, positionYaw, lastUTM):
# TODO: Check where is these angles
# global angle_btwXcam_East = positionYaw[2] #radians(self.last_robot_yaw_edit)#radians(self.utm_phi_value)#45.0) # 0 = align with east for x and north = y
angle_btwXcam_East = self.angle_btwXcam_East
last_gps_yaw = radians(0.0) #in radians
self.distance_x = float(self._widget.distance_x_edit.text())
wheel_odom_distance = (float(self.distance_x)) #3.0 # meter
# if the map is already drew... just send the transfrom position
if (self.init_transf == False):
self.angle_btwXcam_East = positionYaw[2]
# Use East as x-axis
direction_vecteur = [
float(wheel_odom_distance * np.cos(last_gps_yaw)),
float(wheel_odom_distance * np.sin(last_gps_yaw))
]
# Robot position in UTM inside the map
first_robot_x = lastUTM[0] + direction_vecteur[0]
first_robot_y = lastUTM[1] + direction_vecteur[1]
# Rotation from camera frame to UTM
rotated_x = positionYaw[0] * np.cos(angle_btwXcam_East) - (
-1) * positionYaw[1] * np.sin(angle_btwXcam_East)
rotated_y = positionYaw[0] * np.sin(angle_btwXcam_East) + (
-1) * positionYaw[1] * np.cos(angle_btwXcam_East)
self.origin_cam_x = first_robot_x - rotated_x
self.origin_cam_y = first_robot_y - rotated_y
# Draw the selected map
self.generalizedDrawingMap(
self.map_forRviz_file_name, angle_btwXcam_East,
[self.origin_cam_x, self.origin_cam_y],
[self.joao_origin_utm_x_value, self.joao_origin_utm_y_value],
self.currentMap_marker_pub)
self.init_transf = True
# print(origin_cam_x, ', ', origin_cam_x)
x = positionYaw[0]
y = positionYaw[1]
yaw = positionYaw[2]
# Rotation from camera frame to UTM
# Translation
x_prim = x * np.cos(angle_btwXcam_East) - (-1) * y * np.sin(
angle_btwXcam_East
) + self.origin_cam_x - self.joao_origin_utm_x_value
# ATTENTION: ADD a minus here on y position to be on the map
y_prim = (x * np.sin(angle_btwXcam_East) +
(-1) * y * np.cos(angle_btwXcam_East)
) + self.origin_cam_y - self.joao_origin_utm_y_value
yaw_prim = -yaw + angle_btwXcam_East # Right !!!
return x_prim, y_prim, yaw_prim
"""
"""
def indoor_isCheck(self):
self.indoor = self._widget.indoor_checkBox_edit.isChecked()
"""
"""
def publish_transform_pos(self, msg):
if self.indoor:
# Angle in rad
(roll, pitch,
yaw_before) = tf.transformations.euler_from_quaternion([
msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z, msg.pose.pose.orientation.w
])
# Transform the camera data for Rviz
# 468655
# 5264080
# 468598.24
# 5264012.01
x, y, yaw = self.transformFromLastGPS([
msg.pose.pose.position.x, msg.pose.pose.position.y, yaw_before
], [
468655.0 + self.last_robot_x_edit,
5264080.0 + self.last_robot_y_edit
])
# x, y, yaw = self.transformFromLastGPS([msg.pose.pose.position.x,
# msg.pose.pose.position.y, yaw_before], [468655.0+3.0, 5264080.0-1.0])
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = 'odom'
odom.child_frame_id = 'base_link'
odom.pose.pose.position.x = x
odom.pose.pose.position.y = y
odom.pose.pose.position.z = 0
# Don't forget the "Quaternion()" and *
odom.pose.pose.orientation = Quaternion(
*tf.transformations.quaternion_from_euler(0, 0, yaw))
# publisher
self.transform_cameraPos_pub.publish(odom)
# else:
# print('Outdoor')
"""
Metric
"""
def publish_metric_to_rviz(self, msg):
# print ("Receive something")
if self.indoor:
x, y, yaw = self.transformFromLastGPS([msg.x, msg.y, 0.0], [
468655.0 + self.last_robot_x_edit,
5264080.0 + self.last_robot_y_edit
])
# x, y, yaw = self.transformFromLastGPS([msg.pose.pose.position.x,
# msg.pose.pose.position.y, yaw_before], [468655.0+3.0, 5264080.0-1.0])
new_marker = visualization_msgs.msg.Marker()
new_marker.header.stamp = rospy.Time.now()
new_marker.header.frame_id = 'odom'
new_marker.ns = 'Position issue'
new_marker.type = visualization_msgs.msg.Marker.SPHERE
new_marker.action = visualization_msgs.msg.Marker.ADD
new_marker.text = str(msg.cost)
new_marker.pose.position.x = x
new_marker.pose.position.y = y
new_marker.pose.position.z = 0.0
new_marker.scale.x = 0.5
new_marker.scale.y = 0.5
new_marker.scale.z = 0.5
new_marker.color.a = 1.0
new_marker.color.r = 0.0
new_marker.color.g = 1.0
new_marker.color.b = 0.0
# publisher
self.transform_metric_pub.publish(new_marker)
new_marker = visualization_msgs.msg.Marker()
new_marker.header.stamp = rospy.Time.now()
new_marker.header.frame_id = 'odom'
new_marker.ns = 'Position issue'
new_marker.type = visualization_msgs.msg.Marker.TEXT_VIEW_FACING
new_marker.action = visualization_msgs.msg.Marker.ADD
new_marker.text = str(msg.cost)
new_marker.pose.position.x = x
new_marker.pose.position.y = y
new_marker.pose.position.z = 0.6
new_marker.scale.z = 0.8
new_marker.color.a = 1.0
new_marker.color.r = 0.0
new_marker.color.g = 1.0
new_marker.color.b = 0.0
# publisher
self.transform_metric_pub.publish(new_marker)
class VelTeleop(Plugin):
def __init__(self, context):
super(VelTeleop, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('VelTeleop')
# 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_drone_teleop'),
'resource', 'VelocityTeleop.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('VelTeleopUi')
# 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_drone_teleop'),
'resource', 'jderobot.png'))
self._widget.img_logo.setPixmap(pixmap.scaled(121, 121))
# Set Variables
self.play_code_flag = False
self.takeoff = False
self.linear_velocity_scaling_factor = 1
self.vertical_velocity_scaling_factor = 0.8
self.angular_velocity_scaling_factor = 0.5
self._widget.term_out.setReadOnly(True)
self._widget.term_out.setLineWrapMode(self._widget.term_out.NoWrap)
# Set functions for each GUI Item
self._widget.takeoffButton.clicked.connect(self.call_takeoff_land)
self._widget.playButton.clicked.connect(self.call_play)
self._widget.stopButton.clicked.connect(self.stop_drone)
# Add Publishers
self.play_stop_pub = rospy.Publisher('gui/play_stop',
Bool,
queue_size=1)
# Add global variables
self.extended_state = ExtendedState()
self.shared_twist_msg = Twist()
self.current_pose = Pose()
self.pose_frame = ''
self.current_twist = Twist()
self.twist_frame = ''
self.is_running = True
self.stop_icon = QIcon()
self.stop_icon.addPixmap(
QPixmap(
os.path.join(rospkg.RosPack().get_path('rqt_drone_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_drone_teleop'),
'resource', 'play.png')), QIcon.Normal, QIcon.Off)
self.teleop_stick_1 = TeleopWidget(self, 'set_linear_xy', 151)
self._widget.tlLayout.addWidget(self.teleop_stick_1)
self.teleop_stick_1.setVisible(True)
self.teleop_stick_2 = TeleopWidget(self, 'set_alt_yawrate', 151)
self._widget.tlLayout_2.addWidget(self.teleop_stick_2)
self.teleop_stick_2.setVisible(True)
self.sensors_widget = SensorsWidget(self)
self._widget.sensorsCheck.stateChanged.connect(
self.show_sensors_widget)
# Add widget to the user interface
context.add_widget(self._widget)
# Add Subscribers
rospy.Subscriber('drone_wrapper/local_position/pose', PoseStamped,
self.pose_stamped_cb)
rospy.Subscriber('drone_wrapper/local_position/velocity_body',
TwistStamped, self.twist_stamped_cb)
rospy.Subscriber('drone_wrapper/extended_state', ExtendedState,
self.extended_state_cb)
# Add Timer
self.update_status_info()
def show_sensors_widget(self, state):
if state == Qt.Checked:
self.sensors_widget.show()
else:
self.sensors_widget.hide()
def pose_stamped_cb(self, msg):
self.current_pose = msg.pose
self.pose_frame = msg.header.frame_id
self.sensors_widget.sensorsUpdate.emit()
def twist_stamped_cb(self, msg):
self.current_twist = msg.twist
self.twist_frame = msg.header.frame_id
def update_status_info(self):
threading.Timer(0.5, self.update_status_info).start()
if self.is_running:
self.set_info_pos(self.current_pose, self.pose_frame)
self.set_info_vel(self.current_twist, self.twist_frame)
def set_info_pos(self, pose, frame):
self._widget.posX.setText(str(round(pose.position.x, 2)))
self._widget.posY.setText(str(round(pose.position.y, 2)))
self._widget.posZ.setText(str(round(pose.position.z, 2)))
self._widget.posFrame.setText(str(frame))
def set_info_vel(self, twist, frame):
self._widget.velX.setText(str(round(twist.linear.x, 2)))
self._widget.velY.setText(str(round(twist.linear.y, 2)))
self._widget.velZ.setText(str(round(twist.linear.z, 2)))
self._widget.velYaw.setText(str(round(twist.angular.z, 2)))
self._widget.velFrame.setText(str(frame))
def extended_state_cb(self, msg):
if self.extended_state.landed_state != msg.landed_state:
self.extended_state = msg
if self.extended_state.landed_state == 1: # ON GROUND
self._widget.takeoffButton.setText("Take Off")
elif self.extended_state.landed_state == 2: # IN AIR
self._widget.takeoffButton.setText("Land")
def takeoff_drone(self):
try:
global drone
drone.takeoff()
self.takeoff = True
finally:
rospy.loginfo('Takeoff finished')
def call_takeoff_land(self):
if self.extended_state.landed_state == 0: # UNDEFINED --> not ready
self._widget.term_out.append('Drone not ready')
return
if self.takeoff == True:
rospy.loginfo('Landing')
self._widget.term_out.append('Landing')
global drone
drone.land()
self.takeoff = False
else:
rospy.loginfo('Taking off')
self._widget.term_out.append('Taking off')
x = threading.Thread(target=self.takeoff_drone)
x.daemon = True
x.start()
def call_play(self):
if not self.play_code_flag:
self._widget.playButton.setText('Stop Code')
self._widget.playButton.setStyleSheet("background-color: #ec7063")
self._widget.playButton.setIcon(self.stop_icon)
rospy.loginfo('Executing student code')
self._widget.term_out.append('Executing student code')
self.play_stop_pub.publish(Bool(True))
self.play_code_flag = True
else:
self._widget.playButton.setText('Play Code')
self._widget.playButton.setStyleSheet("background-color: #7dcea0")
self._widget.playButton.setIcon(self.play_icon)
rospy.loginfo('Stopping student code')
self._widget.term_out.append('Stopping student code')
self.play_stop_pub.publish(Bool(False))
self.play_code_flag = False
def stop_drone(self):
self._widget.term_out.append('Stopping Drone')
rospy.loginfo('Stopping Drone')
self.teleop_stick_1.stop()
self.teleop_stick_2.stop()
if self.play_code_flag:
self.call_play()
for i in range(5):
self.shared_twist_msg = Twist()
global drone
drone.set_cmd_vel(self.shared_twist_msg.linear.x,
self.shared_twist_msg.linear.y,
self.shared_twist_msg.linear.z,
self.shared_twist_msg.angular.z)
rospy.sleep(0.05)
def set_linear_xy(self, u, v):
x = -self.linear_velocity_scaling_factor * v
y = -self.linear_velocity_scaling_factor * u
self._widget.XValue.setText('%.2f' % x)
self._widget.YValue.setText('%.2f' % y)
rospy.logdebug('Stick 2 value changed to - x: %.2f y: %.2f', x, y)
self.shared_twist_msg.linear.x = x
self.shared_twist_msg.linear.y = y
global drone
drone.set_cmd_vel(self.shared_twist_msg.linear.x,
self.shared_twist_msg.linear.y,
self.shared_twist_msg.linear.z,
self.shared_twist_msg.angular.z)
def set_alt_yawrate(self, u, v):
az = -self.vertical_velocity_scaling_factor * u
z = -self.angular_velocity_scaling_factor * v
self._widget.rotValue.setText('%.2f' % az)
self._widget.altdValue.setText('%.2f' % z)
rospy.logdebug('Stick 1 value changed to - az: %.2f z: %.2f', az, z)
self.shared_twist_msg.linear.z = z
self.shared_twist_msg.angular.z = az
global drone
drone.set_cmd_vel(self.shared_twist_msg.linear.x,
self.shared_twist_msg.linear.y,
self.shared_twist_msg.linear.z,
self.shared_twist_msg.angular.z)
def shutdown_plugin(self):
# TODO unregister all publishers here
self.is_running = False
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 GaitSelectionPlugin(Plugin):
def __init__(self, context):
super(GaitSelectionPlugin, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('GaitSelectionPlugin')
# Connect to services
try:
rospy.wait_for_service('/march/gait_selection/get_version_map', 3)
rospy.wait_for_service('/march/gait_selection/get_directory_structure', 3)
rospy.wait_for_service('/march/gait_selection/set_version_map', 3)
rospy.wait_for_service('/march/gait_selection/update_default_versions', 3)
except rospy.ROSException:
rospy.logerr('Shutting down march_rqt_gait_selection, could not connect to march_gait_selection.')
rospy.signal_shutdown('Shutting down march_rqt_gait_selection, could not connect to march_gait_selection.')
sys.exit(0)
self.get_version_map = rospy.ServiceProxy('/march/gait_selection/get_version_map', Trigger)
self.get_directory_structure = rospy.ServiceProxy('/march/gait_selection/get_directory_structure', Trigger)
self.set_version_map = rospy.ServiceProxy('/march/gait_selection/set_version_map', StringTrigger)
self.update_default_versions = rospy.ServiceProxy('/march/gait_selection/update_default_versions', Trigger)
# 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('march_rqt_gait_selection'), 'resource', 'gait_selection.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('Gait Selection')
# 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.Gaits.findChild(QWidget, 'scrollArea').findChild(QWidget, 'content').setLayout(QVBoxLayout())
self.refresh()
self._widget.findChild(QPushButton, 'Refresh').clicked.connect(lambda: self.refresh(True))
self._widget.findChild(QPushButton, 'Apply').clicked.connect(
lambda: [
self.set_gait_selection_map(True),
self.refresh(),
])
self._widget.findChild(QPushButton, 'SaveDefault').clicked.connect(
lambda: [
self.set_gait_selection_map(),
self.update_defaults(True),
self.refresh(),
])
self.log('Welcome to the Gait Selection.', Color.Debug)
self.log('Select the versions of subgaits you want to select and press Apply.', Color.Info)
self.log('Save as default persists between launches', Color.Info)
self.log('Any warnings or errors will be displayed in this log.', Color.Warning)
self.log('--------------------------------------', Color.Info)
def log(self, msg, level):
self._widget \
.findChild(QPlainTextEdit, 'Log') \
.appendHtml('<p style="color:' + str(level.value) + '">' + msg + '</p>')
scrollbar = self._widget.findChild(QPlainTextEdit, 'Log').verticalScrollBar()
scrollbar.setValue(scrollbar.maximum())
def refresh(self, notify=False):
if notify:
self.log('Refreshing gait directory', Color.Debug)
rospy.logdebug('Refreshing ui with %s', str(self.get_directory_structure().message))
try:
gait_version_map = ast.literal_eval(self.get_version_map().message)
except ValueError:
self.log('Gait selection map is not valid'
+ str(self.get_version_map().message), Color.Error)
rospy.logerr('Gait selection map is not valid'
+ str(self.get_version_map().message))
return
try:
gait_directory_structure = ast.literal_eval(self.get_directory_structure().message)
except ValueError:
self.log('Gait directory structure is not valid '
+ str(self.get_directory_structure().message), Color.Error)
rospy.logerr('Gait directory structure is not valid '
+ str(self.get_directory_structure().message))
return
gaits = self._widget.Gaits \
.findChild(QWidget, 'scrollArea') \
.findChild(QWidget, 'content') \
.findChildren(QGroupBox, 'Gait')
for gait in gaits:
gait.deleteLater()
self.load_ui(gait_directory_structure, gait_version_map)
def load_ui(self, gait_directory_structure, gait_selection_map):
for gait_name, gait in gait_directory_structure.iteritems():
try:
selection_map = gait_selection_map[gait_name]
except KeyError:
selection_map = None
gait_group_box = self.create_gait(gait_name, gait, selection_map)
self._widget.Gaits \
.findChild(QWidget, 'scrollArea') \
.findChild(QWidget, 'content') \
.layout() \
.addWidget(gait_group_box)
def create_gait(self, name, gait, selections):
gait_group_box = QGroupBox()
gait_group_box.setObjectName('Gait')
gait_group_box.setLayout(QHBoxLayout())
gait_group_box.setTitle(name)
image = QLabel()
image.setStyleSheet(
'background: url(' + gait['image'] + ') no-repeat center center 100px 100px;')
image.setFixedSize(64, 80)
gait_group_box.layout().addWidget(image)
for subgait_name, subgait in gait['subgaits'].iteritems():
subgait_group_box = self.create_subgait(subgait_name, subgait, selections)
gait_group_box.layout().addWidget(subgait_group_box)
return gait_group_box
def create_subgait(self, name, subgait, version_selection):
subgait_group_box = QGroupBox()
subgait_group_box.setLayout(QGridLayout())
subgait_group_box.setObjectName('Subgait')
subgait_group_box.setTitle(name)
try:
version_name = version_selection[name]
except TypeError:
version_name = None
except KeyError:
version_name = None
dropdown = self.create_dropdown(subgait, version_name)
subgait_group_box.layout().addWidget(dropdown, 0, 0)
return subgait_group_box
def create_dropdown(self, options, selection):
try:
index = options.index(selection)
except ValueError:
rospy.loginfo('Selection %s not found in options %s.', str(selection), str(options))
index = -1
dropdown = QComboBox()
for option in options:
dropdown.addItem(option)
dropdown.setCurrentIndex(index)
return dropdown
def set_gait_selection_map(self, notify=False):
gait_selection_map = {}
gaits = self._widget.Gaits \
.findChild(QWidget, 'scrollArea') \
.findChild(QWidget, 'content') \
.findChildren(QGroupBox, 'Gait')
for gait in gaits:
gait_name = str(gait.title())
gait_selection_map[gait_name] = {}
subgaits = gait.findChildren(QGroupBox, 'Subgait')
for subgait in subgaits:
subgait_name = str(subgait.title())
version = str(subgait.findChild(QComboBox).currentText())
gait_selection_map[gait_name][subgait_name] = version
res = self.set_version_map(str(gait_selection_map))
if res.success:
if notify:
self.log('Selection applied.', Color.Debug)
rospy.loginfo(res.message)
else:
self.log(res.message, Color.Error)
self.log('Resetting to last valid selection', Color.Warning)
rospy.logwarn(res.message)
def update_defaults(self, notify=False):
res = self.update_default_versions()
if res.success:
if notify:
self.log('Default selection updated.', Color.Debug)
rospy.loginfo(res.message)
else:
self.log(res.message, Color.Error)
self.log('Resetting to last valid selection', Color.Warning)
rospy.logwarn(res.message)
class kms40_emulator_plugin(Plugin):
def __init__(self, context):
super(kms40_emulator_plugin, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('kms40_emulator_plugin')
# 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__)), 'weiss_kms40_emulator.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('kms40_emulator_pluginUi')
# Connect UI to slots
self._widget.pushButton_run.clicked[bool].connect(self._handle_run_clicked)
self._widget.pushButton_stop.clicked[bool].connect(self._handle_stop_clicked)
self._widget.spinBox_rate.valueChanged.connect(self._handle_rate_changed)
self._widget.doubleSpinBox_forceX.valueChanged.connect(self._handle_forceX_changed)
self._widget.doubleSpinBox_forceY.valueChanged.connect(self._handle_forceY_changed)
self._widget.doubleSpinBox_forceZ.valueChanged.connect(self._handle_forceZ_changed)
self._widget.doubleSpinBox_forceA.valueChanged.connect(self._handle_forceA_changed)
self._widget.doubleSpinBox_forceB.valueChanged.connect(self._handle_forceB_changed)
self._widget.doubleSpinBox_forceC.valueChanged.connect(self._handle_forceC_changed)
self._widget.horizontalSlider_forceX.valueChanged.connect(self._handle_sliderX_changed)
self._widget.horizontalSlider_forceY.valueChanged.connect(self._handle_sliderY_changed)
self._widget.horizontalSlider_forceZ.valueChanged.connect(self._handle_sliderZ_changed)
self._widget.horizontalSlider_forceA.valueChanged.connect(self._handle_sliderA_changed)
self._widget.horizontalSlider_forceB.valueChanged.connect(self._handle_sliderB_changed)
self._widget.horizontalSlider_forceC.valueChanged.connect(self._handle_sliderC_changed)
self._widget.lineEdit_topic.editingFinished.connect(self._handle_topic_changed)
self._widget.lineEdit_frameId.textChanged.connect(self._handle_frameId_changed)
# 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)
# PublisherThread
self.t = PublisherThread()
self.t.start()
self.updateWidgets()
def shutdown_plugin(self):
self.t.end()
self.t.join()
# 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 _handle_run_clicked(self):
print "run now"
self.t.restart()
def _handle_stop_clicked(self):
print "stop now"
self.t.stop()
def _handle_rate_changed(self):
val = self._widget.spinBox_rate.value()
#print "handle_rate_changed", int(val)
self.t.setRate(val)
#spinBoxes
def _handle_forceX_changed(self):
val = self._widget.doubleSpinBox_forceX.value()
#print "_handle_forceX_changed", val
self.t.setWrenchFX(val)
self.updateWidgets()
def _handle_forceY_changed(self):
val = self._widget.doubleSpinBox_forceY.value()
#print "_handle_forceY_changed", val
self.t.setWrenchFY(val)
self.updateWidgets()
def _handle_forceZ_changed(self):
val = self._widget.doubleSpinBox_forceZ.value()
#print "_handle_forceZ_changed", val
self.t.setWrenchFZ(val)
self.updateWidgets()
def _handle_forceA_changed(self):
val = self._widget.doubleSpinBox_forceA.value()
#print "_handle_forceA_changed", val
self.t.setWrenchTZ(val)
self.updateWidgets()
def _handle_forceB_changed(self):
val = self._widget.doubleSpinBox_forceB.value()
#print "_handle_forceB_changed", val
self.t.setWrenchTY(val)
self.updateWidgets()
def _handle_forceC_changed(self):
val = self._widget.doubleSpinBox_forceC.value()
#print "_handle_forceC_changed", val
self.t.setWrenchTX(val)
self.updateWidgets()
# sliders
def _handle_sliderX_changed(self):
val = self._widget.horizontalSlider_forceX.value()
#print "_handle_sliderX_changed", val
self.t.setWrenchFX(val)
self.updateWidgets()
def _handle_sliderY_changed(self):
val = self._widget.horizontalSlider_forceY.value()
#print "_handle_sliderY_changed", val
self.t.setWrenchFY(val)
self.updateWidgets()
def _handle_sliderZ_changed(self):
val = self._widget.horizontalSlider_forceZ.value()
#print "_handle_sliderZ_changed", val
self.t.setWrenchFZ(val)
self.updateWidgets()
def _handle_sliderA_changed(self):
val = self._widget.horizontalSlider_forceA.value()
#print "_handle_sliderA_changed", val
self.t.setWrenchTZ(val)
self.updateWidgets()
def _handle_sliderB_changed(self):
val = self._widget.horizontalSlider_forceB.value()
#print "_handle_sliderB_changed", val
self.t.setWrenchTY(val)
self.updateWidgets()
def _handle_sliderC_changed(self):
val = self._widget.horizontalSlider_forceC.value()
#print "_handle_sliderC_changed", val
self.t.setWrenchTX(val)
self.updateWidgets()
def _handle_frameId_changed(self):
val = self._widget.lineEdit_frameId.text()
#print "_handle_topic_changed", val#self._widget.lineEdit_topic.toPlainText()
self.t.setFrameId(val)
def _handle_topic_changed(self):
val = self._widget.lineEdit_topic.text()
#print "_handle_topic_changed", val#self._widget.lineEdit_topic.toPlainText()
self.t.setTopic(val)
self.t.stop()
rospy.sleep(0.1)
self.t.restart()
def updateWidgets(self):
wrench = self.t.getWrench()
self._widget.doubleSpinBox_forceX.setValue(wrench.force.x)
self._widget.doubleSpinBox_forceY.setValue(wrench.force.y)
self._widget.doubleSpinBox_forceZ.setValue(wrench.force.z)
self._widget.doubleSpinBox_forceA.setValue(wrench.torque.z)
self._widget.doubleSpinBox_forceB.setValue(wrench.torque.y)
self._widget.doubleSpinBox_forceC.setValue(wrench.torque.x)
self._widget.horizontalSlider_forceX.setValue(wrench.force.x)
self._widget.horizontalSlider_forceY.setValue(wrench.force.y)
self._widget.horizontalSlider_forceZ.setValue(wrench.force.z)
self._widget.horizontalSlider_forceA.setValue(wrench.torque.z)
self._widget.horizontalSlider_forceB.setValue(wrench.torque.y)
self._widget.horizontalSlider_forceC.setValue(wrench.torque.x)
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 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 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)
