Stretch Dashboard

stretch_dashboard/CMakeLists.txt

@@ -0,0 +1,14 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(stretch_dashboard)
+
+find_package(catkin REQUIRED)
+
+catkin_python_setup()
+
+catkin_package()
+install(FILES plugin.xml
+        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+catkin_install_python(PROGRAMS scripts/dashboard
+                      DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)

stretch_dashboard/README.md

@@ -0,0 +1,13 @@
+![Stretch ROS banner](../images/banner.png)
+
+# Stretch Dashboard
+![screenshot](Screenshot.png)
+
+ * Gear icon used for calibration
+ * Next button used for changing modes
+ * Battery shows voltage/current as a tooltip
+
+## To Run
+    rosrun stretch_dashboard dashboard
+
+*Note*: The first time you run it you may need to add the `--force-discover` arg to the end of the above command.

stretch_dashboard/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>stretch_dashboard</name>
+  <version>0.2.0</version>
+  <description>
+      A Python GUI plugin for displaying a dashboard that displays and interacts with the Stretch robot.
+  </description>
+  <maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>rqt_gui</exec_depend>
+  <exec_depend>rqt_robot_dashboard</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>std_srvs</exec_depend>
+
+  <export>
+    <rqt_gui plugin="${prefix}/plugin.xml"/>
+  </export>
+</package>

stretch_dashboard/plugin.xml

@@ -0,0 +1,20 @@
+<library path="src">
+    <class name="StretchDashboard" type="stretch_dashboard.stretch_dashboard.StretchDashboard"
+           base_class_type="rqt_gui_py::Plugin">
+        <description>
+            A Python GUI plugin for displaying a dashboard that displays and interacts with the Stretch robot.
+        </description>
+        <qtgui>
+            <group>
+                <label>Robot</label>
+                <icon type="theme">folder</icon>
+                <statustip>Plugins related to specific robots.</statustip>
+            </group>
+            <label>Stretch Dashboard</label>
+            <icon type="theme">emblem-important</icon>
+            <statustip>
+                A Python GUI plugin for displaying a dashboard that displays and interacts with the Stretch robot.
+            </statustip>
+        </qtgui>
+    </class>
+</library>

stretch_dashboard/scripts/dashboard

@@ -0,0 +1,12 @@
+#!/usr/bin/env python3
+import sys
+
+from rqt_gui.main import Main
+
+
+def main():
+    sys.exit(Main().main(sys.argv, standalone='stretch_dashboard.stretch_dashboard.StretchDashboard'))
+
+
+if __name__ == '__main__':
+    main()

stretch_dashboard/setup.py

@@ -0,0 +1,11 @@
+#!/usr/bin/env python
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+package_info = generate_distutils_setup(
+    packages=['stretch_dashboard'],
+    package_dir={'': 'src'}
+)
+
+setup(**package_info)

stretch_dashboard/src/stretch_dashboard/stretch_dashboard.py

@@ -0,0 +1,90 @@
+import math
+
+import rospy
+
+from rqt_robot_dashboard.dashboard import Dashboard
+from rqt_robot_dashboard.menu_dash_widget import MenuDashWidget
+from rqt_robot_dashboard.widgets import BatteryDashWidget, ConsoleDashWidget, MonitorDashWidget
+
+from sensor_msgs.msg import BatteryState
+
+from std_msgs.msg import Bool, String
+
+from std_srvs.srv import Trigger
+
+
+class CalibrateWidget(MenuDashWidget):
+    def __init__(self):
+        MenuDashWidget.__init__(self, 'Calibration', icons=[
+            ['bg-grey.svg', 'ic-motors.svg'],     # State 0: Unknown
+            ['bg-green.svg', 'ic-motors.svg'],    # State 1: Calibrated
+            ['bg-yellow.svg', 'ic-motors.svg']])  # State 2: Not Calibrated
+        self.update_state(0)
+        self.setToolTip('Calibration')
+
+        self.client = rospy.ServiceProxy('/calibrate_the_robot', Trigger)
+        self.status_sub = rospy.Subscriber('is_calibrated', Bool, self.status_cb, queue_size=1)
+
+        self.add_action('Calibrate!', lambda: self.client.call())
+
+    def status_cb(self, msg):
+        if msg.data:
+            self.update_state(1)
+        else:
+            self.update_state(2)
+
+
+class ModeWidget(MenuDashWidget):
+    def __init__(self):
+        # TODO: These could probably use custom icons
+        MenuDashWidget.__init__(self, 'Robot Mode', icons=[
+            ['bg-grey.svg'],                            # State 0: Unknown
+            ['bg-green.svg', 'ic-steering-wheel.svg'],  # State 1: Navigation
+            ['bg-green.svg', 'ic-wrench.svg'],          # State 2: Manipulation
+            ['bg-green.svg', 'ic-runstop-off.svg']])    # State 3: Position
+        self.update_state(0)
+        self.setToolTip('Unknown Mode')
+
+        self.mode_map = {
+            'navigation': 1,
+            'manipulation': 2,
+            'position': 3
+        }
+
+        self.clients = {}
+        for mode in self.mode_map:
+            self.clients[mode] = rospy.ServiceProxy(f'/switch_to_{mode}_mode', Trigger)
+            self.add_action(f'Switch to {mode} mode',
+                            lambda mode_arg=mode: self.clients[mode_arg].call())
+
+        self.status_sub = rospy.Subscriber('mode', String, self.status_cb, queue_size=1)
+
+    def status_cb(self, msg):
+        if msg.data in self.mode_map:
+            self.update_state(self.mode_map[msg.data])
+            self.setToolTip(msg.data.title() + ' Mode')
+        else:
+            self.update_state(0)
+            self.setToolTip('Unknown Mode')
+
+
+class StretchDashboard(Dashboard):
+    def __init__(self, context):
+        super(StretchDashboard, self).__init__(context)
+        self.power_subscriber = rospy.Subscriber('/battery', BatteryState, self.battery_cb, 10)
+
+    def get_widgets(self):
+        self.monitor = MonitorDashWidget(self.context)
+        self.console = ConsoleDashWidget(self.context)
+        self.calibrate = CalibrateWidget()
+        self.mode_w = ModeWidget()
+        self.battery = BatteryDashWidget(self.context)
+
+        return [[self.monitor, self.console, self.calibrate, self.mode_w], [self.battery]]
+
+    def battery_cb(self, msg):
+        # TODO: Incorporate logic if robot is plugged in
+        if not math.isnan(msg.percentage):
+            self.battery.update_perc(msg.percentage)
+
+        self.battery.setToolTip('{:.2f} V\n{:.2f} A'.format(msg.voltage, msg.current))