diff --git a/.flake8 b/.flake8 index b23ecc7..c05d28f 100644 --- a/.flake8 +++ b/.flake8 @@ -7,6 +7,7 @@ application-package-names = march_rqt_gait_selection, march_rqt_input_device, march_rqt_note_taker, + march_rqt_software_check, march_shared_classes, march_shared_resources import-order-style = appnexus @@ -33,4 +34,4 @@ ignore = W503, # C815: Trailing comma in Python 3.5 C815 -ignore-names = rowCount, columnCount, headerData, setUp +ignore-names = rowCount, columnCount, headerData, setUp, paintEvent diff --git a/.travis.yml b/.travis.yml index c58bdf9..c0e4ab0 100644 --- a/.travis.yml +++ b/.travis.yml @@ -27,7 +27,7 @@ env: - DOCKER_RUN_OPTS='$CI_ENV -e CATKIN_TEST_COVERAGE=1 -e NOSE_COVER_INCLUSIVE=True -e NOSE_COVER_BRANCHES=True' - AFTER_INSTALL_TARGET_DEPENDENCIES='pip install coverage' - AFTER_RUN_TARGET_TEST='cd "$target_ws" && ./src/monitor/coverage.sh && bash <(curl -s https://codecov.io/bash) -R src/monitor -F test && bash <(curl -s https://codecov.io/bash) -R src/monitor -F production' - - UPSTREAM_WORKSPACE='.rosinstall -march/march_data_collector -march/march_description -march/march_gain_scheduling -march/march_gait_scheduler -march/march_gait_selection -march/march_launch -march/march_safety -march/march_shared_classes -march/march_state_machine' + - UPSTREAM_WORKSPACE='.rosinstall -march/march_data_collector -march/march_description -march/march_gain_scheduling -march/march_gait_scheduler -march/march_gait_selection -march/march_launch -march/march_moveit -march/march_safety -march/march_shared_classes -march/march_state_machine' jobs: include: diff --git a/march_rqt_gait_selection/launch/march_rqt_gait_selection.launch b/march_rqt_gait_selection/launch/march_rqt_gait_selection.launch index d80eaee..c15215f 100644 --- a/march_rqt_gait_selection/launch/march_rqt_gait_selection.launch +++ b/march_rqt_gait_selection/launch/march_rqt_gait_selection.launch @@ -1,3 +1,3 @@ - + diff --git a/march_rqt_gait_selection/plugin.xml b/march_rqt_gait_selection/plugin.xml index cbaa745..fd61927 100644 --- a/march_rqt_gait_selection/plugin.xml +++ b/march_rqt_gait_selection/plugin.xml @@ -1,5 +1,5 @@ - + RQT plugin to configure which version of gaits will be performed at runtime. diff --git a/march_rqt_gait_selection/resource/gait_selection.ui b/march_rqt_gait_selection/resource/gait_selection.ui index 06b9974..2f38753 100644 --- a/march_rqt_gait_selection/resource/gait_selection.ui +++ b/march_rqt_gait_selection/resource/gait_selection.ui @@ -6,10 +6,22 @@ 0 0 - 873 - 588 + 1138 + 288 + + + 1 + 1 + + + + + 16777215 + 16777215 + + March Gait Selection @@ -25,90 +37,670 @@ QGroupBox::title { padding: 0 3px 0 3px; } - - - + + + - + 0 0 + + Qt::LeftToRight + + + false + QFrame::StyledPanel QFrame::Raised - + - - - Refresh + + + true - - - - - - Apply + + + 0 + 0 + - - - - - - Save as default + + + 1100 + 250 + - - - - - - false + + Qt::LeftToRight false - - #Log { -background-color: #F2F1F0 -} - - - - - - - - - - - 0 - 0 - - - - QFrame::StyledPanel - - - QFrame::Raised - - - - true - + + Qt::AlignCenter + + 0 0 - 551 - 548 + 1098 + 248 + + true + + + + true + + + + 350 + 50 + 271 + 31 + + + + + 0 + 0 + + + + + Ubuntu Condensed + + + + + + true + + + + 280 + 90 + 341 + 141 + + + + + 0 + 0 + + + + QPushButton{background: #287cbc} +QPushButton{color: #FFFFFF} + + + true + + + + + 0 + 0 + 339 + 139 + + + + + + 160 + 100 + 161 + 21 + + + + + 0 + 0 + + + + <html><head/><body><p>Save the current selected versions as default</p></body></html> + + + + + + Save as default + + + + + + 160 + 60 + 161 + 21 + + + + + 0 + 0 + + + + <html><head/><body><p>Apply the current changes, if another gait is selected the changes will be lost</p></body></html> + + + + + + Apply + + + + + + 160 + 20 + 161 + 21 + + + + + 0 + 0 + + + + <html><head/><body><p>Refresh the gait version map</p></body></html> + + + + + + Refresh + + + + + + 20 + 20 + 81 + 41 + + + + Clear +Logger + + + + + + 20 + 80 + 81 + 41 + + + + <html><head/><body><p>Create pop up to display all gaits and subgait versions</p></body></html> + + + See all +versions + + + + + + 120 + 10 + 20 + 121 + + + + Qt::Vertical + + + + + + + + 630 + 10 + 181 + 20 + + + + + 0 + 0 + + + + + true + + + + SUBGAITS + + + Qt::AlignCenter + + + + + false + + + + 10 + 50 + 241 + 181 + + + + + 0 + 0 + + + + false + + + #Log { +background-color: #F2F1F0 +} + + + + + + + + + 280 + 10 + 341 + 20 + + + + + 0 + 0 + + + + + true + + + + GAIT + + + Qt::AlignCenter + + + + + + 630 + 50 + 181 + 20 + + + + + 0 + 0 + + + + QFrame::NoFrame + + + QFrame::Plain + + + Unused + + + Qt::AlignCenter + + + + + + 630 + 90 + 181 + 20 + + + + + 0 + 0 + + + + Unused + + + Qt::AlignCenter + + + + + + 630 + 130 + 181 + 20 + + + + + 0 + 0 + + + + Unused + + + Qt::AlignCenter + + + + + + 630 + 170 + 181 + 20 + + + + + 0 + 0 + + + + Unused + + + Qt::AlignCenter + + + + + true + + + + 630 + 210 + 181 + 21 + + + + + 0 + 0 + + + + Unused + + + Qt::AlignCenter + + + + + + 820 + 10 + 271 + 20 + + + + + 0 + 0 + + + + + true + + + + VERSIONS + + + Qt::AlignCenter + + + + + + 810 + 50 + 271 + 25 + + + + + 0 + 0 + + + + + Ubuntu Condensed + + + + + + + 810 + 90 + 271 + 25 + + + + + 0 + 0 + + + + + Ubuntu Condensed + + + + + + + 810 + 130 + 271 + 25 + + + + + 0 + 0 + + + + + Ubuntu Condensed + + + + + + + 810 + 170 + 271 + 25 + + + + + 0 + 0 + + + + + Ubuntu Condensed + + + + + + + 810 + 210 + 271 + 25 + + + + + 0 + 0 + + + + + Ubuntu Condensed + + + + + + + 10 + 10 + 261 + 20 + + + + + 0 + 0 + + + + + true + + + + LOGGER + + + Qt::AlignCenter + + + + + + 10 + 30 + 1071 + 20 + + + + Qt::Horizontal + + + + + + 280 + 50 + 61 + 31 + + + + + 0 + 0 + + + + + true + + + + Select : + + + Qt::AlignCenter + + + + + + 10 + 10 + 31 + 25 + + + + <html><head/><body><p>This tool is used to set subgait-versions of a gait when the exoskeleton is running. If a change is made it has to be applied before selecting a new gait, otherwise the changes will be lost.</p></body></html> + + + QPushButton{background: #287cbc} +QPushButton{color: #FFFFFF} + + + ? + + diff --git a/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection.py b/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection.py deleted file mode 100644 index f4329e1..0000000 --- a/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection.py +++ /dev/null @@ -1,227 +0,0 @@ -import ast -from enum import Enum -import os -import sys - -from python_qt_binding import loadUi -from python_qt_binding.QtWidgets import QComboBox -from python_qt_binding.QtWidgets import QGridLayout -from python_qt_binding.QtWidgets import QGroupBox -from python_qt_binding.QtWidgets import QHBoxLayout -from python_qt_binding.QtWidgets import QLabel -from python_qt_binding.QtWidgets import QPlainTextEdit -from python_qt_binding.QtWidgets import QPushButton -from python_qt_binding.QtWidgets import QVBoxLayout -from python_qt_binding.QtWidgets import QWidget -from qt_gui.plugin import Plugin -import rospkg -import rospy - -from march_shared_resources.srv import StringTrigger, Trigger - - -class Color(Enum): - Debug = '#009100' - Info = '#000000' - Warning = '#b27300' # noqa A003 - Error = '#FF0000' - Fatal = '#FF0000' - - -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('

' + msg + '

') - 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) diff --git a/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_controller.py b/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_controller.py new file mode 100644 index 0000000..fd13a57 --- /dev/null +++ b/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_controller.py @@ -0,0 +1,55 @@ + +import ast +import sys + +import rospy + +from march_shared_resources.srv import StringTrigger, Trigger + + +class GaitSelectionController(object): + def __init__(self): + """Base class to communicate with the gait selection node.""" + 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._set_default_versions = rospy.ServiceProxy('/march/gait_selection/update_default_versions', Trigger) + + def get_version_map(self): + """Get the gait version map used in the gait selection node.""" + try: + return dict(ast.literal_eval(self._get_version_map().message)) + except ValueError: + return None + + def get_directory_structure(self): + """Get the gait directory of the selected gait_directory in the gait selection node.""" + try: + return dict(ast.literal_eval(self._get_directory_structure().message)) + except ValueError: + return None + + def set_version_map(self, gait_version_map): + """Set a new gait version map to use in the gait selection node. + + :param gait_version_map: + The new gait version map as dictionary to parse to the gait selection node + """ + result = self._set_version_map(str(gait_version_map)) + return result.success + + def set_default_versions(self): + """Save the current gait version map in the gait selection node as a default.""" + result = self._set_default_versions() + return result.success diff --git a/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_plugin.py b/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_plugin.py new file mode 100644 index 0000000..b2ac126 --- /dev/null +++ b/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_plugin.py @@ -0,0 +1,25 @@ + +import os + +from qt_gui.plugin import Plugin +import rospkg + +from .gait_selection_controller import GaitSelectionController +from .gait_selection_view import GaitSelectionView + + +class GaitSelectionPlugin(Plugin): + def __init__(self, context): + """Initiating the viewer and controller for the gait selection interface.""" + super(GaitSelectionPlugin, self).__init__(context) + + self.setObjectName('GaitSelectionPlugin') + + ui_file = os.path.join(rospkg.RosPack().get_path('march_rqt_gait_selection'), 'resource', 'gait_selection.ui') + + self._controller = GaitSelectionController() + self._widget = GaitSelectionView(ui_file, self._controller) + context.add_widget(self._widget) + + if context.serial_number() > 1: + self._widget.setWindowTitle('{0} ({1})'.format(self._widget.windowTitle(), context.serial_number())) diff --git a/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_pop_up.py b/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_pop_up.py new file mode 100644 index 0000000..9616a23 --- /dev/null +++ b/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_pop_up.py @@ -0,0 +1,43 @@ + +from PyQt5.QtCore import Qt +from PyQt5.QtWidgets import QDialog, QGridLayout, QLabel, QScrollArea, QSizePolicy, QWidget + + +class PopUpWindow(QDialog): + def __init__(self, parent, width=500, height=600): + """Base class for creating a pop up window over an existing widget. + + :param parent: + The parent widget to connect to the pop up + :param width: + Starting width of the the pop up widget + :param height: + Starting height of the the pop up widget + """ + super(PopUpWindow, self).__init__(parent=parent, flags=Qt.Window) + self.resize(width, height) + self.setSizePolicy(QSizePolicy.Minimum, QSizePolicy.Minimum) + + self._layout = QGridLayout(self) + + self._scroll_area = QScrollArea() + self._scroll_area.setVerticalScrollBarPolicy(Qt.ScrollBarAsNeeded) + self._scroll_area.setHorizontalScrollBarPolicy(Qt.ScrollBarAsNeeded) + self._scroll_area.setSizePolicy(QSizePolicy.Minimum, QSizePolicy.Minimum) + self._scroll_area.setWidgetResizable(True) + + self._content_frame = QWidget(self._scroll_area, flags=Qt.Window) + self._content_frame.setSizePolicy(QSizePolicy.Minimum, QSizePolicy.Minimum) + self._content = QGridLayout(self._content_frame) + + self.msg_label = QLabel(self) + self._content.addWidget(self.msg_label) + + self._scroll_area.setWidget(self._content_frame) + self._layout.addWidget(self._scroll_area) + + def show_message(self, message): + """Add message to the pop up and show the window.""" + self.msg_label.clear() + self.msg_label.setText(message) + return super(PopUpWindow, self).show() diff --git a/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py b/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py new file mode 100644 index 0000000..0bd0967 --- /dev/null +++ b/march_rqt_gait_selection/src/march_rqt_gait_selection/gait_selection_view.py @@ -0,0 +1,218 @@ +from PyQt5.QtCore import Qt +from PyQt5.QtWidgets import QWidget +from python_qt_binding import loadUi +import rospy + +from .gait_selection_pop_up import PopUpWindow + + +AMOUNT_OF_AVAILABLE_SUBGAITS = 5 + + +class GaitSelectionView(QWidget): + def __init__(self, ui_file, controller): + """Base class to load and use the gait_selection.ui. + + :param ui_file: + Path to the .ui file to load with the widget + :param controller: + A reference to the controller corresponding to the rqt gait selection functionality + """ + super(GaitSelectionView, self).__init__(flags=Qt.Window) + + self._controller = controller + + # Extend the widget with all attributes and children from UI file + loadUi(ui_file, self) + + # Coding flags + self._colors = {'info': '#000000', 'success': '#009100', 'error': '#FF0000', 'warning': '#b27300'} + self._is_refresh_active = True + self._is_update_active = True + + # Search for children in GUI + self._gait_content = self.GaitSelectionInterface + self._gait_menu = self.GaitMenu + self._logger = self.Log + + self._subgait_labels = [] + self._subgait_menus = [] + + for index in range(AMOUNT_OF_AVAILABLE_SUBGAITS): + self._subgait_labels.append(getattr(self, 'SubgaitLabel_{nr}'.format(nr=index))) + self._subgait_menus.append(getattr(self, 'SubgaitMenu_{nr}'.format(nr=index))) + + # bind functions to callbacks of buttons and menus + self.Refresh.pressed.connect(lambda: self._refresh()) + self.Apply.pressed.connect(lambda: [self._apply(), self._refresh()]) + self.SaveDefault.pressed.connect(lambda: [self._apply(), self._save_default(), self._refresh()]) + + self.ClearLogger.clicked.connect(lambda: self._logger.clear()) + self.SeeAllVersions.clicked.connect(lambda: self._show_version_map_pop_up()) + + self._gait_menu.currentIndexChanged.connect(lambda: self.update_version_menus()) + for subgait_menu in self._subgait_menus: + subgait_menu.currentIndexChanged.connect(lambda: self.update_version_colors()) + + # loaded gaits from gait selection node + self.available_gaits = {} + self.version_map = {} + + # pop up window + self._version_map_pop_up = PopUpWindow(self) + + # populate gait menu for the first time + self._refresh() + + # gait and subgait related layout functions + def update_version_menus(self): + """When a gait is selected set the subgait labels and populate the subgait menus with the available versions.""" + self._is_update_active = True + if self._is_refresh_active: + return + + self._clear_gui() + + gait_name = self._gait_menu.currentText() + subgaits = self.available_gaits[gait_name]['subgaits'] + + latest_used_index = 0 + for index, (subgait_name, versions) in enumerate(subgaits.items()): + + try: + subgait_menu = self._subgait_menus[index] + subgait_label = self._subgait_labels[index] + except index: + rospy.logfatal('Not enough labels and menus available in GUI, change layout using Qt tool.') + return + + subgait_menu.setDisabled(0) + subgait_label.setDisabled(0) + + subgait_label.setText(subgait_name) + subgait_menu.addItems(versions) + + try: + current_version = self.version_map[gait_name][subgait_name] + current_version_index = versions.index(current_version) + subgait_menu.setCurrentIndex(current_version_index) + + except ValueError: + self._log('Default version of {gn} {sgn} does not exist in loaded gaits.' + .format(gn=gait_name, sgn=subgait_name), 'error') + except KeyError: + self._log('{gn} has no default version for {sgn}.' + .format(gn=gait_name, sgn=subgait_name), 'error') + + latest_used_index = index + 1 + + for unused_index in range(latest_used_index, AMOUNT_OF_AVAILABLE_SUBGAITS): + self._subgait_labels[unused_index].setDisabled(1) + self._subgait_menus[unused_index].setDisabled(1) + + self._is_update_active = False + + def update_selected_versions(self): + """Read the subgait version menus and save the newly selected versions to the local version map.""" + gait_name = self._gait_menu.currentText() + + for subgait_label, subgait_menu in zip(self._subgait_labels, self._subgait_menus): + subgait_name = subgait_label.text() + if subgait_name != 'Unused': + try: + self.version_map[gait_name][subgait_name] = subgait_menu.currentText() + except KeyError: + pass + + def update_version_colors(self): + """Update the subgait labels with a specific color to represent a change in version.""" + if self._is_update_active or self._is_refresh_active: + return + + gait_name = self._gait_menu.currentText() + for subgait_label, subgait_menu in zip(self._subgait_labels, self._subgait_menus): + subgait_name = subgait_label.text() + if subgait_name != 'Unused': + try: + if str(self.version_map[gait_name][subgait_name]) != str(subgait_menu.currentText()): + subgait_label.setStyleSheet('color:{color}'.format(color=self._colors['warning'])) + else: + subgait_label.setStyleSheet('color:{color}'.format(color=self._colors['info'])) + except KeyError: + pass + + def _refresh(self): + """Request the gait map from the gait selection node and display the available gaits in the gait menu.""" + self._is_refresh_active = True + self._clear_gui(clear_gait_menu=True) + + self.available_gaits = self._controller.get_directory_structure() + self.version_map = self._controller.get_version_map() + + self._gait_menu.addItems(sorted(self.available_gaits.keys())) + + self._log('Directory data refreshed', 'success') + self._is_refresh_active = False + + # logger + def _log(self, msg, color_tag='info'): + """Use the logger window in the GUI to display data. + + :param msg: + The message to display in the widget + :param color_tag: + The tag which represents the color of the text in the screen (info, warning, error) + """ + try: + color = self._colors[color_tag] + except KeyError: + color = self._colors['error'] + + self._logger.appendHtml('

' + msg + '

') + scrollbar = self.Log.verticalScrollBar() + scrollbar.setValue(scrollbar.maximum()) + + # button functions + def _clear_gui(self, clear_gait_menu=False): + """Clear the subgait menus and subgait labels and optionally the gait menu. + + :param clear_gait_menu: + Set this to true if the gait menu should also be cleared + """ + for subgait_menu in self._subgait_menus: + subgait_menu.clear() + + for subgait_label in self._subgait_labels: + subgait_label.setStyleSheet('color:#000000') + subgait_label.setText('Unused') + + if clear_gait_menu: + self._gait_menu.clear() + + def _save_default(self): + """Save the currently selected subgait versions as default values.""" + if self._controller.set_default_versions(): + self._log('Set new default versions', 'success') + else: + self._log('Set new default versions failed', 'error') + + def _apply(self): + """Apply newly selected subgait versions to the gait selection node.""" + self.update_selected_versions() + if self._controller.set_version_map(self.version_map): + self._log('Version change applied', 'success') + else: + self._log('Version change applied failed', 'failed') + + def _show_version_map_pop_up(self): + """Use a pop up window to display all the gait, subgaits and currently used versions.""" + version_map = self._controller.get_version_map() + version_map_string = '' + + for gait_name in sorted(version_map.keys()): + version_map_string += '{gait} \n'.format(gait=gait_name) + for subgait_name, version in version_map[gait_name].items(): + version_map_string += '\t{sb:<30} \t {vs} \n'.format(sb=subgait_name, vs=version) + version_map_string += '\n' + + self._version_map_pop_up.show_message(version_map_string) diff --git a/march_rqt_input_device/CMakeLists.txt b/march_rqt_input_device/CMakeLists.txt index d44d4f4..2a292dd 100644 --- a/march_rqt_input_device/CMakeLists.txt +++ b/march_rqt_input_device/CMakeLists.txt @@ -1,10 +1,10 @@ cmake_minimum_required(VERSION 2.8.3) project(march_rqt_input_device) -find_package(catkin REQUIRED) +find_package(catkin REQUIRED COMPONENTS march_shared_resources std_msgs) catkin_python_setup() -catkin_package() +catkin_package(CATKIN_DEPENDS march_shared_resources std_msgs) install(FILES plugin.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} diff --git a/march_rqt_input_device/launch/march_rqt_input_device.launch b/march_rqt_input_device/launch/march_rqt_input_device.launch index 96f9ac6..2484bc6 100644 --- a/march_rqt_input_device/launch/march_rqt_input_device.launch +++ b/march_rqt_input_device/launch/march_rqt_input_device.launch @@ -1,7 +1,7 @@ - + diff --git a/march_rqt_input_device/package.xml b/march_rqt_input_device/package.xml index b7a7565..905ab70 100644 --- a/march_rqt_input_device/package.xml +++ b/march_rqt_input_device/package.xml @@ -9,6 +9,9 @@ catkin + std_msgs + march_shared_resources + rospy rqt_gui rqt_gui_py diff --git a/march_rqt_input_device/plugin.xml b/march_rqt_input_device/plugin.xml index 3192e6c..51f9f89 100644 --- a/march_rqt_input_device/plugin.xml +++ b/march_rqt_input_device/plugin.xml @@ -1,5 +1,5 @@ - + An example Python GUI plugin to create a great user interface. @@ -9,4 +9,4 @@ Great user interface to provide real value. - \ No newline at end of file + diff --git a/march_rqt_input_device/resource/img/continue.png b/march_rqt_input_device/resource/img/continue.png new file mode 100644 index 0000000..84b3e9a Binary files /dev/null and b/march_rqt_input_device/resource/img/continue.png differ diff --git a/march_rqt_input_device/resource/img/gait_ramp_door_last_step.png b/march_rqt_input_device/resource/img/gait_ramp_door_last_step.png new file mode 100644 index 0000000..d839dc7 Binary files /dev/null and b/march_rqt_input_device/resource/img/gait_ramp_door_last_step.png differ diff --git a/march_rqt_input_device/resource/img/gait_ramp_door_slope_down.png b/march_rqt_input_device/resource/img/gait_ramp_door_slope_down.png new file mode 100644 index 0000000..c7e361a Binary files /dev/null and b/march_rqt_input_device/resource/img/gait_ramp_door_slope_down.png differ diff --git a/march_rqt_input_device/resource/img/gait_ramp_door_slope_up.png b/march_rqt_input_device/resource/img/gait_ramp_door_slope_up.png new file mode 100644 index 0000000..cdc4dff Binary files /dev/null and b/march_rqt_input_device/resource/img/gait_ramp_door_slope_up.png differ diff --git a/march_rqt_input_device/resource/img/gait_rough_terrain_first_middle_step.png b/march_rqt_input_device/resource/img/gait_rough_terrain_first_middle_step.png new file mode 100644 index 0000000..3f695ca Binary files /dev/null and b/march_rqt_input_device/resource/img/gait_rough_terrain_first_middle_step.png differ diff --git a/march_rqt_input_device/resource/img/gait_rough_terrain_high_step.png b/march_rqt_input_device/resource/img/gait_rough_terrain_high_step.png new file mode 100644 index 0000000..4d23c3b Binary files /dev/null and b/march_rqt_input_device/resource/img/gait_rough_terrain_high_step.png differ diff --git a/march_rqt_input_device/resource/img/gait_rough_terrain_second_middle_step.png b/march_rqt_input_device/resource/img/gait_rough_terrain_second_middle_step.png new file mode 100644 index 0000000..57a7984 Binary files /dev/null and b/march_rqt_input_device/resource/img/gait_rough_terrain_second_middle_step.png differ diff --git a/march_rqt_input_device/resource/img/gait_rough_terrain_third_middle_step.png b/march_rqt_input_device/resource/img/gait_rough_terrain_third_middle_step.png new file mode 100644 index 0000000..4a2482a Binary files /dev/null and b/march_rqt_input_device/resource/img/gait_rough_terrain_third_middle_step.png differ diff --git a/march_rqt_input_device/resource/img/gait_single_step_medium.png b/march_rqt_input_device/resource/img/gait_single_step_medium.png new file mode 100644 index 0000000..036356c Binary files /dev/null and b/march_rqt_input_device/resource/img/gait_single_step_medium.png differ diff --git a/march_rqt_input_device/resource/img/gait_single_step_normal.png b/march_rqt_input_device/resource/img/gait_single_step_normal.png deleted file mode 100644 index 326774e..0000000 Binary files a/march_rqt_input_device/resource/img/gait_single_step_normal.png and /dev/null differ diff --git a/march_rqt_input_device/resource/img/gait_single_step_small.png b/march_rqt_input_device/resource/img/gait_single_step_small.png index 97a3288..eb43469 100644 Binary files a/march_rqt_input_device/resource/img/gait_single_step_small.png and b/march_rqt_input_device/resource/img/gait_single_step_small.png differ diff --git a/march_rqt_input_device/resource/img/gait_sit.png b/march_rqt_input_device/resource/img/gait_sit.png index 315ff52..71886c4 100644 Binary files a/march_rqt_input_device/resource/img/gait_sit.png and b/march_rqt_input_device/resource/img/gait_sit.png differ diff --git a/march_rqt_input_device/resource/img/gait_slope_down.png b/march_rqt_input_device/resource/img/gait_slope_down.png deleted file mode 100644 index 54ebd69..0000000 Binary files a/march_rqt_input_device/resource/img/gait_slope_down.png and /dev/null differ diff --git a/march_rqt_input_device/resource/img/gait_slope_down_final_step.png b/march_rqt_input_device/resource/img/gait_slope_down_final_step.png deleted file mode 100644 index fa6fd8e..0000000 Binary files a/march_rqt_input_device/resource/img/gait_slope_down_final_step.png and /dev/null differ diff --git a/march_rqt_input_device/resource/img/gait_slope_up.png b/march_rqt_input_device/resource/img/gait_slope_up.png deleted file mode 100644 index 6110ab7..0000000 Binary files a/march_rqt_input_device/resource/img/gait_slope_up.png and /dev/null differ diff --git a/march_rqt_input_device/resource/img/gait_sofa_sit.png b/march_rqt_input_device/resource/img/gait_sofa_sit.png index 2b31cc7..637766b 100644 Binary files a/march_rqt_input_device/resource/img/gait_sofa_sit.png and b/march_rqt_input_device/resource/img/gait_sofa_sit.png differ diff --git a/march_rqt_input_device/resource/img/gait_sofa_stand.png b/march_rqt_input_device/resource/img/gait_sofa_stand.png new file mode 100644 index 0000000..6fc70f6 Binary files /dev/null and b/march_rqt_input_device/resource/img/gait_sofa_stand.png differ diff --git a/march_rqt_input_device/resource/img/gait_sofa_stand_up.png b/march_rqt_input_device/resource/img/gait_sofa_stand_up.png deleted file mode 100644 index ac89395..0000000 Binary files a/march_rqt_input_device/resource/img/gait_sofa_stand_up.png and /dev/null differ diff --git a/march_rqt_input_device/resource/img/gait_stairs_down.png b/march_rqt_input_device/resource/img/gait_stairs_down.png index 85a0b20..8bb0d32 100644 Binary files a/march_rqt_input_device/resource/img/gait_stairs_down.png and b/march_rqt_input_device/resource/img/gait_stairs_down.png differ diff --git a/march_rqt_input_device/resource/img/gait_stairs_down_final_step.png b/march_rqt_input_device/resource/img/gait_stairs_down_final_step.png deleted file mode 100644 index 818f694..0000000 Binary files a/march_rqt_input_device/resource/img/gait_stairs_down_final_step.png and /dev/null differ diff --git a/march_rqt_input_device/resource/img/gait_stairs_down_single_step.png b/march_rqt_input_device/resource/img/gait_stairs_down_single_step.png new file mode 100644 index 0000000..de9dd78 Binary files /dev/null and b/march_rqt_input_device/resource/img/gait_stairs_down_single_step.png differ diff --git a/march_rqt_input_device/resource/img/gait_stairs_up.png b/march_rqt_input_device/resource/img/gait_stairs_up.png index 6cc2bd7..9be5fe3 100644 Binary files a/march_rqt_input_device/resource/img/gait_stairs_up.png and b/march_rqt_input_device/resource/img/gait_stairs_up.png differ diff --git a/march_rqt_input_device/resource/img/gait_stairs_up_single_step.png b/march_rqt_input_device/resource/img/gait_stairs_up_single_step.png new file mode 100644 index 0000000..dcf3435 Binary files /dev/null and b/march_rqt_input_device/resource/img/gait_stairs_up_single_step.png differ diff --git a/march_rqt_input_device/resource/img/gait_stand.png b/march_rqt_input_device/resource/img/gait_stand.png index 8ad5f1f..338a963 100644 Binary files a/march_rqt_input_device/resource/img/gait_stand.png and b/march_rqt_input_device/resource/img/gait_stand.png differ diff --git a/march_rqt_input_device/resource/img/gait_tilted_path_left_single_step.png b/march_rqt_input_device/resource/img/gait_tilted_path_left_single_step.png new file mode 100644 index 0000000..713f359 Binary files /dev/null and b/march_rqt_input_device/resource/img/gait_tilted_path_left_single_step.png differ diff --git a/march_rqt_input_device/resource/img/gait_tilted_path_left_straight_end.png b/march_rqt_input_device/resource/img/gait_tilted_path_left_straight_end.png new file mode 100644 index 0000000..01b6520 Binary files /dev/null and b/march_rqt_input_device/resource/img/gait_tilted_path_left_straight_end.png differ diff --git a/march_rqt_input_device/resource/img/gait_tilted_path_left_straight_start.png b/march_rqt_input_device/resource/img/gait_tilted_path_left_straight_start.png new file mode 100644 index 0000000..de35487 Binary files /dev/null and b/march_rqt_input_device/resource/img/gait_tilted_path_left_straight_start.png differ diff --git a/march_rqt_input_device/resource/img/gait_tilted_path_right_single_step.png b/march_rqt_input_device/resource/img/gait_tilted_path_right_single_step.png new file mode 100644 index 0000000..c25609e Binary files /dev/null and b/march_rqt_input_device/resource/img/gait_tilted_path_right_single_step.png differ diff --git a/march_rqt_input_device/resource/img/gait_tilted_path_right_straight_end.png b/march_rqt_input_device/resource/img/gait_tilted_path_right_straight_end.png new file mode 100644 index 0000000..ac4a1de Binary files /dev/null and b/march_rqt_input_device/resource/img/gait_tilted_path_right_straight_end.png differ diff --git a/march_rqt_input_device/resource/img/gait_tilted_path_right_straight_start.png b/march_rqt_input_device/resource/img/gait_tilted_path_right_straight_start.png new file mode 100644 index 0000000..4edad25 Binary files /dev/null and b/march_rqt_input_device/resource/img/gait_tilted_path_right_straight_start.png differ diff --git a/march_rqt_input_device/resource/img/gait_walk.png b/march_rqt_input_device/resource/img/gait_walk.png new file mode 100644 index 0000000..98e46f6 Binary files /dev/null and b/march_rqt_input_device/resource/img/gait_walk.png differ diff --git a/march_rqt_input_device/resource/img/gait_walk_large.png b/march_rqt_input_device/resource/img/gait_walk_large.png new file mode 100644 index 0000000..6378d19 Binary files /dev/null and b/march_rqt_input_device/resource/img/gait_walk_large.png differ diff --git a/march_rqt_input_device/resource/img/gait_walk_normal.png b/march_rqt_input_device/resource/img/gait_walk_normal.png deleted file mode 100644 index 8b9e8a0..0000000 Binary files a/march_rqt_input_device/resource/img/gait_walk_normal.png and /dev/null differ diff --git a/march_rqt_input_device/resource/img/gait_walk_small.png b/march_rqt_input_device/resource/img/gait_walk_small.png index 1a6777e..a55a943 100644 Binary files a/march_rqt_input_device/resource/img/gait_walk_small.png and b/march_rqt_input_device/resource/img/gait_walk_small.png differ diff --git a/march_rqt_input_device/resource/img/home_sit.png b/march_rqt_input_device/resource/img/home_sit.png index 75bf1a8..f8f4656 100644 Binary files a/march_rqt_input_device/resource/img/home_sit.png and b/march_rqt_input_device/resource/img/home_sit.png differ diff --git a/march_rqt_input_device/resource/img/home_stand.png b/march_rqt_input_device/resource/img/home_stand.png index 8575212..440e998 100644 Binary files a/march_rqt_input_device/resource/img/home_stand.png and b/march_rqt_input_device/resource/img/home_stand.png differ diff --git a/march_rqt_input_device/resource/img/off.png b/march_rqt_input_device/resource/img/off.png deleted file mode 100644 index ba7bc8e..0000000 Binary files a/march_rqt_input_device/resource/img/off.png and /dev/null differ diff --git a/march_rqt_input_device/resource/img/pause.png b/march_rqt_input_device/resource/img/pause.png new file mode 100644 index 0000000..b4903ef Binary files /dev/null and b/march_rqt_input_device/resource/img/pause.png differ diff --git a/march_rqt_input_device/resource/img/rocker_switch_down.png b/march_rqt_input_device/resource/img/rocker_switch_down.png new file mode 100644 index 0000000..2b4272d Binary files /dev/null and b/march_rqt_input_device/resource/img/rocker_switch_down.png differ diff --git a/march_rqt_input_device/resource/img/rocker_switch_up.png b/march_rqt_input_device/resource/img/rocker_switch_up.png new file mode 100644 index 0000000..d10248b Binary files /dev/null and b/march_rqt_input_device/resource/img/rocker_switch_up.png differ diff --git a/march_rqt_input_device/resource/input_device.ui b/march_rqt_input_device/resource/input_device.ui index 1a6b61c..494b60a 100644 --- a/march_rqt_input_device/resource/input_device.ui +++ b/march_rqt_input_device/resource/input_device.ui @@ -6,34 +6,94 @@ 0 0 - 400 - 400 + 890 + 549 March Input Device - + + + QLayout::SetMinAndMaxSize + + + + + + + + 16777215 + 302 + + + + Idle + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + + + + + <html><head/><body><p>Refreshes the buttons with possible gaits</p></body></html> + + + Refresh + + + + + - - - 0 - 0 - + + true + + + Qt::NoFocus - true + false 0 0 - 380 - 380 + 870 + 493 + + QPushButton { + background-color: #B6BDC9; + color: #000000; +} + +QPushButton:disabled { + background-color: #80858D; +} + diff --git a/march_rqt_input_device/src/march_rqt_input_device/image_button.py b/march_rqt_input_device/src/march_rqt_input_device/image_button.py new file mode 100644 index 0000000..a9db58a --- /dev/null +++ b/march_rqt_input_device/src/march_rqt_input_device/image_button.py @@ -0,0 +1,15 @@ +from PyQt5.QtGui import QColor, QPainter, QPixmap +from PyQt5.QtWidgets import QAbstractButton + + +class ImageButton(QAbstractButton): + def __init__(self, image_path): + super(ImageButton, self).__init__() + self._pixmap = QPixmap(image_path) + + def paintEvent(self, event): + painter = QPainter(self) + painter.drawPixmap(0, 0, self._pixmap) + if not self.isEnabled(): + painter.setOpacity(0.3) + painter.fillRect(event.rect(), QColor(0, 0, 0)) diff --git a/march_rqt_input_device/src/march_rqt_input_device/input_device_controller.py b/march_rqt_input_device/src/march_rqt_input_device/input_device_controller.py index 22128e4..8778078 100644 --- a/march_rqt_input_device/src/march_rqt_input_device/input_device_controller.py +++ b/march_rqt_input_device/src/march_rqt_input_device/input_device_controller.py @@ -1,59 +1,125 @@ +import getpass +import socket + import rospy -import std_msgs.msg +from std_msgs.msg import Header, String -from march_shared_resources.msg import Error, GaitInstruction +from march_shared_resources.msg import Alive, Error, GaitInstruction, GaitInstructionResponse +from march_shared_resources.srv import PossibleGaits class InputDeviceController(object): - def __init__(self): - self.instruction_gait_pub = rospy.Publisher('/march/input_device/instruction', GaitInstruction, queue_size=10) - self.error_pub = rospy.Publisher('/march/error', Error, queue_size=10) - self._ping = rospy.get_param('~ping_safety_node', True) + # Format of the identifier for the alive message + ID_FORMAT = 'rqt@{machine}@{user}' + + def __init__(self, ping): + self._instruction_gait_pub = rospy.Publisher('/march/input_device/instruction', GaitInstruction, queue_size=10) + self._instruction_response_pub = rospy.Subscriber('/march/input_device/instruction_response', + GaitInstructionResponse, callback=self._response_callback, + queue_size=10) + self._current_gait = rospy.Subscriber('/march/gait/current', String, + callback=self._current_gait_callback, queue_size=1) + self._error_pub = rospy.Publisher('/march/error', Error, queue_size=10) + self._get_possible_gaits = rospy.ServiceProxy('/march/state_machine/get_possible_gaits', PossibleGaits) + + self.accepted_cb = None + self.finished_cb = None + self.rejected_cb = None + self.current_gait_cb = None + + self._ping = ping + self._id = self.ID_FORMAT.format(machine=socket.gethostname(), + user=getpass.getuser()) if self._ping: - self._alive_pub = rospy.Publisher('/march/input_device/alive', std_msgs.msg.Time, queue_size=10) - self._alive_timer = rospy.Timer(rospy.Duration(0.05), self._timer_callback) + self._alive_pub = rospy.Publisher('/march/input_device/alive', Alive, queue_size=10) + period = rospy.Duration().from_sec(0.2) + self._alive_timer = rospy.Timer(period, self._timer_callback) - def shutdown_plugin(self): + def __del__(self): + self._instruction_gait_pub.unregister() + self._error_pub.unregister() if self._ping: self._alive_timer.shutdown() + self._alive_timer.join() self._alive_pub.unregister() + def _response_callback(self, msg): + """ + Callback for instruction response messages. + + Calls registered callbacks when the gait is accepted, finished or rejected. + + :type msg: GaitInstructionResponse + """ + if msg.result == GaitInstructionResponse.GAIT_ACCEPTED: + if callable(self.accepted_cb): + self.accepted_cb() + elif msg.result == GaitInstructionResponse.GAIT_FINISHED: + if callable(self.finished_cb): + self.finished_cb() + elif msg.result == GaitInstructionResponse.GAIT_REJECTED: + if callable(self.rejected_cb): + self.rejected_cb() + + def _current_gait_callback(self, msg): + if callable(self.current_gait_cb): + self.current_gait_cb(msg.data) + def _timer_callback(self, event): - self._alive_pub.publish(event.current_real) + msg = Alive(stamp=event.current_real, id=self._id) + self._alive_pub.publish(msg) + + def get_possible_gaits(self): + """ + Returns a list of names of possible gaits. + + :rtype: list(str) + :return: List of possible gaits + """ + try: + return self._get_possible_gaits().gaits + except rospy.ServiceException: + rospy.logwarn('Failed to contact get_possible_gaits service') + return [] def publish_increment_step_size(self): rospy.logdebug('Mock Input Device published step size increment') - self.instruction_gait_pub.publish(GaitInstruction(std_msgs.msg.Header(stamp=rospy.Time.now()), - GaitInstruction.INCREMENT_STEP_SIZE, '')) + self._instruction_gait_pub.publish(GaitInstruction(Header(stamp=rospy.Time.now()), + GaitInstruction.INCREMENT_STEP_SIZE, '', self._id)) def publish_decrement_step_size(self): rospy.logdebug('Mock Input Device published step size decrement') - self.instruction_gait_pub.publish(GaitInstruction(std_msgs.msg.Header(stamp=rospy.Time.now()), - GaitInstruction.DECREMENT_STEP_SIZE, '')) + self._instruction_gait_pub.publish(GaitInstruction(Header(stamp=rospy.Time.now()), + GaitInstruction.DECREMENT_STEP_SIZE, '', self._id)) def publish_gait(self, string): rospy.logdebug('Mock Input Device published gait: ' + string) - self.instruction_gait_pub.publish(GaitInstruction(std_msgs.msg.Header(stamp=rospy.Time.now()), - GaitInstruction.GAIT, string)) + self._instruction_gait_pub.publish(GaitInstruction(Header(stamp=rospy.Time.now()), + GaitInstruction.GAIT, string, self._id)) def publish_stop(self): rospy.logdebug('Mock Input Device published stop') - self.instruction_gait_pub.publish(GaitInstruction(std_msgs.msg.Header(stamp=rospy.Time.now()), - GaitInstruction.STOP, '')) + self._instruction_gait_pub.publish(GaitInstruction(Header(stamp=rospy.Time.now()), + GaitInstruction.STOP, '', self._id)) def publish_continue(self): rospy.logdebug('Mock Input Device published continue') - self.instruction_gait_pub.publish(GaitInstruction(std_msgs.msg.Header(stamp=rospy.Time.now()), - GaitInstruction.CONTINUE, '')) + self._instruction_gait_pub.publish(GaitInstruction(Header(stamp=rospy.Time.now()), + GaitInstruction.CONTINUE, '', self._id)) def publish_pause(self): rospy.logdebug('Mock Input Device published pause') - self.instruction_gait_pub.publish(GaitInstruction(std_msgs.msg.Header(stamp=rospy.Time.now()), - GaitInstruction.PAUSE, '')) + self._instruction_gait_pub.publish(GaitInstruction(Header(stamp=rospy.Time.now()), + GaitInstruction.PAUSE, '', self._id)) def publish_error(self): rospy.logdebug('Mock Input Device published error') - self.error_pub.publish(Error(std_msgs.msg.Header(stamp=rospy.Time.now()), - 'Fake error thrown by the develop input device.', Error.FATAL)) + self._error_pub.publish(Error(Header(stamp=rospy.Time.now()), + 'Fake error thrown by the develop input device.', Error.FATAL)) + + def publish_sm_to_unknown(self): + rospy.logdebug('Mock Input Device published state machine to unknown') + self._instruction_gait_pub.publish(GaitInstruction(Header(stamp=rospy.Time.now()), + GaitInstruction.UNKNOWN, '', self._id)) diff --git a/march_rqt_input_device/src/march_rqt_input_device/input_device_plugin.py b/march_rqt_input_device/src/march_rqt_input_device/input_device_plugin.py new file mode 100644 index 0000000..560bd68 --- /dev/null +++ b/march_rqt_input_device/src/march_rqt_input_device/input_device_plugin.py @@ -0,0 +1,26 @@ +import os + +from qt_gui.plugin import Plugin +import rospkg +import rospy + +from .input_device_controller import InputDeviceController +from .input_device_view import InputDeviceView + + +class InputDevicePlugin(Plugin): + def __init__(self, context): + super(InputDevicePlugin, self).__init__(context) + + self.setObjectName('InputDevicePlugin') + + ping = rospy.get_param('~ping_safety_node', True) + ui_file = os.path.join(rospkg.RosPack().get_path('march_rqt_input_device'), 'resource', 'input_device.ui') + + self._controller = InputDeviceController(ping) + self._widget = InputDeviceView(ui_file, self._controller) + context.add_widget(self._widget) + + # Show _widget.windowTitle on left-top of each plugin (when it's set in _widget). (useful for multiple windows) + if context.serial_number() > 1: + self._widget.setWindowTitle('{0} ({1})'.format(self._widget.windowTitle(), context.serial_number())) diff --git a/march_rqt_input_device/src/march_rqt_input_device/input_device_view.py b/march_rqt_input_device/src/march_rqt_input_device/input_device_view.py index ccf4c61..9d653eb 100644 --- a/march_rqt_input_device/src/march_rqt_input_device/input_device_view.py +++ b/march_rqt_input_device/src/march_rqt_input_device/input_device_view.py @@ -1,230 +1,262 @@ import os - from python_qt_binding import loadUi from python_qt_binding.QtCore import QSize from python_qt_binding.QtWidgets import QGridLayout from python_qt_binding.QtWidgets import QPushButton from python_qt_binding.QtWidgets import QWidget -from qt_gui.plugin import Plugin import rospkg -from march_rqt_input_device.input_device_controller import InputDeviceController - +from .image_button import ImageButton -class InputDevicePlugin(Plugin): - def __init__(self, context): - super(InputDevicePlugin, self).__init__(context) - self.controller = InputDeviceController() - # region initialising the widget - self.setObjectName('InputDevicePlugin') +class InputDeviceView(QWidget): + def __init__(self, ui_file, controller): + """ + Initializes the view with a UI file and controller. - # Create QWidget - self._widget = QWidget() + :type ui_file: str + :param ui_file: path to a Qt UI file + :type controller: InputDeviceController + :param controller: input device controller for sending ROS messages + """ + super(InputDeviceView, self).__init__() + self._controller = controller + self._controller.accepted_cb = self._accepted_cb + self._controller.finished_cb = self._finished_cb + self._controller.rejected_cb = self._rejected_cb + self._controller.current_gait_cb = self._current_gait_cb - # 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_input_device'), 'resource', 'input_device.ui') + self._always_enabled_buttons = [] # Extend the widget with all attributes and children from UI file - loadUi(ui_file, self._widget) + loadUi(ui_file, self) - # Give QObjects reasonable names - self._widget.setObjectName('Input Device') + self.refresh_button.clicked.connect(self._update_possible_gaits) - # Show _widget.windowTitle on left-top of each plugin (when it's set in _widget). (useful for multiple windows) - if context.serial_number() > 1: - self._widget.setWindowTitle('{0} ({1})'.format(self._widget.windowTitle(), context.serial_number())) - - # Add widget to the user interface - context.add_widget(self._widget) + self._create_buttons() + self._update_possible_gaits() + def _create_buttons(self): # Create buttons here rocker_switch_increment = \ - self.create_button('rocker_switch_up', color_code='#239b56', - callback=lambda: self.controller.publish_increment_step_size()) + self.create_button('rocker_switch_up', image_path='/rocker_switch_up.png', + callback=lambda: self._controller.publish_increment_step_size(), + always_enabled=True) rocker_switch_decrement = \ - self.create_button('rocker_switch_down', color_code='#239b56', - callback=lambda: self.controller.publish_decrement_step_size()) + self.create_button('rocker_switch_down', image_path='/rocker_switch_down.png', + callback=lambda: self._controller.publish_decrement_step_size(), + always_enabled=True) home_sit = \ self.create_button('home_sit', image_path='/home_sit.png', - callback=lambda: self.controller.publish_gait('home_sit')) + callback=lambda: self._controller.publish_gait('home_sit')) home_stand = \ self.create_button('home_stand', image_path='/home_stand.png', - callback=lambda: self.controller.publish_gait('home_stand')) + callback=lambda: self._controller.publish_gait('home_stand')) gait_sit = \ self.create_button('gait_sit', image_path='/gait_sit.png', - callback=lambda: self.controller.publish_gait('gait_sit')) + callback=lambda: self._controller.publish_gait('gait_sit')) gait_walk = \ - self.create_button('gait_walk', image_path='/gait_walk_normal.png', - callback=lambda: self.controller.publish_gait('gait_walk')) + self.create_button('gait_walk', image_path='/gait_walk.png', + callback=lambda: self._controller.publish_gait('gait_walk')) gait_walk_small = \ self.create_button('gait_walk_small', image_path='/gait_walk_small.png', - callback=lambda: self.controller.publish_gait('gait_walk_small')) + callback=lambda: self._controller.publish_gait('gait_walk_small')) gait_walk_large = \ - self.create_button('gait_walk_large', - callback=lambda: self.controller.publish_gait('gait_walk_large')) + self.create_button('gait_walk_large', image_path='/gait_walk_large.png', + callback=lambda: self._controller.publish_gait('gait_walk_large')) + gait_balanced_walk = \ + self.create_button('gait_balanced_walk', + callback=lambda: self._controller.publish_gait('gait_balanced_walk')) gait_single_step_small = \ self.create_button('gait_single_step_small', image_path='/gait_single_step_small.png', - callback=lambda: self.controller.publish_gait('gait_single_step_small')) + callback=lambda: self._controller.publish_gait('gait_single_step_small')) gait_single_step_normal = \ - self.create_button('gait_single_step_normal', image_path='/gait_single_step_normal.png', - callback=lambda: self.controller.publish_gait('gait_single_step_normal')) + self.create_button('gait_single_step_normal', image_path='/gait_single_step_medium.png', + callback=lambda: self._controller.publish_gait('gait_single_step_normal')) gait_side_step_left = \ self.create_button('gait_side_step_left', image_path='/gait_side_step_left.png', - callback=lambda: self.controller.publish_gait('gait_side_step_left')) + callback=lambda: self._controller.publish_gait('gait_side_step_left')) gait_side_step_right = \ self.create_button('gait_side_step_right', image_path='/gait_side_step_right.png', - callback=lambda: self.controller.publish_gait('gait_side_step_right')) + callback=lambda: self._controller.publish_gait('gait_side_step_right')) gait_side_step_left_small = \ self.create_button('gait_side_step_left_small', - callback=lambda: self.controller.publish_gait('gait_side_step_left_small')) + callback=lambda: self._controller.publish_gait('gait_side_step_left_small')) gait_side_step_right_small = \ self.create_button('gait_side_step_right_small', - callback=lambda: self.controller.publish_gait('gait_side_step_right_small')) + callback=lambda: self._controller.publish_gait('gait_side_step_right_small')) gait_stand = \ self.create_button('gait_stand', image_path='/gait_stand.png', - callback=lambda: self.controller.publish_gait('gait_stand')) + callback=lambda: self._controller.publish_gait('gait_stand')) gait_sofa_stand = \ - self.create_button('gait_sofa_stand', image_path='/gait_sofa_stand_up.png', - callback=lambda: self.controller.publish_gait('gait_sofa_stand')) + self.create_button('gait_sofa_stand', image_path='/gait_sofa_stand.png', + callback=lambda: self._controller.publish_gait('gait_sofa_stand')) gait_sofa_sit = \ self.create_button('gait_sofa_sit', image_path='/gait_sofa_sit.png', - callback=lambda: self.controller.publish_gait('gait_sofa_sit')) + callback=lambda: self._controller.publish_gait('gait_sofa_sit')) gait_stairs_up = \ self.create_button('gait_stairs_up', image_path='/gait_stairs_up.png', - callback=lambda: self.controller.publish_gait('gait_stairs_up')) + callback=lambda: self._controller.publish_gait('gait_stairs_up')) gait_stairs_down = \ self.create_button('gait_stairs_down', image_path='/gait_stairs_down.png', - callback=lambda: self.controller.publish_gait('gait_stairs_down')) + callback=lambda: self._controller.publish_gait('gait_stairs_down')) gait_stairs_up_single_step = \ - self.create_button('gait_stairs_up_single_step', - callback=lambda: self.controller.publish_gait('gait_stairs_up_single_step')) + self.create_button('gait_stairs_up_single_step', image_path='/gait_stairs_up_single_step.png', + callback=lambda: self._controller.publish_gait('gait_stairs_up_single_step')) gait_stairs_down_single_step = \ - self.create_button('gait_stairs_down_single_step', - callback=lambda: self.controller.publish_gait('gait_stairs_down_single_step')) + self.create_button('gait_stairs_down_single_step', image_path='/gait_stairs_down_single_step.png', + callback=lambda: self._controller.publish_gait('gait_stairs_down_single_step')) gait_rough_terrain_high_step = \ - self.create_button('gait_rough_terrain_high_step', - callback=lambda: self.controller.publish_gait('gait_rough_terrain_high_step')) + self.create_button('gait_rough_terrain_high_step', image_path='/gait_rough_terrain_high_step.png', + callback=lambda: self._controller.publish_gait('gait_rough_terrain_high_step')) gait_rough_terrain_middle_steps = \ self.create_button('gait_rough_terrain_middle_steps', - callback=lambda: self.controller.publish_gait('gait_rough_terrain_middle_steps')) + callback=lambda: self._controller.publish_gait('gait_rough_terrain_middle_steps')) gait_rough_terrain_first_middle_step = \ self.create_button('gait_rough_terrain_first_middle_step', - callback=lambda: self.controller.publish_gait('gait_rough_terrain_first_middle_step')) + image_path='/gait_rough_terrain_first_middle_step.png', + callback=lambda: self._controller.publish_gait('gait_rough_terrain_first_middle_step')) gait_rough_terrain_second_middle_step = \ self.create_button('gait_rough_terrain_second_middle_step', - callback=lambda: self.controller.publish_gait('gait_rough_terrain_second_middle_step')) + image_path='/gait_rough_terrain_second_middle_step.png', + callback=lambda: self._controller.publish_gait('gait_rough_terrain_second_middle_step')) gait_rough_terrain_third_middle_step = \ self.create_button('gait_rough_terrain_third_middle_step', - callback=lambda: self.controller.publish_gait('gait_rough_terrain_third_middle_step')) + image_path='/gait_rough_terrain_third_middle_step.png', + callback=lambda: self._controller.publish_gait('gait_rough_terrain_third_middle_step')) gait_ramp_door_slope_up = \ - self.create_button('gait_ramp_door_slope_up', - callback=lambda: self.controller.publish_gait('gait_ramp_door_slope_up')) + self.create_button('gait_ramp_door_slope_up', image_path='/gait_ramp_door_slope_up.png', + callback=lambda: self._controller.publish_gait('gait_ramp_door_slope_up')) gait_ramp_door_slope_down = \ - self.create_button('gait_ramp_door_slope_down', - callback=lambda: self.controller.publish_gait('gait_ramp_door_slope_down')) + self.create_button('gait_ramp_door_slope_down', image_path='/gait_ramp_door_slope_down.png', + callback=lambda: self._controller.publish_gait('gait_ramp_door_slope_down')) gait_ramp_door_last_step = \ - self.create_button('gait_ramp_door_last_step', - callback=lambda: self.controller.publish_gait('gait_ramp_door_last_step')) + self.create_button('gait_ramp_door_last_step', image_path='/gait_ramp_door_last_step.png', + callback=lambda: self._controller.publish_gait('gait_ramp_door_last_step')) gait_tilted_path_left_straight_start = \ self.create_button('gait_tilted_path_left_straight_start', - callback=lambda: self.controller.publish_gait('gait_tilted_path_left_straight_start')) + image_path='/gait_tilted_path_left_straight_start.png', + callback=lambda: self._controller.publish_gait('gait_tilted_path_left_straight_start')) gait_tilted_path_left_single_step = \ self.create_button('gait_tilted_path_left_single_step', - callback=lambda: self.controller.publish_gait('gait_tilted_path_left_single_step')) + image_path='/gait_tilted_path_left_single_step.png', + callback=lambda: self._controller.publish_gait('gait_tilted_path_left_single_step')) gait_tilted_path_left_straight_end = \ self.create_button('gait_tilted_path_left_straight_end', - callback=lambda: self.controller.publish_gait('gait_tilted_path_left_straight_end')) + image_path='/gait_tilted_path_left_straight_end.png', + callback=lambda: self._controller.publish_gait('gait_tilted_path_left_straight_end')) gait_tilted_path_left_flexed_knee_step = \ self.create_button('gait_tilted_path_left_flexed_knee_step', - callback=lambda: self.controller.publish_gait('gait_tilted_path_left_flexed_knee_step')) + callback=lambda: self._controller.publish_gait('gait_tilted_path_left_flexed_knee_step')) + + gait_tilted_path_left_knee_bend = \ + self.create_button('gait_tilted_path_left_knee_bend', + callback=lambda: self._controller.publish_gait('gait_tilted_path_left_knee_bend')) gait_tilted_path_right_straight_start = \ self.create_button('gait_tilted_path_right_straight_start', - callback=lambda: self.controller.publish_gait('gait_tilted_path_right_straight_start')) + image_path='/gait_tilted_path_right_straight_start.png', + callback=lambda: self._controller.publish_gait('gait_tilted_path_right_straight_start')) gait_tilted_path_right_single_step = \ self.create_button('gait_tilted_path_right_single_step', - callback=lambda: self.controller.publish_gait('gait_tilted_path_right_single_step')) + image_path='/gait_tilted_path_right_single_step.png', + callback=lambda: self._controller.publish_gait('gait_tilted_path_right_single_step')) gait_tilted_path_right_straight_end = \ self.create_button('gait_tilted_path_right_straight_end', - callback=lambda: self.controller.publish_gait('gait_tilted_path_right_straight_end')) + image_path='/gait_tilted_path_right_straight_end.png', + callback=lambda: self._controller.publish_gait('gait_tilted_path_right_straight_end')) gait_tilted_path_right_flexed_knee_step = \ self.create_button('gait_tilted_path_right_flexed_knee_step', - callback=lambda: self.controller.publish_gait('gait_tilted_path_right_flexed_knee_step')) + callback=lambda: self._controller.publish_gait( + 'gait_tilted_path_right_flexed_knee_step')) + + gait_tilted_path_right_knee_bend = \ + self.create_button('gait_tilted_path_right_knee_bend', + callback=lambda: self._controller.publish_gait('gait_tilted_path_right_knee_bend')) gait_tilted_path_first_start = \ self.create_button('gait_tilted_path_first_start', - callback=lambda: self.controller.publish_gait('gait_tilted_path_first_start')) + callback=lambda: self._controller.publish_gait('gait_tilted_path_first_start')) gait_tilted_path_second_start = \ self.create_button('gait_tilted_path_second_start', - callback=lambda: self.controller.publish_gait('gait_tilted_path_second_start')) + callback=lambda: self._controller.publish_gait('gait_tilted_path_second_start')) gait_tilted_path_first_end = \ self.create_button('gait_tilted_path_first_end', - callback=lambda: self.controller.publish_gait('gait_tilted_path_first_end')) + callback=lambda: self._controller.publish_gait('gait_tilted_path_first_end')) gait_tilted_path_second_end = \ self.create_button('gait_tilted_path_second_end', - callback=lambda: self.controller.publish_gait('gait_tilted_path_second_end')) + callback=lambda: self._controller.publish_gait('gait_tilted_path_second_end')) stop_button = self.create_button('gait_stop', image_path='/stop.png', - callback=lambda: self.controller.publish_stop()) + callback=lambda: self._controller.publish_stop(), + always_enabled=True) - pause_button = self.create_button('gait_pause', - callback=lambda: self.controller.publish_pause()) + pause_button = self.create_button('gait_pause', image_path='/pause.png', + callback=lambda: self._controller.publish_pause(), + always_enabled=True) - continue_button = self.create_button('gait_continue', - callback=lambda: self.controller.publish_continue()) + continue_button = self.create_button('gait_continue', image_path='/continue.png', + callback=lambda: self._controller.publish_continue(), + always_enabled=True) error_button = self.create_button('error', image_path='/error.png', - callback=lambda: self.controller.publish_error()) + callback=lambda: self._controller.publish_error(), + always_enabled=True) + + sm_to_unknown_button = self.create_button('force unknown', + callback=lambda: self._controller.publish_sm_to_unknown(), + always_enabled=True) # The button layout. # Position in the array determines position on screen. march_button_layout = [ - [home_sit, home_stand, gait_walk, gait_walk_small, gait_walk_large, stop_button, pause_button], + [home_sit, home_stand, gait_walk, gait_walk_small, gait_walk_large, gait_balanced_walk, + sm_to_unknown_button], - [gait_sit, gait_stand, gait_single_step_normal, gait_single_step_small, continue_button, error_button], + [gait_sit, gait_stand, rocker_switch_increment, rocker_switch_decrement, stop_button, error_button], - [gait_sofa_sit, gait_sofa_stand, rocker_switch_increment, rocker_switch_decrement], + [gait_sofa_sit, gait_sofa_stand, gait_single_step_normal, gait_single_step_small, continue_button, + pause_button], [gait_stairs_up, gait_stairs_down, gait_stairs_up_single_step, gait_stairs_down_single_step], @@ -236,10 +268,12 @@ def __init__(self, context): [gait_ramp_door_slope_up, gait_ramp_door_slope_down, gait_ramp_door_last_step], [gait_tilted_path_left_straight_start, gait_tilted_path_left_single_step, - gait_tilted_path_left_straight_end, gait_tilted_path_left_flexed_knee_step], + gait_tilted_path_left_straight_end, gait_tilted_path_left_flexed_knee_step, + gait_tilted_path_left_knee_bend], [gait_tilted_path_right_straight_start, gait_tilted_path_right_single_step, - gait_tilted_path_right_straight_end, gait_tilted_path_right_flexed_knee_step], + gait_tilted_path_right_straight_end, gait_tilted_path_right_flexed_knee_step, + gait_tilted_path_right_knee_bend], [gait_tilted_path_first_start, gait_tilted_path_second_start, gait_tilted_path_first_end, gait_tilted_path_second_end], @@ -249,45 +283,81 @@ def __init__(self, context): qt_layout = self.create_layout(march_button_layout) # Apply the qt_layout to the top level widget. - self._widget.frame.findChild(QWidget, 'content').setLayout(qt_layout) + self.content.setLayout(qt_layout) # Make the frame as tight as possible with spacing between the buttons. qt_layout.setSpacing(15) - self._widget.frame.findChild(QWidget, 'content').adjustSize() - - @staticmethod - def create_button(text, callback=None, image_path=None, size=(125, 150), visible=True, color_code='#1F1E24'): + self.content.adjustSize() + + def _accepted_cb(self): + self.status_label.setText('Gait accepted') + self._update_possible_gaits() + + def _finished_cb(self): + self.status_label.setText('Gait finished') + self.gait_label.setText('') + self._update_possible_gaits() + + def _rejected_cb(self): + self.status_label.setText('Gait rejected') + self.gait_label.setText('') + self._update_possible_gaits() + + def _current_gait_cb(self, gait_name): + self.gait_label.setText(gait_name) + + def _update_possible_gaits(self): + self.frame.setEnabled(False) + self.frame.verticalScrollBar().setEnabled(False) + possible_gaits = self._controller.get_possible_gaits() + layout = self.content.layout() + if layout: + for i in range(layout.count()): + button = layout.itemAt(i).widget() + name = button.objectName() + if name in self._always_enabled_buttons: + continue + + if name in possible_gaits: + button.setEnabled(True) + else: + button.setEnabled(False) + self.frame.setEnabled(True) + self.frame.verticalScrollBar().setEnabled(True) + + def create_button(self, name, callback=None, image_path=None, size=(128, 160), always_enabled=False): """Create a push button which the mock input device can register. - :param text: - Possible name of the button + :param name: + Name of the button :param callback: The callback to attach to the button when pressed :param image_path: The name of the image file :param size: Default size of the button - :param visible: - Turn button invisible on the gui - :param color_code: - Possible background color of the button + :param always_enabled: + Never disable the button if it's not in possible gaits :return: A QPushButton object which contains the passed arguments """ - qt_button = QPushButton() + if image_path is None: + qt_button = QPushButton() - if image_path: - qt_button.setStyleSheet(create_image_button_css(get_image_path(image_path))) - else: - text = check_string(text) - qt_button.setStyleSheet(create_color_button_css(color_code)) + text = check_string(name) qt_button.setText(text) + else: + qt_button = ImageButton(get_image_path(image_path)) - qt_button.setMinimumSize(QSize(*size)) + qt_button.setObjectName(name) - if not visible: - qt_button.setVisible(False) + if always_enabled: + self._always_enabled_buttons.append(name) + qt_button.setEnabled(True) + + qt_button.setMinimumSize(QSize(*size)) + qt_button.setMaximumSize(QSize(*size)) if callback: qt_button.clicked.connect(callback) @@ -314,25 +384,6 @@ def create_layout(layout_list): return qt_button_layout -def create_image_button_css(img_path): - """CSS of a button with a background-image.""" - css_base = """ - background: url() no-repeat center center; - background-color:#1F1E24; - color: #000000; - """ - return css_base.replace('', img_path) - - -def create_color_button_css(color_code): - """CSS of a button with a background-color.""" - css_base = """ - background-color: ; - color: #FFFFFF; - """ - return css_base.replace('', color_code) - - def get_image_path(img_name): """Create an absolute image path to an image.""" return os.path.join( @@ -342,25 +393,11 @@ def get_image_path(img_name): def check_string(text): - """Split text into two lines if sentence is to long. - - :param text: - The text to split in half. + """Split text into new lines on every third word. - :return: - New string which contains of an enter in the middle. + :type text: str + :param text: The text to split + :return New string which contains newlines """ - text = text.replace('_', ' ') - split_text = text.split(' ') - if len(split_text) >= 3: - - new_string = '' - middle = len(split_text) / 2 - - new_string += ' '.join(split_text[:middle]) - new_string += '\n' - new_string += ' '.join(split_text[middle:]) - return new_string - - else: - return text + words = enumerate(text.replace('_', ' ').split(' ')) + return reduce(lambda acc, (i, x): acc + '\n' + x if i % 3 == 0 else acc + ' ' + x, words, '')[1:] diff --git a/march_rqt_robot_monitor/CMakeLists.txt b/march_rqt_robot_monitor/CMakeLists.txt new file mode 100644 index 0000000..42fc122 --- /dev/null +++ b/march_rqt_robot_monitor/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 2.8.3) +project(march_rqt_robot_monitor) + +find_package(catkin REQUIRED COMPONENTS + diagnostic_aggregator + diagnostic_updater + march_shared_resources + std_msgs +) + +catkin_package(CATKIN_DEPENDS + diagnostic_aggregator + diagnostic_updater + march_shared_resources + std_msgs +) + +catkin_python_setup() + +install(DIRECTORY config launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +catkin_install_python(PROGRAMS scripts/${PROJECT_NAME}_node + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) diff --git a/march_rqt_robot_monitor/config/analyzers.yaml b/march_rqt_robot_monitor/config/analyzers.yaml new file mode 100644 index 0000000..d772e39 --- /dev/null +++ b/march_rqt_robot_monitor/config/analyzers.yaml @@ -0,0 +1,23 @@ +pub_rate: 1.0 +base_path: '' +analyzers: + temperatures: + type: diagnostic_aggregator/GenericAnalyzer + path: Temperatures + contains: 'Temperature' + remove_prefix: 'march_rqt_robot_monitor_node: ' + inputs: + type: diagnostic_aggregator/GenericAnalyzer + path: Inputs + contains: 'input_device' + remove_prefix: 'march_rqt_robot_monitor_node: ' + control: + type: diagnostic_aggregator/GenericAnalyzer + path: Control + contains: 'Control' + remove_prefix: 'march_rqt_robot_monitor_node: ' + imotioncubes: + type: diagnostic_aggregator/GenericAnalyzer + path: iMotioncubes + contains: 'IMC' + remove_prefix: 'march_rqt_robot_monitor_node: IMC ' diff --git a/march_rqt_robot_monitor/launch/march_rqt_robot_monitor.launch b/march_rqt_robot_monitor/launch/march_rqt_robot_monitor.launch new file mode 100644 index 0000000..08f5706 --- /dev/null +++ b/march_rqt_robot_monitor/launch/march_rqt_robot_monitor.launch @@ -0,0 +1,13 @@ + + + + + + + + + + diff --git a/march_rqt_robot_monitor/package.xml b/march_rqt_robot_monitor/package.xml new file mode 100644 index 0000000..c2ccdcd --- /dev/null +++ b/march_rqt_robot_monitor/package.xml @@ -0,0 +1,19 @@ + + + march_rqt_robot_monitor + 0.0.0 + rqt automated software tool to analyze the March exoskeleton during runtime + + Project March + TODO + + catkin + + std_msgs + diagnostic_aggregator + diagnostic_updater + march_shared_resources + + roscpp + rospy + diff --git a/march_rqt_robot_monitor/scripts/march_rqt_robot_monitor_node b/march_rqt_robot_monitor/scripts/march_rqt_robot_monitor_node new file mode 100755 index 0000000..8ca0ad1 --- /dev/null +++ b/march_rqt_robot_monitor/scripts/march_rqt_robot_monitor_node @@ -0,0 +1,5 @@ +#!/usr/bin/env python +from march_rqt_robot_monitor import updater + +if __name__ == '__main__': + updater.main() diff --git a/march_rqt_robot_monitor/setup.py b/march_rqt_robot_monitor/setup.py new file mode 100644 index 0000000..383b5c8 --- /dev/null +++ b/march_rqt_robot_monitor/setup.py @@ -0,0 +1,12 @@ +#!/usr/bin/env python +from distutils.core import setup + +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + packages=['march_rqt_robot_monitor', 'march_rqt_robot_monitor.diagnostic_analyzers'], + package_dir={'': 'src'}, + scripts=['scripts/march_rqt_robot_monitor_node'], +) + +setup(**d) diff --git a/march_rqt_robot_monitor/src/march_rqt_robot_monitor/__init__.py b/march_rqt_robot_monitor/src/march_rqt_robot_monitor/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/march_rqt_robot_monitor/src/march_rqt_robot_monitor/diagnostic_analyzers/__init__.py b/march_rqt_robot_monitor/src/march_rqt_robot_monitor/diagnostic_analyzers/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/march_rqt_robot_monitor/src/march_rqt_robot_monitor/diagnostic_analyzers/control.py b/march_rqt_robot_monitor/src/march_rqt_robot_monitor/diagnostic_analyzers/control.py new file mode 100644 index 0000000..a215298 --- /dev/null +++ b/march_rqt_robot_monitor/src/march_rqt_robot_monitor/diagnostic_analyzers/control.py @@ -0,0 +1,146 @@ + +from diagnostic_msgs.msg import DiagnosticStatus +import rospy +from urdf_parser_py import urdf + + +WARN_PERCENTAGE = 5 + + +class CheckJointValues(object): + """Base class to diagnose the joint movement values.""" + + def __init__(self, topic, msg_type): + rospy.Subscriber(topic, msg_type, self.cb) + + # callback variables + self._timestamp = None + self._joint_names = None + self._position = None + self._velocity = None + self._effort = None + + # robot properties + self._lower_soft_limits = {} + self._upper_soft_limits = {} + self._velocity_limits = {} + self._effort_limits = {} + + self._robot = urdf.Robot.from_parameter_server('/robot_description') + for joint in self._robot.joints: + try: + self._lower_soft_limits[joint.name] = joint.safety_controller.soft_lower_limit + self._upper_soft_limits[joint.name] = joint.safety_controller.soft_upper_limit + self._velocity_limits[joint.name] = joint.limit.velocity + self._effort_limits[joint.name] = joint.limit.effort + except AttributeError: + pass + + def cb(self, msg): + """Save the latest published movement values with corresponding timestamp.""" + self._timestamp = msg.header.stamp + self._joint_names = msg.name + self._position = msg.position + self._velocity = msg.velocity + self._effort = msg.effort + + def position_diagnostics(self, stat): + """The diagnostic message to display the positions in standard format.""" + if self._timestamp is None: + stat.add('Topic error', 'No events recorded') + stat.summary(DiagnosticStatus.STALE, 'No position recorded') + return stat + + stat.add('Timestamp', self._timestamp) + + joint_outside_soft_limits = [] + joint_in_warning_zone_soft_limits = [] + + for index in range(len(self._joint_names)): + joint_name = self._joint_names[index] + stat.add(self._joint_names[index], self._position[index]) + + if self._position[index] >= self._upper_soft_limits[joint_name]: + joint_outside_soft_limits.append(self._joint_names[index]) + elif self._position[index] >= (self._upper_soft_limits[joint_name] * (1 - WARN_PERCENTAGE / 100)): + joint_in_warning_zone_soft_limits.append(self._joint_names[index]) + + if self._position[index] <= self._lower_soft_limits[joint_name]: + joint_outside_soft_limits.append(self._joint_names[index]) + elif self._position[index] <= (self._lower_soft_limits[joint_name] * (1 - WARN_PERCENTAGE / 100)): + joint_in_warning_zone_soft_limits.append(self._joint_names[index]) + + if joint_outside_soft_limits: + stat.summary(DiagnosticStatus.ERROR, 'Outside soft limits: {ls}' + .format(ls=str(joint_outside_soft_limits))) + elif joint_in_warning_zone_soft_limits: + stat.summary(DiagnosticStatus.WARN, 'Close to soft limits: {ls}' + .format(ls=str(joint_in_warning_zone_soft_limits))) + else: + stat.summary(DiagnosticStatus.OK, 'OK') + + return stat + + def velocity_diagnostics(self, stat): + """The diagnostic message to display the velocities in standard format.""" + if self._timestamp is None: + stat.add('Topic error', 'No events recorded') + stat.summary(DiagnosticStatus.STALE, 'No velocity recorded') + return stat + + stat.add('Timestamp', self._timestamp) + + joints_at_velocity_limit = [] + joint_in_warning_zone_velocity_limit = [] + + for index in range(len(self._joint_names)): + joint_name = self._joint_names[index] + stat.add(self._joint_names[index], self._velocity[index]) + + if self._velocity[index] >= self._velocity_limits[joint_name]: + joints_at_velocity_limit.append(self._joint_names[index]) + elif self._velocity[index] >= (self._velocity_limits[joint_name] * (1 - WARN_PERCENTAGE / 100)): + joint_in_warning_zone_velocity_limit.append(self._joint_names[index]) + + if joints_at_velocity_limit: + stat.summary(DiagnosticStatus.ERROR, 'At velocity limit: {ls}' + .format(ls=str(joints_at_velocity_limit))) + elif joint_in_warning_zone_velocity_limit: + stat.summary(DiagnosticStatus.WARN, 'Close to velocity limit: {ls}' + .format(ls=str(joint_in_warning_zone_velocity_limit))) + else: + stat.summary(DiagnosticStatus.OK, 'OK') + + return stat + + def effort_diagnostics(self, stat): + """The diagnostic message to display the efforts in standard format.""" + if self._timestamp is None: + stat.add('Topic error', 'No events recorded') + stat.summary(DiagnosticStatus.STALE, 'No effort recorded') + return stat + + stat.add('Timestamp', self._timestamp) + + joints_at_effort_limit = [] + joints_in_warning_zone_effort_limits = [] + + for index in range(len(self._joint_names)): + joint_name = self._joint_names[index] + stat.add(self._joint_names[index], self._position[index]) + + if self._effort[index] >= self._effort_limits[joint_name]: + joints_at_effort_limit.append(self._joint_names[index]) + elif self._effort[index] >= (self._effort_limits[joint_name] * (1 - WARN_PERCENTAGE / 100)): + joints_in_warning_zone_effort_limits.append(self._joint_names[index]) + + if joints_at_effort_limit: + stat.summary(DiagnosticStatus.ERROR, 'At effort limit: {ls}' + .format(ls=str(joints_at_effort_limit))) + elif joints_in_warning_zone_effort_limits: + stat.summary(DiagnosticStatus.WARN, 'Close to effort limit: {ls}' + .format(ls=str(joints_in_warning_zone_effort_limits))) + else: + stat.summary(DiagnosticStatus.OK, 'OK') + + return stat diff --git a/march_rqt_robot_monitor/src/march_rqt_robot_monitor/diagnostic_analyzers/imc_state.py b/march_rqt_robot_monitor/src/march_rqt_robot_monitor/diagnostic_analyzers/imc_state.py new file mode 100644 index 0000000..4d65541 --- /dev/null +++ b/march_rqt_robot_monitor/src/march_rqt_robot_monitor/diagnostic_analyzers/imc_state.py @@ -0,0 +1,59 @@ +from diagnostic_msgs.msg import DiagnosticStatus +import rospy + +from march_shared_resources.msg import ImcState + + +class CheckImcStatus: + def __init__(self, updater): + """Initializes an IMC diagnostic which analyzes IMC states. + + :type updater: diagnostic_updater.Updater + """ + self._updater = updater + self._sub = rospy.Subscriber('/march/imc_states', ImcState, self._cb) + self._imc_state = None + + self._diagnostics = set() + + def _cb(self, msg): + """Callback for imc_states. + + :type msg: ImcState + """ + self._imc_state = msg + for i in range(len(msg.joint_names)): + joint_name = msg.joint_names[i] + if joint_name not in self._diagnostics: + self._diagnostics.add(joint_name) + self._updater.add('IMC {0}'.format(joint_name), self._diagnostic(i)) + + def _diagnostic(self, index): + """Creates a diagnostic function for an IMC. + + :type index: int + :param index: index of the joint + :return Curried diagnostic function that updates the diagnostic status + according to the given index + """ + def d(stat): + if self._imc_state.joint_names[index] is None: + stat.summary(DiagnosticStatus.STALE, 'No more events recorded') + return stat + detailed_error = int(self._imc_state.detailed_error[index]) + motion_error = int(self._imc_state.motion_error[index]) + state = self._imc_state.state[index] + + stat.add('Status word', self._imc_state.status_word[index]) + if detailed_error != 0 or motion_error != 0: + stat.add('Detailed error', self._imc_state.detailed_error[index]) + stat.add('Detailed error description', self._imc_state.detailed_error_description[index]) + stat.add('Motion error', self._imc_state.motion_error[index]) + stat.add('Motion error description', self._imc_state.motion_error_description[index]) + stat.summary(DiagnosticStatus.ERROR, state) + else: + stat.summary(DiagnosticStatus.OK, state) + + return stat + + return d diff --git a/march_rqt_robot_monitor/src/march_rqt_robot_monitor/diagnostic_analyzers/temperature.py b/march_rqt_robot_monitor/src/march_rqt_robot_monitor/diagnostic_analyzers/temperature.py new file mode 100644 index 0000000..2de6882 --- /dev/null +++ b/march_rqt_robot_monitor/src/march_rqt_robot_monitor/diagnostic_analyzers/temperature.py @@ -0,0 +1,58 @@ + +from diagnostic_msgs.msg import DiagnosticStatus +import rospy + + +LOWER_THRESHOLD_VALID_TEMPERATURE_VALUE = 0 +UPPER_THRESHOLD_VALID_TEMPERATURE_VALUE = 100 + + +class CheckJointTemperature(object): + """Base class to diagnose the joint temperatures.""" + + def __init__(self, joint_name, topic, msg_type): + rospy.Subscriber(topic, msg_type, self.cb) + + self._default_thresholds = \ + rospy.get_param('safety_node/default_temperature_threshold', None) + self._thresholds_warning = \ + rospy.get_param('safety_node/temperature_thresholds_warning/{jn}'.format(jn=joint_name), None) + self._thresholds_non_fatal = \ + rospy.get_param('safety_node/temperature_thresholds_non_fatal/{jn}'.format(jn=joint_name), None) + self._thresholds_fatal = \ + rospy.get_param('safety_node/temperature_thresholds_fatal/{jn}'.format(jn=joint_name), None) + + self._timestamp = None + self._temperature = None + + def cb(self, msg): + """Save the latest published temperature with corresponding timestamp.""" + self._temperature = float(msg.temperature) + self._timestamp = msg.header.stamp + + def diagnostics(self, stat): + """The diagnostic message to display in the standard stat format.""" + if self._timestamp is None: + stat.add('Topic error', 'No events recorded') + stat.summary(DiagnosticStatus.STALE, 'No temperature recorded') + return stat + + if not LOWER_THRESHOLD_VALID_TEMPERATURE_VALUE < self._temperature < UPPER_THRESHOLD_VALID_TEMPERATURE_VALUE: + stat.add('Current temperature', self._temperature) + stat.summary(DiagnosticStatus.WARN, 'No valid temperature value ({tp}).'.format(tp=self._temperature)) + return stat + + stat.add('Default threshold', self._default_thresholds) + stat.add('Warning threshold', self._thresholds_warning) + stat.add('Non fatal threshold', self._thresholds_non_fatal) + stat.add('Fatal threshold', self._thresholds_fatal) + stat.add('Current temperature', self._temperature) + + if self._temperature < self._thresholds_warning: + stat.summary(DiagnosticStatus.OK, 'OK ({tp}).'.format(tp=self._temperature)) + elif self._thresholds_warning < self._temperature < self._thresholds_non_fatal: + stat.summary(DiagnosticStatus.WARN, 'Temperature almost too high ({tp}).'.format(tp=self._temperature)) + else: + stat.summary(DiagnosticStatus.ERROR, 'Temperature to high ({tp}).'.format(tp=self._temperature)) + + return stat diff --git a/march_rqt_robot_monitor/src/march_rqt_robot_monitor/diagnostic_analyzers/topic_frequency.py b/march_rqt_robot_monitor/src/march_rqt_robot_monitor/diagnostic_analyzers/topic_frequency.py new file mode 100644 index 0000000..7a305a4 --- /dev/null +++ b/march_rqt_robot_monitor/src/march_rqt_robot_monitor/diagnostic_analyzers/topic_frequency.py @@ -0,0 +1,19 @@ + +import diagnostic_updater +import rospy + + +class CheckTopicFrequency(object): + """Base class to diagnose whether the IPD is connected properly.""" + + def __init__(self, name, topic, message_type, general_updater, frequency): + self._name = name + self._frequency_bounds = {'min': frequency, 'max': frequency} + + self._topic_frequency = diagnostic_updater.HeaderlessTopicDiagnostic( + topic, general_updater, diagnostic_updater.FrequencyStatusParam(self._frequency_bounds, 0.1, 10)) + rospy.Subscriber(topic, message_type, self.cb) + + def cb(self, msg): + """Update the frequency checker.""" + self._topic_frequency.tick() diff --git a/march_rqt_robot_monitor/src/march_rqt_robot_monitor/updater.py b/march_rqt_robot_monitor/src/march_rqt_robot_monitor/updater.py new file mode 100644 index 0000000..ebe11e2 --- /dev/null +++ b/march_rqt_robot_monitor/src/march_rqt_robot_monitor/updater.py @@ -0,0 +1,66 @@ +import diagnostic_updater +import rospy +from sensor_msgs.msg import JointState, Temperature +from std_msgs.msg import Time + +from .diagnostic_analyzers.control import CheckJointValues +from .diagnostic_analyzers.imc_state import CheckImcStatus +from .diagnostic_analyzers.temperature import CheckJointTemperature +from .diagnostic_analyzers.topic_frequency import CheckTopicFrequency + + +def main(): + rospy.init_node('Diagnostic_updater') + + updater = diagnostic_updater.Updater() + updater.setHardwareID('MARCH IVc') + + # Frequency checks + CheckTopicFrequency('Input_Device', '/march/input_device/alive', Time, updater, 5) + + # Temperature checks + check_temp_joint_left_ankle = \ + CheckJointTemperature('Temperature left ankle', '/march/temperature/left_ankle', Temperature) + updater.add('Temperature left ankle', check_temp_joint_left_ankle.diagnostics) + + check_temp_joint_left_knee = \ + CheckJointTemperature('Temperature left knee', '/march/temperature/left_knee', Temperature) + updater.add('Temperature left knee', check_temp_joint_left_knee.diagnostics) + + check_temp_joint_left_hip_fe = \ + CheckJointTemperature('Temperature left hip FE', '/march/temperature/left_hip_fe', Temperature) + updater.add('Temperature left hip FE', check_temp_joint_left_hip_fe.diagnostics) + + check_temp_joint_left_hip_aa = \ + CheckJointTemperature('Temperature left hip AA', '/march/temperature/left_hip_aa', Temperature) + updater.add('Temperature left hip AA', check_temp_joint_left_hip_aa.diagnostics) + + check_temp_joint_right_ankle = \ + CheckJointTemperature('Temperature right ankle', '/march/temperature/right_ankle', Temperature) + updater.add('Temperature right ankle', check_temp_joint_right_ankle.diagnostics) + + check_temp_joint_right_knee = \ + CheckJointTemperature('Temperature right knee', '/march/temperature/right_knee', Temperature) + updater.add('Temperature right knee', check_temp_joint_right_knee.diagnostics) + + check_temp_joint_right_hip_fe = \ + CheckJointTemperature('Temperature right hip FE', '/march/temperature/right_hip_fe', Temperature) + updater.add('Temperature right hip FE', check_temp_joint_right_hip_fe.diagnostics) + + check_temp_joint_right_hip_aa = \ + CheckJointTemperature('Temperature right hip AA', '/march/temperature/right_hip_aa', Temperature) + updater.add('Temperature right hip AA', check_temp_joint_right_hip_aa.diagnostics) + + # control checks + check_current_movement_values = CheckJointValues('march/joint_states', JointState) + updater.add('Control position values', check_current_movement_values.position_diagnostics) + updater.add('Control velocity values', check_current_movement_values.velocity_diagnostics) + updater.add('Control effort values', check_current_movement_values.effort_diagnostics) + + CheckImcStatus(updater) + + updater.force_update() + + while not rospy.is_shutdown(): + rospy.sleep(0.1) + updater.update() diff --git a/march_rqt_software_check/CMakeLists.txt b/march_rqt_software_check/CMakeLists.txt new file mode 100644 index 0000000..d410fec --- /dev/null +++ b/march_rqt_software_check/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 2.8.3) +project(march_rqt_software_check) + +find_package(catkin REQUIRED) + +catkin_python_setup() +catkin_package() + +install(FILES plugin.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +install(DIRECTORY launch resource + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +install(PROGRAMS scripts/march_git_branch_check scripts/${PROJECT_NAME} + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) diff --git a/march_rqt_software_check/launch/checks/git_branch.launch b/march_rqt_software_check/launch/checks/git_branch.launch new file mode 100644 index 0000000..c895499 --- /dev/null +++ b/march_rqt_software_check/launch/checks/git_branch.launch @@ -0,0 +1,5 @@ + + + + + diff --git a/march_rqt_software_check/launch/checks/slave_count.launch b/march_rqt_software_check/launch/checks/slave_count.launch new file mode 100644 index 0000000..b7dc188 --- /dev/null +++ b/march_rqt_software_check/launch/checks/slave_count.launch @@ -0,0 +1,5 @@ + + + + + diff --git a/march_rqt_software_check/launch/march_rqt_software_check.launch b/march_rqt_software_check/launch/march_rqt_software_check.launch new file mode 100644 index 0000000..9859e7c --- /dev/null +++ b/march_rqt_software_check/launch/march_rqt_software_check.launch @@ -0,0 +1,3 @@ + + + diff --git a/march_rqt_software_check/package.xml b/march_rqt_software_check/package.xml new file mode 100644 index 0000000..e47638d --- /dev/null +++ b/march_rqt_software_check/package.xml @@ -0,0 +1,21 @@ + + + march_rqt_software_check + 0.0.0 + rqt automated software checks for the March exoskeleton + + Project March + + TODO + + catkin + + python-rospkg + rospy + rqt_gui + rqt_gui_py + + + + + diff --git a/march_rqt_software_check/plugin.xml b/march_rqt_software_check/plugin.xml new file mode 100644 index 0000000..d101d0a --- /dev/null +++ b/march_rqt_software_check/plugin.xml @@ -0,0 +1,12 @@ + + + + RQT plugin to perform all software checks. + + + + resource/img/march-walking.png + March Software Check + + + diff --git a/march_rqt_software_check/resource/img/march-walking.png b/march_rqt_software_check/resource/img/march-walking.png new file mode 100644 index 0000000..15f9c92 Binary files /dev/null and b/march_rqt_software_check/resource/img/march-walking.png differ diff --git a/march_rqt_software_check/resource/software_check.ui b/march_rqt_software_check/resource/software_check.ui new file mode 100644 index 0000000..369445c --- /dev/null +++ b/march_rqt_software_check/resource/software_check.ui @@ -0,0 +1,143 @@ + + + SoftwareCheck + + + + 0 + 0 + 1109 + 796 + + + + March Software Check + + + QGroupBox { + border: 1px solid gray; + margin-top: 0.5em; +} + +QGroupBox::title { + subcontrol-origin: margin; + left: 10px; + padding: 0 3px 0 3px; +} + + + + + + QFrame::StyledPanel + + + QFrame::Raised + + + + + + false + + + false + + + #Log { +background-color: #F2F1F0 +} + + + + + + + + + + QFrame::StyledPanel + + + QFrame::Raised + + + + + + Local + + + + + + + 150 + 150 + + + + Gait File Location + + + + + + + + 150 + 150 + + + + March IV +URDF Check + + + + + + + + + + Exoskeleton + + + + + + + 150 + 150 + + + + Slave Count + + + + + + + + 150 + 150 + + + + Git Branch + + + + + + + + + + + + + + diff --git a/march_rqt_software_check/scripts/march_git_branch_check b/march_rqt_software_check/scripts/march_git_branch_check new file mode 100755 index 0000000..0cdf130 --- /dev/null +++ b/march_rqt_software_check/scripts/march_git_branch_check @@ -0,0 +1,22 @@ +#!/usr/bin/env python +import os + +from pygit2 import Repository, GitError +import rospkg +import rospy + +result = [] + +march_launch_path = rospkg.RosPack().get_path("march_launch") +head = os.path.split(march_launch_path)[0] +source_path = os.path.split(head)[0] + +for repository_name in os.listdir(source_path): + repository_path = os.path.join(source_path, repository_name) + try: + branch_name = Repository(repository_path).head.shorthand + result.append([repository_name, branch_name]) + except GitError as e: + pass + +rospy.set_param("/checks/git_branch", result) diff --git a/march_rqt_software_check/scripts/march_rqt_software_check b/march_rqt_software_check/scripts/march_rqt_software_check new file mode 100755 index 0000000..3476365 --- /dev/null +++ b/march_rqt_software_check/scripts/march_rqt_software_check @@ -0,0 +1,9 @@ +#!/usr/bin/env python + +import sys + +from rqt_gui.main import Main + +plugin = 'software_check' +main = Main(filename=plugin) +sys.exit(main.main(standalone=plugin)) diff --git a/march_rqt_software_check/setup.py b/march_rqt_software_check/setup.py new file mode 100644 index 0000000..659c0ab --- /dev/null +++ b/march_rqt_software_check/setup.py @@ -0,0 +1,12 @@ +#!/usr/bin/env python +from distutils.core import setup + +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + packages=['march_rqt_software_check', 'march_rqt_software_check.checks'], + package_dir={'': 'src'}, + scripts=['scripts/march_rqt_software_check'], +) + +setup(**d) diff --git a/march_rqt_software_check/src/march_rqt_software_check/__init__.py b/march_rqt_software_check/src/march_rqt_software_check/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/march_rqt_software_check/src/march_rqt_software_check/check_runner.py b/march_rqt_software_check/src/march_rqt_software_check/check_runner.py new file mode 100644 index 0000000..85294b0 --- /dev/null +++ b/march_rqt_software_check/src/march_rqt_software_check/check_runner.py @@ -0,0 +1,82 @@ +from python_qt_binding.QtWidgets import QMessageBox +import rospy + +from .checks.gait_file_directory_check import GaitFileDirectoryCheck +from .checks.git_branch_check import GitBranchCheck +from .checks.slave_count_check import SlaveCountCheck +from .checks.urdf_check import URDFCheck +from .color import Color +from .software_check_thread import SoftwareCheckThread + + +class CheckRunner: + def __init__(self, logger=None): + self.checks = [GitBranchCheck(), GaitFileDirectoryCheck(), URDFCheck(), SlaveCountCheck()] + for check in self.checks: + check.log_signal.connect(lambda msg, color: self.log(msg, color)) + self.logger = logger + self.thread = None + + def run_check_by_name(self, name): + check = self.get_check(name) + if check is None: + self.log('Check with name ' + name + ' does not exist', Color.Error) + + return self.run_check(check) + + def run_check(self, check): + self.log('--------------------------------------', Color.Info) + + if self.thread is not None: + self.log('Already running another check', Color.Warning) + + if check is None: + self.log('Check does not exist', Color.Error) + return + + self.log('Starting check ' + str(check.name) + ': ' + str(check.description), Color.Info) + + start = rospy.get_rostime() + self.thread = SoftwareCheckThread(check) + self.thread.start() + + while not check.done: + if rospy.get_rostime() < start + rospy.Duration.from_sec(check.timeout): + rospy.sleep(0.1) + + else: + self.log('Check ' + str(check.name) + ' timed out after ' + str(check.timeout) + 's', Color.Error) + self.thread.exit() + self.thread = None + + check.reset() + return False + + self.thread.wait() + self.thread = None + result = check.passed + if result and check.manual_confirmation: + result = self.validate_manually() + check.reset() + + if result: + self.log('Check ' + str(check.name) + ' was succesful!', Color.Debug) + else: + self.log('Check ' + str(check.name) + ' has failed', Color.Error) + + return result + + def get_check(self, name): + for check in self.checks: + if check.name == name: + return check + return None + + def log(self, msg, level): + if self.logger is not None: + self.logger(msg, level) + + @staticmethod + def validate_manually(): + reply = QMessageBox.question(None, 'Message', 'Did the test pass?', QMessageBox.Yes, QMessageBox.No) + return reply == QMessageBox.Yes diff --git a/march_rqt_software_check/src/march_rqt_software_check/checks/__init__.py b/march_rqt_software_check/src/march_rqt_software_check/checks/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/march_rqt_software_check/src/march_rqt_software_check/checks/gait_file_directory_check.py b/march_rqt_software_check/src/march_rqt_software_check/checks/gait_file_directory_check.py new file mode 100644 index 0000000..105a538 --- /dev/null +++ b/march_rqt_software_check/src/march_rqt_software_check/checks/gait_file_directory_check.py @@ -0,0 +1,33 @@ +import rospkg +import rospy + +from march_rqt_software_check.color import Color + +from .launch_check import LaunchCheck + + +class GaitFileDirectoryCheck(LaunchCheck): + + def __init__(self): + LaunchCheck.__init__(self, 'GaitFileDirectory', 'Gaits are loaded from the following directory', + 'march_gait_selection', 'march_gait_selection.launch', manual_confirmation=True) + + def perform(self): + self.launch() + + rospy.sleep(rospy.Duration.from_sec(5)) + self.stop_launch_process() + package_name = self.get_key_from_parameter_server('/march/gait_file_package') + file_directory = self.get_key_from_parameter_server('/march/gait_file_directory') + + try: + rospkg.RosPack().get_path(package_name) + except rospkg.common.ResourceNotFound: + self.log('Package ' + str(package_name) + ' not found on local machine.', Color.Error) + self.fail_check() + + self.log('Package name: ' + str(package_name), Color.Info) + self.log('Gait file directory: ' + str(file_directory), Color.Info) + + self.passed = package_name is not None and file_directory is not None + self.done = True diff --git a/march_rqt_software_check/src/march_rqt_software_check/checks/git_branch_check.py b/march_rqt_software_check/src/march_rqt_software_check/checks/git_branch_check.py new file mode 100644 index 0000000..5cbaea5 --- /dev/null +++ b/march_rqt_software_check/src/march_rqt_software_check/checks/git_branch_check.py @@ -0,0 +1,38 @@ +import rospy + +from march_rqt_software_check.color import Color + +from .launch_check import LaunchCheck + + +class GitBranchCheck(LaunchCheck): + + def __init__(self): + LaunchCheck.__init__(self, + 'GitBranch', + 'Please confirm the branches below', + 'march_rqt_software_check', + 'git_branch.launch', + 10, + True) + + def perform(self): + self.launch() + + rospy.sleep(4) + self.stop_launch_process() + + branch_tuples = self.get_key_from_parameter_server('/checks/git_branch') + + for branch_tuple in branch_tuples: + repository_name, branch_name = branch_tuple + print_string = repository_name + ': ' + branch_name + if branch_name == 'develop': + self.log(print_string, Color.Debug) + elif 'training/' in branch_name: + self.log(print_string, Color.Debug) + else: + self.log(print_string, Color.Warning) + + self.done = True + self.passed = True diff --git a/march_rqt_software_check/src/march_rqt_software_check/checks/launch_check.py b/march_rqt_software_check/src/march_rqt_software_check/checks/launch_check.py new file mode 100644 index 0000000..7d03a27 --- /dev/null +++ b/march_rqt_software_check/src/march_rqt_software_check/checks/launch_check.py @@ -0,0 +1,22 @@ +import subprocess + +from .software_check import SoftwareCheck + + +class LaunchCheck(SoftwareCheck): + + def __init__(self, name, description, package, launch_file, timeout=10000, manual_confirmation=False): + SoftwareCheck.__init__(self, name, description, timeout, manual_confirmation) + self.package = package + self.file = launch_file + self.launch_process = None + + def launch(self): + cmd = 'roslaunch ' + self.package + ' ' + self.file + self.launch_process = subprocess.Popen(cmd, shell=True, executable='/bin/bash') + + def perform(self): + raise NotImplementedError('Please implement method "perform()" on the subclass') + + def stop_launch_process(self): + self.launch_process.kill() diff --git a/march_rqt_software_check/src/march_rqt_software_check/checks/slave_count_check.py b/march_rqt_software_check/src/march_rqt_software_check/checks/slave_count_check.py new file mode 100644 index 0000000..97bee90 --- /dev/null +++ b/march_rqt_software_check/src/march_rqt_software_check/checks/slave_count_check.py @@ -0,0 +1,22 @@ +import rospy + +from march_rqt_software_check.color import Color + +from .launch_check import LaunchCheck + + +class SlaveCountCheck(LaunchCheck): + + def __init__(self): + LaunchCheck.__init__(self, 'SlaveCount', '', 'march_launch', 'slave_count.launch', manual_confirmation=True) + + def perform(self): + self.launch() + + rospy.sleep(rospy.Duration.from_sec(5)) + self.stop_launch_process() + slave_count = self.get_key_from_parameter_server('/check/slave_count') + + self.log('Ethercat found ' + str(slave_count) + ' slave(s)', Color.Info) + self.passed = slave_count > 0 + self.done = True diff --git a/march_rqt_software_check/src/march_rqt_software_check/checks/software_check.py b/march_rqt_software_check/src/march_rqt_software_check/checks/software_check.py new file mode 100644 index 0000000..ea2053d --- /dev/null +++ b/march_rqt_software_check/src/march_rqt_software_check/checks/software_check.py @@ -0,0 +1,50 @@ +from PyQt5.QtCore import pyqtSignal, QObject +import rospy + +from march_rqt_software_check.color import Color + + +class SoftwareCheck(QObject): + log_signal = pyqtSignal(str, Color) + + def __init__(self, name, description, timeout=10, manual_confirmation=False): + QObject.__init__(self, None) + self.name = name + self.description = description + self.manual_confirmation = manual_confirmation + self.timeout = timeout + self.passed = False + self.done = False + + def reset(self): + self.passed = False + self.done = False + + def is_passed(self): + if self.done: + return self.passed + return False + + def perform(self): + raise NotImplementedError('Please implement method "perform()" on the subclass') + + def pass_check(self): + self.passed = True + self.done = True + + def fail_check(self): + self.passed = False + self.done = True + + def get_key_from_parameter_server(self, key, fail_on_exception=True): + try: + value = rospy.get_param(key) + except KeyError: + self.log('Could not find key ' + str(key), Color.Error) + if fail_on_exception: + self.fail_check() + return + return value + + def log(self, msg, level): + self.log_signal.emit(str(msg), level) diff --git a/march_rqt_software_check/src/march_rqt_software_check/checks/urdf_check.py b/march_rqt_software_check/src/march_rqt_software_check/checks/urdf_check.py new file mode 100644 index 0000000..6f1eed3 --- /dev/null +++ b/march_rqt_software_check/src/march_rqt_software_check/checks/urdf_check.py @@ -0,0 +1,43 @@ +import rospy +from urdf_parser_py import urdf + +from march_rqt_software_check.color import Color + +from .launch_check import LaunchCheck + + +class URDFCheck(LaunchCheck): + + def __init__(self): + LaunchCheck.__init__(self, 'URDF', 'Please check if the loaded joints are correct', 'march_launch', + 'upload_march_urdf.launch', 10, True) + + def perform(self): + self.launch() + + rospy.sleep(rospy.Duration.from_sec(3)) + try: + robot = urdf.Robot.from_parameter_server() + except KeyError: + self.stop_launch_process() + self.fail_check() + + count = 0 + for joint in robot.joints: + if joint.type != 'fixed': + count += 1 + + self.log('Loaded ' + str(count) + ' joints', Color.Info) + for joint in robot.joints: + if joint.type != 'fixed': + lower = str(round(joint.safety_controller.soft_lower_limit, 4)) + upper = str(round(joint.safety_controller.soft_upper_limit, 4)) + velocity = str(round(joint.limit.velocity, 4)) + effort = str(round(joint.limit.effort, 4)) + msg = joint.name + ': (' + lower + ', ' + upper + ') max velocity: ' + \ + velocity + ' rad/s max effort ' + effort + ' IU' + self.log(msg, Color.Info) + + self.stop_launch_process() + self.passed = True + self.done = True diff --git a/march_rqt_software_check/src/march_rqt_software_check/color.py b/march_rqt_software_check/src/march_rqt_software_check/color.py new file mode 100644 index 0000000..57cc0bd --- /dev/null +++ b/march_rqt_software_check/src/march_rqt_software_check/color.py @@ -0,0 +1,12 @@ +from enum import Enum + + +class Color(Enum): + Debug = '#009100' + Info = '#000000' + Warning = '#b27300' # noqa A003 + Error = '#FF0000' + Fatal = '#FF0000' + Check_Unknown = '#b27300' + Check_Failed = '#FF0000' + Check_Passed = '#009100' diff --git a/march_rqt_software_check/src/march_rqt_software_check/software_check.py b/march_rqt_software_check/src/march_rqt_software_check/software_check.py new file mode 100644 index 0000000..f7732df --- /dev/null +++ b/march_rqt_software_check/src/march_rqt_software_check/software_check.py @@ -0,0 +1,68 @@ +import os + +from python_qt_binding import loadUi +from python_qt_binding.QtWidgets import QPlainTextEdit, QPushButton, QWidget +from qt_gui.plugin import Plugin +import rospkg + +from .check_runner import CheckRunner +from .color import Color + + +class SoftwareCheckPlugin(Plugin): + + def __init__(self, context): + super(SoftwareCheckPlugin, self).__init__(context) + # Give QObjects reasonable names + self.setObjectName('SoftwareCheckPlugin') + + # 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_software_check'), 'resource', 'software_check.ui') + + # Extend the widget with all attributes and children from UI file + loadUi(ui_file, self._widget) + # Give QObjects reasonable names + self._widget.setObjectName('SoftwareCheck') + # 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.log('Welcome to the Software Check.', Color.Debug) + self.log('Select the software checks you want to perform.', Color.Info) + + self.check_runner = CheckRunner(self.log) + + self._widget.Checks.findChild(QPushButton, 'GitBranch') \ + .clicked.connect(lambda: self.run_test('GitBranch')) + self._widget.Checks.findChild(QPushButton, 'GaitFileDirectory') \ + .clicked.connect(lambda: self.run_test('GaitFileDirectory')) + self._widget.Checks.findChild(QPushButton, 'URDF') \ + .clicked.connect(lambda: self.run_test('URDF')) + self._widget.Checks.findChild(QPushButton, 'SlaveCount') \ + .clicked.connect(lambda: self.run_test('SlaveCount')) + + def log(self, msg, level): + self._widget.findChild(QPlainTextEdit, 'Log').appendHtml( + '

' + str(msg) + '

') + scrollbar = self._widget.findChild(QPlainTextEdit, 'Log').verticalScrollBar() + scrollbar.setValue(scrollbar.maximum()) + + def run_test(self, name): + result = self.check_runner.run_check_by_name(name) + self.update_test_button(name, result) + + def update_test_button(self, name, result): + button = self._widget.Checks.findChild(QPushButton, name) + if result: + button.setStyleSheet('background-color: ' + str(Color.Check_Passed.value)) + else: + button.setStyleSheet('background-color: ' + str(Color.Check_Failed.value)) diff --git a/march_rqt_software_check/src/march_rqt_software_check/software_check_thread.py b/march_rqt_software_check/src/march_rqt_software_check/software_check_thread.py new file mode 100644 index 0000000..1dcad43 --- /dev/null +++ b/march_rqt_software_check/src/march_rqt_software_check/software_check_thread.py @@ -0,0 +1,11 @@ +from pyqtgraph.Qt import QtCore + + +class SoftwareCheckThread(QtCore.QThread): + + def __init__(self, check): + QtCore.QThread.__init__(self) + self.check = check + + def run(self): + self.check.perform() diff --git a/multiplot_configurations/actual_desired.xml b/multiplot_configurations/actual_desired.xml new file mode 100644 index 0000000..226a9bb --- /dev/null +++ b/multiplot_configurations/actual_desired.xml @@ -0,0 +1,1020 @@ + + + + #ffffff + #000000 + false + false + + + + + + + Time + 1 + false + + + Position (rad) + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/state + control_msgs/JointTrajectoryControllerState + + + actual/positions/0 + 0 + + 2 + -0.1 + 0 + -1000 + 0 + + /march/controller/trajectory/state + control_msgs/JointTrajectoryControllerState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Actual + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/state + control_msgs/JointTrajectoryControllerState + + + desired/positions/0 + 0 + + 2 + -0.1 + 0 + -1000 + 0 + + /march/controller/trajectory/state + control_msgs/JointTrajectoryControllerState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Desired + + + + true + + 30 + left_ankle + + + + + + Time + 1 + false + + + Position (rad) + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/state + control_msgs/JointTrajectoryControllerState + + + actual/positions/4 + 0 + + 2 + -0.1 + 0 + -1000 + 0 + + /march/controller/trajectory/state + control_msgs/JointTrajectoryControllerState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Actual + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/state + control_msgs/JointTrajectoryControllerState + + + desired/positions/4 + 0 + + 2 + -0.1 + 0 + -1000 + 0 + + /march/controller/trajectory/state + control_msgs/JointTrajectoryControllerState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Desired + + + + true + + 30 + right_ankle + + + + + + + + Time + 1 + false + + + Position (rad) + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/state + control_msgs/JointTrajectoryControllerState + + + actual/positions/1 + 0 + + 2 + -0.1 + 0 + -1000 + 0 + + /march/controller/trajectory/state + control_msgs/JointTrajectoryControllerState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Actual + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/state + control_msgs/JointTrajectoryControllerState + + + desired/positions/1 + 0 + + 2 + -0.1 + 0 + -1000 + 0 + + /march/controller/trajectory/state + control_msgs/JointTrajectoryControllerState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Desired + + + + true + + 30 + left_hip_aa + + + + + + Time + 1 + false + + + Position (rad) + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/state + control_msgs/JointTrajectoryControllerState + + + actual/positions/5 + 0 + + 2 + -0.1 + 0 + -1000 + 0 + + /march/controller/trajectory/state + control_msgs/JointTrajectoryControllerState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Actual + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/state + control_msgs/JointTrajectoryControllerState + + + desired/positions/5 + 0 + + 2 + -0.1 + 0 + -1000 + 0 + + /march/controller/trajectory/state + control_msgs/JointTrajectoryControllerState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Desired + + + + true + + 30 + right_hip_aa + + + + + + + + Time + 1 + false + + + Position (rad) + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/state + control_msgs/JointTrajectoryControllerState + + + actual/positions/2 + 0 + + 2 + -0.1 + 0 + -1000 + 0 + + /march/controller/trajectory/state + control_msgs/JointTrajectoryControllerState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Actual + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/state + control_msgs/JointTrajectoryControllerState + + + desired/positions/2 + 0 + + 2 + -0.1 + 0 + -1000 + 0 + + /march/controller/trajectory/state + control_msgs/JointTrajectoryControllerState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Desired + + + + true + + 30 + left_hip_fe + + + + + + Time + 1 + false + + + Position (rad) + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/state + control_msgs/JointTrajectoryControllerState + + + actual/positions/6 + 0 + + 2 + -0.1 + 0 + -1000 + 0 + + /march/controller/trajectory/state + control_msgs/JointTrajectoryControllerState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Actual + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/state + control_msgs/JointTrajectoryControllerState + + + desired/positions/6 + 0 + + 2 + -0.1 + 0 + -1000 + 0 + + /march/controller/trajectory/state + control_msgs/JointTrajectoryControllerState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Desired + + + + true + + 30 + right_hip_fe + + + + + + + + Time + 1 + false + + + Position (rad) + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/state + control_msgs/JointTrajectoryControllerState + + + actual/positions/3 + 0 + + 2 + -0.1 + 0 + -1000 + 0 + + /march/controller/trajectory/state + control_msgs/JointTrajectoryControllerState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Actual + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/state + control_msgs/JointTrajectoryControllerState + + + desired/positions/3 + 0 + + 2 + -0.1 + 0 + -1000 + 0 + + /march/controller/trajectory/state + control_msgs/JointTrajectoryControllerState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Desired + + + + true + + 30 + left_knee + + + + + + Time + 1 + false + + + Position (rad) + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/state + control_msgs/JointTrajectoryControllerState + + + actual/positions/7 + 0 + + 2 + -0.1 + 0 + -1000 + 0 + + /march/controller/trajectory/state + control_msgs/JointTrajectoryControllerState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Actual + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/state + control_msgs/JointTrajectoryControllerState + + + desired/positions/7 + 0 + + 2 + -0.1 + 0 + -1000 + 0 + + /march/controller/trajectory/state + control_msgs/JointTrajectoryControllerState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Desired + + + + true + + 30 + right_knee + + + + false +
+ diff --git a/multiplot_configurations/controller_output.xml b/multiplot_configurations/controller_output.xml new file mode 100644 index 0000000..7c4e14a --- /dev/null +++ b/multiplot_configurations/controller_output.xml @@ -0,0 +1,878 @@ + + + + #ffffff + #000000 + false + false + + + + + + + Time (s) + 1 + true + + + Effort + 1 + true + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_ankle/state + control_msgs/PidState + + + output + 0 + + 500 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/left_ankle/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_ankle + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_hip_aa/state + control_msgs/PidState + + + output + 0 + + 500 + 0 + 500 + 0 + 0 + + /march/controller/trajectory/gains/left_hip_aa/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_hip_aa + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_hip_fe/state + control_msgs/PidState + + + output + 0 + + 500 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/left_hip_fe/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_hip_fe + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_knee/state + control_msgs/PidState + + + output + 0 + + 500 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/left_knee/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_knee + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_ankle/state + control_msgs/PidState + + + output + 0 + + 500 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_ankle/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_ankle + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_hip_aa/state + control_msgs/PidState + + + output + 0 + + 500 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_hip_aa/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_hip_aa + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_hip_fe/state + control_msgs/PidState + + + output + 0 + + 500 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_hip_fe/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_hip_fe + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_knee/state + control_msgs/PidState + + + output + 0 + + 500 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_knee/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_knee + + + + true + + 30 + Controller Output + + + + + + + + Time (s) + 1 + false + + + Effort + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/after_limit_joint_command + march_shared_resources/AfterLimitJointCommand + + + effort_command/0 + 0 + + 20 + 0 + 0 + -1000 + 0 + + /march/controller/after_limit_joint_command + march_shared_resources/AfterLimitJointCommand + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_ankle + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/after_limit_joint_command + march_shared_resources/AfterLimitJointCommand + + + effort_command/1 + 0 + + 20 + 0 + 0 + -1000 + 0 + + /march/controller/after_limit_joint_command + march_shared_resources/AfterLimitJointCommand + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_hip_aa + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/after_limit_joint_command + march_shared_resources/AfterLimitJointCommand + + + effort_command/2 + 0 + + 20 + 0 + 0 + -1000 + 0 + + /march/controller/after_limit_joint_command + march_shared_resources/AfterLimitJointCommand + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_hip_fe + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/after_limit_joint_command + march_shared_resources/AfterLimitJointCommand + + + effort_command/3 + 0 + + 20 + 0 + 0 + -1000 + 0 + + /march/controller/after_limit_joint_command + march_shared_resources/AfterLimitJointCommand + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_knee + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/after_limit_joint_command + march_shared_resources/AfterLimitJointCommand + + + effort_command/4 + 0 + + 20 + 0 + 0 + -1000 + 0 + + /march/controller/after_limit_joint_command + march_shared_resources/AfterLimitJointCommand + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_ankle + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/after_limit_joint_command + march_shared_resources/AfterLimitJointCommand + + + effort_command/5 + 0 + + 20 + 0 + 0 + -1000 + 0 + + /march/controller/after_limit_joint_command + march_shared_resources/AfterLimitJointCommand + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_hip_aa + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/after_limit_joint_command + march_shared_resources/AfterLimitJointCommand + + + effort_command/6 + 0 + + 20 + 0 + 0 + -1000 + 0 + + /march/controller/after_limit_joint_command + march_shared_resources/AfterLimitJointCommand + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_hip_fe + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/after_limit_joint_command + march_shared_resources/AfterLimitJointCommand + + + effort_command/7 + 0 + + 20 + 0 + 0 + -1000 + 0 + + /march/controller/after_limit_joint_command + march_shared_resources/AfterLimitJointCommand + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_knee + + + + true + + 30 + After limit command + + + + false +
+ diff --git a/multiplot_configurations/controller_per_joint/left_ankle.xml b/multiplot_configurations/controller_per_joint/left_ankle.xml new file mode 100644 index 0000000..db4163c --- /dev/null +++ b/multiplot_configurations/controller_per_joint/left_ankle.xml @@ -0,0 +1,768 @@ + + + + #ffffff + #000000 + false + false + + + + + + + Time + 1 + false + + + Position (rad) + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + feedback/actual/positions/0 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Actual + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + feedback/desired/positions/0 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Desired + + + + true + + 30 + Actual/Desired Position + + + + + + Time + 1 + false + + + Position Error (rad) + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_ankle/state + control_msgs/PidState + + + error + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/left_ankle/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Error + + + + true + + 30 + Position Error + + + + + + + + Time + 1 + false + + + Effort () + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/after_limit_joint_command + march_shared_resources/AfterLimitJointCommand + + + effort_command/0 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/after_limit_joint_command + march_shared_resources/AfterLimitJointCommand + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Effort Command + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/joint_states + sensor_msgs/JointState + + + effort/0 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/joint_states + sensor_msgs/JointState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Actual Effort + + + + true + + 30 + Effort + + + + + + Time + 1 + false + + + Effort + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_ankle/state + control_msgs/PidState + + + p_term + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/left_ankle/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + P-term + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_ankle/state + control_msgs/PidState + + + i_term + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/left_ankle/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + I-term + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_ankle/state + control_msgs/PidState + + + d_term + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/left_ankle/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + D-term + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_ankle/state + control_msgs/PidState + + + output + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/left_ankle/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Total + + + + true + + 30 + PID term + + + + + + + + Time + 1 + false + + + Velocity (rad/s) + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + feedback/actual/velocities/0 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Actual + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + feedback/desired/velocities/0 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Desired + + + + true + + 30 + Actual/Desired Velocity + + + + + + Time + 1 + false + + + Error dot (rad/s) + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_ankle/state + control_msgs/PidState + + + error_dot + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/left_ankle/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Position Error derivative + + + + true + + 30 + Position Error derivative + + + + false +
+ diff --git a/multiplot_configurations/controller_per_joint/left_hip_aa.xml b/multiplot_configurations/controller_per_joint/left_hip_aa.xml new file mode 100644 index 0000000..bd41c57 --- /dev/null +++ b/multiplot_configurations/controller_per_joint/left_hip_aa.xml @@ -0,0 +1,768 @@ + + + + #ffffff + #000000 + false + false + + + + + + + Time + 1 + false + + + Position (rad) + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + feedback/actual/positions/1 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Actual + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + feedback/desired/positions/1 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Desired + + + + true + + 30 + Actual/Desired Position + + + + + + Time + 1 + false + + + Position Error (rad) + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_hip_aa/state + control_msgs/PidState + + + error + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/left_hip_aa/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Error + + + + true + + 30 + Position Error + + + + + + + + Time + 1 + false + + + Effort () + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/after_limit_joint_command + march_shared_resources/AfterLimitJointCommand + + + effort_command/1 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/after_limit_joint_command + march_shared_resources/AfterLimitJointCommand + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Effort Command + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/joint_states + sensor_msgs/JointState + + + effort/1 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/joint_states + sensor_msgs/JointState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Actual Effort + + + + true + + 30 + Effort + + + + + + Time + 1 + false + + + Effort + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_hip_aa/state + control_msgs/PidState + + + p_term + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/left_hip_aa/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + P-term + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_hip_aa/state + control_msgs/PidState + + + i_term + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/left_hip_aa/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + I-term + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_hip_aa/state + control_msgs/PidState + + + d_term + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/left_hip_aa/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + D-term + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_hip_aa/state + control_msgs/PidState + + + output + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/left_hip_aa/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Total + + + + true + + 30 + PID term + + + + + + + + Time + 1 + false + + + Velocity (rad/s) + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + feedback/actual/velocities/1 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Actual + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + feedback/desired/velocities/1 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Desired + + + + true + + 30 + Actual/Desired Velocity + + + + + + Time + 1 + false + + + Error dot (rad/s) + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_hip_aa/state + control_msgs/PidState + + + error_dot + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/left_hip_aa/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Position Error derivative + + + + true + + 30 + Position Error derivative + + + + false +
+ diff --git a/multiplot_configurations/controller_per_joint/left_hip_fe.xml b/multiplot_configurations/controller_per_joint/left_hip_fe.xml new file mode 100644 index 0000000..5a940df --- /dev/null +++ b/multiplot_configurations/controller_per_joint/left_hip_fe.xml @@ -0,0 +1,768 @@ + + + + #ffffff + #000000 + false + false + + + + + + + Time + 1 + false + + + Position (rad) + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + feedback/actual/positions/2 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Actual + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + feedback/desired/positions/2 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Desired + + + + true + + 30 + Actual/Desired Position + + + + + + Time + 1 + false + + + Position Error (rad) + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_hip_fe/state + control_msgs/PidState + + + error + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/left_hip_fe/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Error + + + + true + + 30 + Position Error + + + + + + + + Time + 1 + false + + + Effort () + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/after_limit_joint_command + march_shared_resources/AfterLimitJointCommand + + + effort_command/2 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/after_limit_joint_command + march_shared_resources/AfterLimitJointCommand + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Effort Command + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/joint_states + sensor_msgs/JointState + + + effort/2 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/joint_states + sensor_msgs/JointState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Actual Effort + + + + true + + 30 + Effort + + + + + + Time + 1 + false + + + Effort + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_hip_fe/state + control_msgs/PidState + + + p_term + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/left_hip_fe/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + P-term + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_hip_fe/state + control_msgs/PidState + + + i_term + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/left_hip_fe/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + I-term + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_hip_fe/state + control_msgs/PidState + + + d_term + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/left_hip_fe/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + D-term + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_hip_fe/state + control_msgs/PidState + + + output + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/left_hip_fe/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Total + + + + true + + 30 + PID term + + + + + + + + Time + 1 + false + + + Velocity (rad/s) + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + feedback/actual/velocities/2 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Actual + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + feedback/desired/velocities/2 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Desired + + + + true + + 30 + Actual/Desired Velocity + + + + + + Time + 1 + false + + + Error dot (rad/s) + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_hip_fe/state + control_msgs/PidState + + + error_dot + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/left_hip_fe/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Position Error derivative + + + + true + + 30 + Position Error derivative + + + + false +
+ diff --git a/multiplot_configurations/controller_per_joint/left_knee.xml b/multiplot_configurations/controller_per_joint/left_knee.xml new file mode 100644 index 0000000..d510558 --- /dev/null +++ b/multiplot_configurations/controller_per_joint/left_knee.xml @@ -0,0 +1,768 @@ + + + + #ffffff + #000000 + false + false + + + + + + + Time + 1 + false + + + Position (rad) + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + feedback/actual/positions/3 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Actual + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + feedback/desired/positions/3 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Desired + + + + true + + 30 + Actual/Desired Position + + + + + + Time + 1 + false + + + Position Error (rad) + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_knee/state + control_msgs/PidState + + + error + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/left_knee/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Error + + + + true + + 30 + Position Error + + + + + + + + Time + 1 + false + + + Effort () + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/after_limit_joint_command + march_shared_resources/AfterLimitJointCommand + + + effort_command/3 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/after_limit_joint_command + march_shared_resources/AfterLimitJointCommand + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Effort Command + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/joint_states + sensor_msgs/JointState + + + effort/3 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/joint_states + sensor_msgs/JointState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Actual Effort + + + + true + + 30 + Effort + + + + + + Time + 1 + false + + + Effort + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_knee/state + control_msgs/PidState + + + p_term + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/left_knee/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + P-term + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_knee/state + control_msgs/PidState + + + i_term + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/left_knee/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + I-term + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_knee/state + control_msgs/PidState + + + d_term + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/left_knee/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + D-term + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_knee/state + control_msgs/PidState + + + output + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/left_knee/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Total + + + + true + + 30 + PID term + + + + + + + + Time + 1 + false + + + Velocity (rad/s) + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + feedback/actual/velocities/3 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Actual + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + feedback/desired/velocities/3 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Desired + + + + true + + 30 + Actual/Desired Velocity + + + + + + Time + 1 + false + + + Error dot (rad/s) + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_knee/state + control_msgs/PidState + + + error_dot + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/left_knee/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Position Error derivative + + + + true + + 30 + Position Error derivative + + + + false +
+ diff --git a/multiplot_configurations/controller_per_joint/right_ankle.xml b/multiplot_configurations/controller_per_joint/right_ankle.xml new file mode 100644 index 0000000..613c7db --- /dev/null +++ b/multiplot_configurations/controller_per_joint/right_ankle.xml @@ -0,0 +1,768 @@ + + + + #ffffff + #000000 + false + false + + + + + + + Time + 1 + false + + + Position (rad) + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + feedback/actual/positions/4 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Actual + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + feedback/desired/positions/4 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Desired + + + + true + + 30 + Actual/Desired Position + + + + + + Time + 1 + false + + + Position Error (rad) + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_ankle/state + control_msgs/PidState + + + error + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_ankle/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Error + + + + true + + 30 + Position Error + + + + + + + + Time + 1 + false + + + Effort () + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/after_limit_joint_command + march_shared_resources/AfterLimitJointCommand + + + effort_command/4 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/after_limit_joint_command + march_shared_resources/AfterLimitJointCommand + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Effort Command + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/joint_states + sensor_msgs/JointState + + + effort/4 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/joint_states + sensor_msgs/JointState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Actual Effort + + + + true + + 30 + Effort + + + + + + Time + 1 + false + + + Effort + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_ankle/state + control_msgs/PidState + + + p_term + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_ankle/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + P-term + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_ankle/state + control_msgs/PidState + + + i_term + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_ankle/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + I-term + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_ankle/state + control_msgs/PidState + + + d_term + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_ankle/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + D-term + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_ankle/state + control_msgs/PidState + + + output + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_ankle/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Total + + + + true + + 30 + PID term + + + + + + + + Time + 1 + false + + + Velocity (rad/s) + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + feedback/actual/velocities/4 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Actual + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + feedback/desired/velocities/4 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Desired + + + + true + + 30 + Actual/Desired Velocity + + + + + + Time + 1 + false + + + Error dot (rad/s) + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_ankle/state + control_msgs/PidState + + + error_dot + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_ankle/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Position Error derivative + + + + true + + 30 + Position Error derivative + + + + false +
+ diff --git a/multiplot_configurations/controller_per_joint/right_hip_aa.xml b/multiplot_configurations/controller_per_joint/right_hip_aa.xml new file mode 100644 index 0000000..01d5cb6 --- /dev/null +++ b/multiplot_configurations/controller_per_joint/right_hip_aa.xml @@ -0,0 +1,768 @@ + + + + #ffffff + #000000 + false + false + + + + + + + Time + 1 + false + + + Position (rad) + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + feedback/actual/positions/5 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Actual + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + feedback/desired/positions/5 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Desired + + + + true + + 30 + Actual/Desired Position + + + + + + Time + 1 + false + + + Position Error (rad) + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_hip_aa/state + control_msgs/PidState + + + error + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_hip_aa/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Error + + + + true + + 30 + Position Error + + + + + + + + Time + 1 + false + + + Effort () + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/after_limit_joint_command + march_shared_resources/AfterLimitJointCommand + + + effort_command/5 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/after_limit_joint_command + march_shared_resources/AfterLimitJointCommand + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Effort Command + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/joint_states + sensor_msgs/JointState + + + effort/5 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/joint_states + sensor_msgs/JointState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Actual Effort + + + + true + + 30 + Effort + + + + + + Time + 1 + false + + + Effort + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_hip_aa/state + control_msgs/PidState + + + p_term + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_hip_aa/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + P-term + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_hip_aa/state + control_msgs/PidState + + + i_term + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_hip_aa/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + I-term + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_hip_aa/state + control_msgs/PidState + + + d_term + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_hip_aa/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + D-term + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_hip_aa/state + control_msgs/PidState + + + output + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_hip_aa/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Total + + + + true + + 30 + PID term + + + + + + + + Time + 1 + false + + + Velocity (rad/s) + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + feedback/actual/velocities/5 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Actual + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + feedback/desired/velocities/5 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Desired + + + + true + + 30 + Actual/Desired Velocity + + + + + + Time + 1 + false + + + Error dot (rad/s) + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_hip_aa/state + control_msgs/PidState + + + error_dot + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_hip_aa/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Position Error derivative + + + + true + + 30 + Position Error derivative + + + + false +
+ diff --git a/multiplot_configurations/controller_per_joint/right_hip_fe.xml b/multiplot_configurations/controller_per_joint/right_hip_fe.xml new file mode 100644 index 0000000..38ad1b3 --- /dev/null +++ b/multiplot_configurations/controller_per_joint/right_hip_fe.xml @@ -0,0 +1,768 @@ + + + + #ffffff + #000000 + false + false + + + + + + + Time + 1 + false + + + Position (rad) + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + feedback/actual/positions/6 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Actual + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + feedback/desired/positions/6 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Desired + + + + true + + 30 + Actual/Desired Position + + + + + + Time + 1 + false + + + Position Error (rad) + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_hip_fe/state + control_msgs/PidState + + + error + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_hip_fe/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Error + + + + true + + 30 + Position Error + + + + + + + + Time + 1 + false + + + Effort () + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/after_limit_joint_command + march_shared_resources/AfterLimitJointCommand + + + effort_command/6 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/after_limit_joint_command + march_shared_resources/AfterLimitJointCommand + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Effort Command + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/joint_states + sensor_msgs/JointState + + + effort/6 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/joint_states + sensor_msgs/JointState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Actual Effort + + + + true + + 30 + Effort + + + + + + Time + 1 + false + + + Effort + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_hip_fe/state + control_msgs/PidState + + + p_term + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_hip_fe/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + P-term + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_hip_fe/state + control_msgs/PidState + + + i_term + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_hip_fe/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + I-term + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_hip_fe/state + control_msgs/PidState + + + d_term + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_hip_fe/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + D-term + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_hip_fe/state + control_msgs/PidState + + + output + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_hip_fe/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Total + + + + true + + 30 + PID term + + + + + + + + Time + 1 + false + + + Velocity (rad/s) + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + feedback/actual/velocities/6 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Actual + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + feedback/desired/velocities/6 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Desired + + + + true + + 30 + Actual/Desired Velocity + + + + + + Time + 1 + false + + + Error dot (rad/s) + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_hip_fe/state + control_msgs/PidState + + + error_dot + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_hip_fe/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Position Error derivative + + + + true + + 30 + Position Error derivative + + + + false +
+ diff --git a/multiplot_configurations/controller_per_joint/right_knee.xml b/multiplot_configurations/controller_per_joint/right_knee.xml new file mode 100644 index 0000000..6429314 --- /dev/null +++ b/multiplot_configurations/controller_per_joint/right_knee.xml @@ -0,0 +1,768 @@ + + + + #ffffff + #000000 + false + false + + + + + + + Time + 1 + false + + + Position (rad) + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + feedback/actual/positions/7 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Actual + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + feedback/desired/positions/7 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Desired + + + + true + + 30 + Actual/Desired Position + + + + + + Time + 1 + false + + + Position Error (rad) + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_knee/state + control_msgs/PidState + + + error + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_knee/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Error + + + + true + + 30 + Position Error + + + + + + + + Time + 1 + false + + + Effort () + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/after_limit_joint_command + march_shared_resources/AfterLimitJointCommand + + + effort_command/7 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/after_limit_joint_command + march_shared_resources/AfterLimitJointCommand + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Effort Command + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/joint_states + sensor_msgs/JointState + + + effort/7 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/joint_states + sensor_msgs/JointState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Actual Effort + + + + true + + 30 + Effort + + + + + + Time + 1 + false + + + Effort + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_knee/state + control_msgs/PidState + + + p_term + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_knee/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + P-term + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_knee/state + control_msgs/PidState + + + i_term + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_knee/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + I-term + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_knee/state + control_msgs/PidState + + + d_term + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_knee/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + D-term + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_knee/state + control_msgs/PidState + + + output + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_knee/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Total + + + + true + + 30 + PID term + + + + + + + + Time + 1 + false + + + Velocity (rad/s) + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + feedback/actual/velocities/7 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Actual + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + feedback/desired/velocities/7 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/follow_joint_trajectory/feedback + control_msgs/FollowJointTrajectoryActionFeedback + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Desired + + + + true + + 30 + Actual/Desired Velocity + + + + + + Time + 1 + false + + + Error dot (rad/s) + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_knee/state + control_msgs/PidState + + + error_dot + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_knee/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Position Error derivative + + + + true + + 30 + Position Error derivative + + + + false +
+ diff --git a/multiplot_configurations/joint_values.xml b/multiplot_configurations/joint_values.xml new file mode 100644 index 0000000..4b580ca --- /dev/null +++ b/multiplot_configurations/joint_values.xml @@ -0,0 +1,1740 @@ + + + + #ffffff + #000000 + false + false + + + + + + + Time + 1 + false + + + Position (rad) + 1 + false + + + + + + + + controller_output/header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/joint_values + march_shared_resources/JointValues + + + controller_output/actual/positions/0 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/joint_values + march_shared_resources/JointValues + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_ankle + + + + + controller_output/header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/joint_values + march_shared_resources/JointValues + + + controller_output/actual/positions/1 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/joint_values + march_shared_resources/JointValues + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_hip_aa + + + + + controller_output/header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/joint_values + march_shared_resources/JointValues + + + controller_output/actual/positions/2 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/joint_values + march_shared_resources/JointValues + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_hip_fe + + + + + controller_output/header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/joint_values + march_shared_resources/JointValues + + + controller_output/actual/positions/3 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/joint_values + march_shared_resources/JointValues + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_knee + + + + + controller_output/header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/joint_values + march_shared_resources/JointValues + + + controller_output/actual/positions/4 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/joint_values + march_shared_resources/JointValues + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_ankle + + + + + controller_output/header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/joint_values + march_shared_resources/JointValues + + + controller_output/actual/positions/5 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/joint_values + march_shared_resources/JointValues + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_hip_aa + + + + + controller_output/header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/joint_values + march_shared_resources/JointValues + + + controller_output/actual/positions/6 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/joint_values + march_shared_resources/JointValues + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_hip_fe + + + + + controller_output/header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/joint_values + march_shared_resources/JointValues + + + controller_output/actual/positions/7 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/joint_values + march_shared_resources/JointValues + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_knee + + + + true + + 30 + Position + + + + + + Time + 1 + false + + + Velocity (rad/s) + 1 + false + + + + + + + + controller_output/header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/joint_values + march_shared_resources/JointValues + + + velocities/0 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/joint_values + march_shared_resources/JointValues + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_ankle + + + + + controller_output/header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/joint_values + march_shared_resources/JointValues + + + velocities/1 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/joint_values + march_shared_resources/JointValues + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_hip_aa + + + + + controller_output/header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/joint_values + march_shared_resources/JointValues + + + velocities/2 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/joint_values + march_shared_resources/JointValues + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_hip_fe + + + + + controller_output/header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/joint_values + march_shared_resources/JointValues + + + velocities/3 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/joint_values + march_shared_resources/JointValues + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_knee + + + + + controller_output/header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/joint_values + march_shared_resources/JointValues + + + velocities/4 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/joint_values + march_shared_resources/JointValues + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_ankle + + + + + controller_output/header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/joint_values + march_shared_resources/JointValues + + + velocities/5 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/joint_values + march_shared_resources/JointValues + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_hip_aa + + + + + controller_output/header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/joint_values + march_shared_resources/JointValues + + + velocities/6 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/joint_values + march_shared_resources/JointValues + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_hip_fe + + + + + controller_output/header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/joint_values + march_shared_resources/JointValues + + + velocities/7 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/joint_values + march_shared_resources/JointValues + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_knee + + + + true + + 30 + Velocitiy + + + + + + + + Time + 1 + false + + + Acclerations (rad/s^2) + 1 + false + + + + + + + + controller_output/header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/joint_values + march_shared_resources/JointValues + + + accelerations/0 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/joint_values + march_shared_resources/JointValues + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_ankle + + + + + controller_output/header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/joint_values + march_shared_resources/JointValues + + + accelerations/1 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/joint_values + march_shared_resources/JointValues + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_hip_aa + + + + + controller_output/header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/joint_values + march_shared_resources/JointValues + + + accelerations/2 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/joint_values + march_shared_resources/JointValues + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_hip_fe + + + + + controller_output/header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/joint_values + march_shared_resources/JointValues + + + accelerations/3 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/joint_values + march_shared_resources/JointValues + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_knee + + + + + controller_output/header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/joint_values + march_shared_resources/JointValues + + + accelerations/4 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/joint_values + march_shared_resources/JointValues + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_knee + + + + + controller_output/header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/joint_values + march_shared_resources/JointValues + + + accelerations/5 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/joint_values + march_shared_resources/JointValues + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_hip_aa + + + + + controller_output/header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/joint_values + march_shared_resources/JointValues + + + accelerations/6 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/joint_values + march_shared_resources/JointValues + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_hip_fe + + + + + controller_output/header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/joint_values + march_shared_resources/JointValues + + + accelerations/7 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/joint_values + march_shared_resources/JointValues + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_knee + + + + true + + 30 + Accelerations + + + + + + Time (s) + 1 + false + + + Jerk (rad/s^3) + 1 + false + + + + + + + + controller_output/header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/joint_values + march_shared_resources/JointValues + + + jerks/0 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/joint_values + march_shared_resources/JointValues + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_ankle + + + + + controller_output/header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/joint_values + march_shared_resources/JointValues + + + jerks/1 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/joint_values + march_shared_resources/JointValues + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_hip_aa + + + + + controller_output/header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/joint_values + march_shared_resources/JointValues + + + jerks/2 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/joint_values + march_shared_resources/JointValues + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_hip_fe + + + + + controller_output/header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/joint_values + march_shared_resources/JointValues + + + jerks/3 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/joint_values + march_shared_resources/JointValues + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_knee + + + + + controller_output/header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/joint_values + march_shared_resources/JointValues + + + jerks/4 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/joint_values + march_shared_resources/JointValues + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_ankle + + + + + controller_output/header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/joint_values + march_shared_resources/JointValues + + + jerks/5 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/joint_values + march_shared_resources/JointValues + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_hip_aa + + + + + controller_output/header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/joint_values + march_shared_resources/JointValues + + + jerks/6 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/joint_values + march_shared_resources/JointValues + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_hip_fe + + + + + controller_output/header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/joint_values + march_shared_resources/JointValues + + + jerks/7 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/joint_values + march_shared_resources/JointValues + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_knee + + + + true + + 30 + Jerk + + + + false +
+ diff --git a/multiplot_configurations/pid_terms.xml b/multiplot_configurations/pid_terms.xml new file mode 100644 index 0000000..9772ab1 --- /dev/null +++ b/multiplot_configurations/pid_terms.xml @@ -0,0 +1,1428 @@ + + + + #ffffff + #000000 + false + false + + + + + + + Time (s) + 1 + false + + + Controller output + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_ankle/state + control_msgs/PidState + + + p_term + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/left_ankle/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + P-term + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_ankle/state + control_msgs/PidState + + + i_term + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/left_ankle/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + I-term + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_ankle/state + control_msgs/PidState + + + d_term + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/left_ankle/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + D-term + + + + true + + 30 + left_ankle + + + + + + Time (s) + 1 + false + + + Controller output + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_ankle/state + control_msgs/PidState + + + p_term + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_ankle/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + P-term + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_ankle/state + control_msgs/PidState + + + i_term + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_ankle/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + I-term + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_ankle/state + control_msgs/PidState + + + d_term + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_ankle/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + D-term + + + + true + + 30 + right_ankle + + + + + + + + Time (s) + 1 + false + + + Controller output + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_hip_aa/state + control_msgs/PidState + + + p_term + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/left_hip_aa/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + P-term + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_hip_aa/state + control_msgs/PidState + + + i_term + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/left_hip_aa/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + I-term + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_hip_aa/state + control_msgs/PidState + + + d_term + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/left_hip_aa/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + D-term + + + + true + + 30 + left_hip_aa + + + + + + Time (s) + 1 + false + + + Controller output + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_hip_aa/state + control_msgs/PidState + + + p_term + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_hip_aa/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + P-term + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_hip_aa/state + control_msgs/PidState + + + i_term + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_hip_aa/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + I-term + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_hip_aa/state + control_msgs/PidState + + + d_term + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_hip_aa/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + D-term + + + + true + + 30 + right_hip_aa + + + + + + + + Time (s) + 1 + false + + + Controller output + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_hip_fe/state + control_msgs/PidState + + + p_term + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/left_hip_fe/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + P-term + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_hip_fe/state + control_msgs/PidState + + + i_term + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/left_hip_fe/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + I-term + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_hip_fe/state + control_msgs/PidState + + + d_term + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/left_hip_fe/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + D-term + + + + true + + 30 + left_hip_fe + + + + + + Time (s) + 1 + false + + + Controller output + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_hip_fe/state + control_msgs/PidState + + + p_term + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_hip_fe/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + P-term + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_hip_fe/state + control_msgs/PidState + + + i_term + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_hip_fe/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + I-term + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_hip_fe/state + control_msgs/PidState + + + d_term + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_hip_fe/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + D-term + + + + true + + 30 + right_hip_fe + + + + + + + + Time (s) + 1 + false + + + Controller output + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_knee/state + control_msgs/PidState + + + p_term + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/left_knee/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + P-term + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_knee/state + control_msgs/PidState + + + i_term + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/left_knee/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + I-term + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_knee/state + control_msgs/PidState + + + d_term + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/left_knee/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + D-term + + + + true + + 30 + left_knee + + + + + + Time (s) + 1 + false + + + Controller output + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_knee/state + control_msgs/PidState + + + p_term + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_knee/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + P-term + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_knee/state + control_msgs/PidState + + + i_term + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_knee/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + I-term + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_knee/state + control_msgs/PidState + + + d_term + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_knee/state + control_msgs/PidState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + D-term + + + + true + + 30 + right_knee + + + + false +
+ diff --git a/multiplot_configurations/pid_updates.xml b/multiplot_configurations/pid_updates.xml new file mode 100644 index 0000000..d97384e --- /dev/null +++ b/multiplot_configurations/pid_updates.xml @@ -0,0 +1,1464 @@ + + + + #ffffff + #000000 + false + false + + + + + + + Time (s) + 1 + false + + + P-value + 1 + false + + + + + + + + + 1 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_ankle/parameter_updates + dynamic_reconfigure/Config + + + doubles/0/value + 0 + + 500 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/left_ankle/parameter_updates + dynamic_reconfigure/Config + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_ankle + + + + + + 1 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_ankle/parameter_updates + dynamic_reconfigure/Config + + + doubles/0/value + 0 + + 500 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/left_ankle/parameter_updates + dynamic_reconfigure/Config + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_ankle + + + + + + 1 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_hip_aa/parameter_updates + dynamic_reconfigure/Config + + + doubles/0/value + 0 + + 500 + 0 + 500 + 0 + 0 + + /march/controller/trajectory/gains/left_hip_aa/parameter_updates + dynamic_reconfigure/Config + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_hip_aa + + + + + + 1 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_hip_fe/parameter_updates + dynamic_reconfigure/Config + + + doubles/0/value + 0 + + 500 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/left_hip_fe/parameter_updates + dynamic_reconfigure/Config + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_hip_fe + + + + + + 1 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_knee/parameter_updates + dynamic_reconfigure/Config + + + doubles/0/value + 0 + + 500 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/left_knee/parameter_updates + dynamic_reconfigure/Config + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_knee + + + + + + 1 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_ankle/parameter_updates + dynamic_reconfigure/Config + + + doubles/0/value + 0 + + 500 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_ankle/parameter_updates + dynamic_reconfigure/Config + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_ankle + + + + + + 1 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_hip_aa/parameter_updates + dynamic_reconfigure/Config + + + doubles/0/value + 0 + + 500 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_hip_aa/parameter_updates + dynamic_reconfigure/Config + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_hip_aa + + + + + + 1 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_hip_fe/parameter_updates + dynamic_reconfigure/Config + + + doubles/0/value + 0 + + 500 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_hip_fe/parameter_updates + dynamic_reconfigure/Config + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_hip_fe + + + + + + 1 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_knee/parameter_updates + dynamic_reconfigure/Config + + + doubles/0/value + 0 + + 500 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_knee/parameter_updates + dynamic_reconfigure/Config + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_knee + + + + true + + 30 + P + + + + + + + + Time (s) + 1 + false + + + I-value + 1 + false + + + + + + + + + 1 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_ankle/parameter_updates + dynamic_reconfigure/Config + + + doubles/1/value + 0 + + 20 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/left_ankle/parameter_updates + dynamic_reconfigure/Config + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_ankle + + + + + + 1 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_ankle/parameter_updates + dynamic_reconfigure/Config + + + doubles/1/value + 0 + + 20 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/left_ankle/parameter_updates + dynamic_reconfigure/Config + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_ankle + + + + + + 1 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_hip_aa/parameter_updates + dynamic_reconfigure/Config + + + doubles/1/value + 0 + + 20 + 0 + 500 + 0 + 0 + + /march/controller/trajectory/gains/left_hip_aa/parameter_updates + dynamic_reconfigure/Config + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_hip_aa + + + + + + 1 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_hip_fe/parameter_updates + dynamic_reconfigure/Config + + + doubles/1/value + 0 + + 20 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/left_hip_fe/parameter_updates + dynamic_reconfigure/Config + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_hip_fe + + + + + + 1 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_knee/parameter_updates + dynamic_reconfigure/Config + + + doubles/1/value + 0 + + 20 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/left_knee/parameter_updates + dynamic_reconfigure/Config + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_knee + + + + + + 1 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_ankle/parameter_updates + dynamic_reconfigure/Config + + + doubles/1/value + 0 + + 20 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_ankle/parameter_updates + dynamic_reconfigure/Config + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_ankle + + + + + + 1 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_hip_aa/parameter_updates + dynamic_reconfigure/Config + + + doubles/1/value + 0 + + 20 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_hip_aa/parameter_updates + dynamic_reconfigure/Config + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_hip_aa + + + + + + 1 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_hip_fe/parameter_updates + dynamic_reconfigure/Config + + + doubles/1/value + 0 + + 20 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_hip_fe/parameter_updates + dynamic_reconfigure/Config + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_hip_fe + + + + + + 1 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_knee/parameter_updates + dynamic_reconfigure/Config + + + doubles/1/value + 0 + + 20 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_knee/parameter_updates + dynamic_reconfigure/Config + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_knee + + + + true + + 30 + I + + + + + + + + Time (s) + 1 + false + + + D-value + 1 + false + + + + + + + + + 1 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_ankle/parameter_updates + dynamic_reconfigure/Config + + + doubles/2/value + 0 + + 20 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/left_ankle/parameter_updates + dynamic_reconfigure/Config + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_ankle + + + + + + 1 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_ankle/parameter_updates + dynamic_reconfigure/Config + + + doubles/2/value + 0 + + 20 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/left_ankle/parameter_updates + dynamic_reconfigure/Config + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_ankle + + + + + + 1 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_hip_aa/parameter_updates + dynamic_reconfigure/Config + + + doubles/2/value + 0 + + 20 + 0 + 500 + 0 + 0 + + /march/controller/trajectory/gains/left_hip_aa/parameter_updates + dynamic_reconfigure/Config + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_hip_aa + + + + + + 1 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_hip_fe/parameter_updates + dynamic_reconfigure/Config + + + doubles/2/value + 0 + + 20 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/left_hip_fe/parameter_updates + dynamic_reconfigure/Config + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_hip_fe + + + + + + 1 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/left_knee/parameter_updates + dynamic_reconfigure/Config + + + doubles/2/value + 0 + + 20 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/left_knee/parameter_updates + dynamic_reconfigure/Config + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_knee + + + + + + 1 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_ankle/parameter_updates + dynamic_reconfigure/Config + + + doubles/2/value + 0 + + 20 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_ankle/parameter_updates + dynamic_reconfigure/Config + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_ankle + + + + + + 1 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_hip_aa/parameter_updates + dynamic_reconfigure/Config + + + doubles/2/value + 0 + + 20 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_hip_aa/parameter_updates + dynamic_reconfigure/Config + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_hip_aa + + + + + + 1 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_hip_fe/parameter_updates + dynamic_reconfigure/Config + + + doubles/2/value + 0 + + 20 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_hip_fe/parameter_updates + dynamic_reconfigure/Config + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_hip_fe + + + + + + 1 + + 1000 + 0 + 0 + -10 + 2 + + /march/controller/trajectory/gains/right_knee/parameter_updates + dynamic_reconfigure/Config + + + doubles/2/value + 0 + + 20 + 0 + 0 + -1000 + 0 + + /march/controller/trajectory/gains/right_knee/parameter_updates + dynamic_reconfigure/Config + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_knee + + + + true + + 30 + D + + + + false +
+ diff --git a/multiplot_configurations/temperature.xml b/multiplot_configurations/temperature.xml new file mode 100644 index 0000000..ec57821 --- /dev/null +++ b/multiplot_configurations/temperature.xml @@ -0,0 +1,445 @@ + + + + #ffffff + #000000 + false + false + + + + + + + Time (s) + 1 + false + + + Temperactuer (C) + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -60 + 2 + + /march/temperature/left_ankle + sensor_msgs/Temperature + + + temperature + 0 + + 100 + 0 + 0 + -1000 + 1 + + /march/temperature/left_ankle + sensor_msgs/Temperature + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_ankle + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -60 + 2 + + /march/temperature/left_hip_aa + sensor_msgs/Temperature + + + temperature + 0 + + 100 + 0 + 0 + -1000 + 1 + + /march/temperature/left_hip_aa + sensor_msgs/Temperature + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_hip_aa + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -60 + 2 + + /march/temperature/left_hip_fe + sensor_msgs/Temperature + + + temperature + 0 + + 100 + 0 + 0 + -1000 + 1 + + /march/temperature/left_hip_fe + sensor_msgs/Temperature + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_hip_fe + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -60 + 2 + + /march/temperature/left_knee + sensor_msgs/Temperature + + + temperature + 0 + + 100 + 0 + 0 + -1000 + 1 + + /march/temperature/left_knee + sensor_msgs/Temperature + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_knee + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -60 + 2 + + /march/temperature/right_ankle + sensor_msgs/Temperature + + + temperature + 0 + + 100 + 0 + 0 + -1000 + 1 + + /march/temperature/right_ankle + sensor_msgs/Temperature + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_ankle + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -60 + 2 + + /march/temperature/right_hip_aa + sensor_msgs/Temperature + + + temperature + 0 + + 100 + 0 + 0 + -1000 + 1 + + /march/temperature/right_hip_aa + sensor_msgs/Temperature + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_hip_aa + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -60 + 2 + + /march/temperature/right_hip_fe + sensor_msgs/Temperature + + + temperature + 0 + + 100 + 0 + 0 + -1000 + 1 + + /march/temperature/right_hip_fe + sensor_msgs/Temperature + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_hip_fe + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -60 + 2 + + /march/temperature/right_knee + sensor_msgs/Temperature + + + temperature + 0 + + 100 + 0 + 0 + -1000 + 1 + + /march/temperature/right_knee + sensor_msgs/Temperature + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_knee + + + + true + + 5 + Temperature + + + + false +
+ diff --git a/multiplot_configurations/velocity.xml b/multiplot_configurations/velocity.xml new file mode 100644 index 0000000..43798cf --- /dev/null +++ b/multiplot_configurations/velocity.xml @@ -0,0 +1,1311 @@ + + + + #ffffff + #000000 + false + false + + + + + + + time (s) + 1 + false + + + velocity (IU/s) + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/imc_states + march_shared_resources/ImcState + + + absolute_velocity/0 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/imc_states + march_shared_resources/ImcState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_ankle + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/imc_states + march_shared_resources/ImcState + + + absolute_velocity/1 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/imc_states + march_shared_resources/ImcState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_hip_aa + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/imc_states + march_shared_resources/ImcState + + + absolute_velocity/2 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/imc_states + march_shared_resources/ImcState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_hip_fe + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/imc_states + march_shared_resources/ImcState + + + absolute_velocity/3 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/imc_states + march_shared_resources/ImcState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_knee + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/imc_states + march_shared_resources/ImcState + + + absolute_velocity/4 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/imc_states + march_shared_resources/ImcState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_ankle + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/imc_states + march_shared_resources/ImcState + + + absolute_velocity/4 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/imc_states + march_shared_resources/ImcState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_hip_aa + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/imc_states + march_shared_resources/ImcState + + + absolute_velocity/6 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/imc_states + march_shared_resources/ImcState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_hip_fe + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/imc_states + march_shared_resources/ImcState + + + absolute_velocity/7 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/imc_states + march_shared_resources/ImcState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_knee + + + + true + + 30 + Velocity from absolute encoder + + + + + + + + time (s) + 1 + false + + + velocity (IU/s) + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/imc_states + march_shared_resources/ImcState + + + incremental_velocity/0 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/imc_states + march_shared_resources/ImcState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_ankle + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/imc_states + march_shared_resources/ImcState + + + incremental_velocity/1 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/imc_states + march_shared_resources/ImcState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_hip_aa + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/imc_states + march_shared_resources/ImcState + + + incremental_velocity/2 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/imc_states + march_shared_resources/ImcState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_hip_fe + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/imc_states + march_shared_resources/ImcState + + + incremental_velocity/3 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/imc_states + march_shared_resources/ImcState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_knee + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/imc_states + march_shared_resources/ImcState + + + incremental_velocity/4 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/imc_states + march_shared_resources/ImcState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_ankle + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/imc_states + march_shared_resources/ImcState + + + incremental_velocity/6 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/imc_states + march_shared_resources/ImcState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_hip_aa + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/imc_states + march_shared_resources/ImcState + + + incremental_velocity/6 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/imc_states + march_shared_resources/ImcState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_hip_fe + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/imc_states + march_shared_resources/ImcState + + + incremental_velocity/7 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/imc_states + march_shared_resources/ImcState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_knee + + + + true + + 30 + Velocity from incremental encoder + + + + + + + + time (s) + 1 + false + + + velocity (rad/s) + 1 + false + + + + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/joint_states + sensor_msgs/JointState + + + velocity/0 + 0 + + 2.5 + -2.5 + 0 + -1000 + 1 + + /march/joint_states + sensor_msgs/JointState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_ankle + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/joint_states + sensor_msgs/JointState + + + velocity/2 + 0 + + 2.5 + -2.5 + 0 + -1000 + 1 + + /march/joint_states + sensor_msgs/JointState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_hip_aa + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/joint_states + sensor_msgs/JointState + + + velocity/2 + 0 + + 0.5 + -2.5 + 0 + -1000 + 1 + + /march/joint_states + sensor_msgs/JointState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_hip_fe + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/joint_states + sensor_msgs/JointState + + + velocity/3 + 0 + + 2 + -2.5 + 0 + -1000 + 1 + + /march/joint_states + sensor_msgs/JointState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + left_knee + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/joint_states + sensor_msgs/JointState + + + velocity/4 + 0 + + 2.5 + -2.5 + 0 + -1000 + 1 + + /march/joint_states + sensor_msgs/JointState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_ankle + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/joint_states + sensor_msgs/JointState + + + velocity/5 + 0 + + 2.5 + -2.5 + 0 + -1000 + 1 + + /march/joint_states + sensor_msgs/JointState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_hip_aa + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/joint_states + sensor_msgs/JointState + + + velocity/6 + 0 + + 2.5 + -2.5 + 0 + -1000 + 1 + + /march/joint_states + sensor_msgs/JointState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_hip_fe + + + + + header/stamp + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /march/joint_states + sensor_msgs/JointState + + + velocity/7 + 0 + + 2.5 + -2.5 + 0 + -1000 + 1 + + /march/joint_states + sensor_msgs/JointState + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + right_knee + + + + true + + 30 + Velocity + + + + false +
+