diff --git a/stretch_core/config/diagnostics.yaml b/stretch_core/config/diagnostics.yaml new file mode 100644 index 0000000..cb386a6 --- /dev/null +++ b/stretch_core/config/diagnostics.yaml @@ -0,0 +1,35 @@ + type: diagnostic_aggregator/AnalyzerGroup + path: Stretch + analyzers: + hardware: + path: Hardware + type: diagnostic_aggregator/AnalyzerGroup + analyzers: + Lift: + type: diagnostic_aggregator/GenericAnalyzer + find_and_remove_prefix: ['stretch_driver: Lift'] + path: Lift + Arm: + type: diagnostic_aggregator/GenericAnalyzer + find_and_remove_prefix: ['stretch_driver: Arm'] + path: Arm + Base: + type: diagnostic_aggregator/GenericAnalyzer + find_and_remove_prefix: ['stretch_driver: Base'] + path: Base + EndOfArm: + type: diagnostic_aggregator/GenericAnalyzer + find_and_remove_prefix: ['stretch_driver: EndOfArm'] + path: EndOfArm + Head: + type: diagnostic_aggregator/GenericAnalyzer + find_and_remove_prefix: ['stretch_driver: Head'] + path: Head + Pimu: + type: diagnostic_aggregator/GenericAnalyzer + find_and_remove_prefix: ['stretch_driver: Pimu'] + path: Pimu + Wacc: + type: diagnostic_aggregator/GenericAnalyzer + find_and_remove_prefix: ['stretch_driver: Wacc'] + path: Wacc diff --git a/stretch_core/launch/stretch_driver.launch b/stretch_core/launch/stretch_driver.launch index 8fadf0f..e97c15a 100644 --- a/stretch_core/launch/stretch_driver.launch +++ b/stretch_core/launch/stretch_driver.launch @@ -40,5 +40,8 @@ - + + + + diff --git a/stretch_core/nodes/stretch_diagnostics.py b/stretch_core/nodes/stretch_diagnostics.py new file mode 100644 index 0000000..744fa4f --- /dev/null +++ b/stretch_core/nodes/stretch_diagnostics.py @@ -0,0 +1,127 @@ +import diagnostic_msgs + +import diagnostic_updater + +import stretch_body.hello_utils as hello_utils # For get_fleet_id + +# Shortcut constants +ERROR = diagnostic_msgs.msg.DiagnosticStatus.ERROR +WARN = diagnostic_msgs.msg.DiagnosticStatus.WARN +OK = diagnostic_msgs.msg.DiagnosticStatus.OK + + +def add_keyvalues_to_status(status, d, skip_keys=None): + """Add all the key value pairs in dictionary d (except those keys in skip_keys) to the diagnostic status.""" + for k, v in d.items(): + if skip_keys and k in skip_keys: + continue + status.add(k, str(v)) + + +def report_stats(status, d, skip_keys=None): + """Diagnostic update function that marks the summary as OK and just reports the stats from d.""" + status.summary(OK, 'OK') + add_keyvalues_to_status(status, d, skip_keys) + return status + + +def analyze_stepper(status, device): + """Diagnostic update function for device with stepper motor.""" + if not device.motor.hw_valid: + status.summary(ERROR, 'Hardware not present') + elif not device.motor.status['pos_calibrated']: + status.summary(WARN, 'Hardware not calibrated') + elif device.motor.status['runstop_on']: + status.summary(WARN, 'Runstop on') + else: + status.summary(OK, 'Hardware present and calibrated') + + add_keyvalues_to_status(status, device.status, ['motor']) + return status + + +def analyze_dynamixel(status, device, joint_name): + """Diagnostic update function for device with dynamixel motor.""" + motor = device.motors[joint_name] + if not motor.hw_valid: + status.summary(ERROR, 'Hardware not present') + elif not motor.is_calibrated: + status.summary(WARN, 'Hardware not calibrated') + else: + status.summary(OK, 'Hardware present and calibrated') + + add_keyvalues_to_status(status, motor.status) + + return status + + +def analyze_base(status, base): + """Diagnostic update function for the base.""" + for wheel in [base.left_wheel, base.right_wheel]: + if not wheel.hw_valid: + status.summary(ERROR, 'Hardware not present') + break + else: + status.summary(OK, 'Hardware present') + + add_keyvalues_to_status(status, base.status, ['left_wheel', 'right_wheel']) + return status + + +def check_range(status, device, key, lower_bound, upper_bound, out_of_bounds_status=ERROR): + """Diagnostic update function that checks if value(s) exceeds the specified bounds.""" + value = device.status[key] + if isinstance(value, list): + values = value + for i, value in enumerate(values): + status.add(f'{key}[{i}]', str(value)) + else: + status.add(key, str(value)) + values = [value] + + for value in values: + if value < lower_bound or value > upper_bound: + status.summary(out_of_bounds_status, f'{key}={value} | out of range [{lower_bound},{upper_bound}]') + return status + + status.summary(OK, 'OK') + return status + + +class StretchDiagnostics: + def __init__(self, node, robot): + self.node = node + self.robot = robot + self.updater = diagnostic_updater.Updater(node) + self.updater.setHardwareID(hello_utils.get_fleet_id()) + + # Configure the different update functions + self.updater.add('Pimu/Device', + lambda status: report_stats(status, self.robot.pimu.status, ['imu', 'transport'])) + + # TODO: Add IMU + self.updater.add('Pimu/Voltage', + lambda status: check_range(status, self.robot.pimu, 'voltage', + self.robot.pimu.config['low_voltage_alert'], 14.5)) + self.updater.add('Pimu/Current', + lambda status: check_range(status, self.robot.pimu, 'current', + 0.5, self.robot.pimu.config['high_current_alert'])) + self.updater.add('Pimu/Temperature', lambda status: check_range(status, self.robot.pimu, 'temp', 10, 40)) + self.updater.add('Pimu/Cliff', + lambda status: check_range(status, self.robot.pimu, 'cliff_range', + self.robot.pimu.config['cliff_thresh'], 20)) + + self.updater.add('Lift/Device', lambda status: analyze_stepper(status, self.robot.lift)) + self.updater.add('Lift/Motor', lambda status: report_stats(status, self.robot.lift.motor.status, ['transport'])) + self.updater.add('Arm/Device', lambda status: analyze_stepper(status, self.robot.arm)) + self.updater.add('Arm/Motor', lambda status: report_stats(status, self.robot.arm.motor.status, ['transport'])) + self.updater.add('Base/Device', lambda status: analyze_base(status, self.robot.base)) + self.updater.add('Base/LeftWheel', + lambda status: report_stats(status, self.robot.base.left_wheel.status, ['transport'])) + self.updater.add('Base/RightWheel', + lambda status: report_stats(status, self.robot.base.right_wheel.status, ['transport'])) + self.updater.add('Wacc/Device', lambda status: check_range(status, self.robot.wacc, 'ax', 8.0, 11.0)) + + for name, device in [('Head', self.robot.head), ('EndOfArm', self.robot.end_of_arm)]: + for jname in device.joints: + self.updater.add(f'{name}/{jname}', lambda status: analyze_dynamixel(status, device, jname)) diff --git a/stretch_core/nodes/stretch_driver b/stretch_core/nodes/stretch_driver index 126d05e..082d622 100755 --- a/stretch_core/nodes/stretch_driver +++ b/stretch_core/nodes/stretch_driver @@ -23,12 +23,13 @@ from std_srvs.srv import Trigger, TriggerResponse from std_srvs.srv import SetBool, SetBoolResponse from nav_msgs.msg import Odometry -from sensor_msgs.msg import JointState, Imu, MagneticField -from std_msgs.msg import Header +from sensor_msgs.msg import BatteryState, JointState, Imu, MagneticField +from std_msgs.msg import Bool, Header, String import hello_helpers.hello_misc as hm from hello_helpers.gripper_conversion import GripperConversion from joint_trajectory_server import JointTrajectoryAction +from stretch_diagnostics import StretchDiagnostics GRIPPER_DEBUG = False BACKLASH_DEBUG = False @@ -220,6 +221,28 @@ class StretchBodyNode: odom.twist.twist.angular.z = theta_vel self.odom_pub.publish(odom) + # TODO: Add way to determine if the robot is charging + # TODO: Calculate the percentage + battery_state = BatteryState() + invalid_reading = float('NaN') + battery_state.header.stamp = current_time + battery_state.voltage = float(robot_status['pimu']['voltage']) + battery_state.current = float(robot_status['pimu']['current']) + battery_state.charge = invalid_reading + battery_state.capacity = invalid_reading + battery_state.percentage = invalid_reading + battery_state.design_capacity = 18.0 + battery_state.present = True + self.power_pub.publish(battery_state) + + calibration_status = Bool() + calibration_status.data = self.robot.is_calibrated() + self.calibration_pub.publish(calibration_status) + + mode_msg = String() + mode_msg.data = self.robot_mode + self.mode_pub.publish(mode_msg) + # publish joint state for the arm joint_state = JointState() joint_state.header.stamp = current_time @@ -363,8 +386,7 @@ class StretchBodyNode: def calibrate(self): def code_to_run(): - self.robot.lift.home() - self.robot.arm.home() + self.robot.home() self.change_mode('calibration', code_to_run) ######## SERVICE CALLBACKS ####### @@ -391,6 +413,14 @@ class StretchBodyNode: message='Stopped the robot.' ) + def calibrate_callback(self, request): + rospy.loginfo('Received calibrate_the_robot service call.') + self.calibrate() + return TriggerResponse( + success=True, + message='Calibrated.' + ) + def navigation_mode_service_callback(self, request): self.turn_on_navigation_mode() return TriggerResponse( @@ -506,6 +536,10 @@ class StretchBodyNode: self.odom_pub = rospy.Publisher('odom', Odometry, queue_size=1) + self.power_pub = rospy.Publisher('battery', BatteryState, queue_size=1) + self.calibration_pub = rospy.Publisher('is_calibrated', Bool, queue_size=1) + self.mode_pub = rospy.Publisher('mode', String, queue_size=1) + self.imu_mobile_base_pub = rospy.Publisher('imu_mobile_base', Imu, queue_size=1) self.magnetometer_mobile_base_pub = rospy.Publisher('magnetometer_mobile_base', MagneticField, queue_size=1) self.imu_wrist_pub = rospy.Publisher('imu_wrist', Imu, queue_size=1) @@ -535,6 +569,7 @@ class StretchBodyNode: self.fail_out_of_range_goal = rospy.get_param('~fail_out_of_range_goal', True) self.joint_trajectory_action = JointTrajectoryAction(self) self.joint_trajectory_action.server.start() + self.diagnostics = StretchDiagnostics(self, self.robot) self.switch_to_navigation_mode_service = rospy.Service('/switch_to_navigation_mode', Trigger, @@ -548,6 +583,10 @@ class StretchBodyNode: Trigger, self.stop_the_robot_callback) + self.calibrate_the_robot_service = rospy.Service('/calibrate_the_robot', + Trigger, + self.calibrate_callback) + self.runstop_service = rospy.Service('/runstop', SetBool, self.runstop_service_callback) diff --git a/stretch_core/package.xml b/stretch_core/package.xml index 9fe3ed2..a4676fa 100644 --- a/stretch_core/package.xml +++ b/stretch_core/package.xml @@ -51,6 +51,8 @@ catkin actionlib actionlib_msgs + diagnostic_msgs + diagnostic_updater geometry_msgs nav_msgs control_msgs @@ -62,6 +64,8 @@ tf2 actionlib actionlib_msgs + diagnostic_msgs + diagnostic_updater geometry_msgs nav_msgs control_msgs @@ -73,6 +77,9 @@ tf2 actionlib actionlib_msgs + diagnostic_msgs + diagnostic_updater + diagnostic_aggregator geometry_msgs laser_filters nav_msgs diff --git a/stretch_dashboard/CMakeLists.txt b/stretch_dashboard/CMakeLists.txt new file mode 100644 index 0000000..cfa21ff --- /dev/null +++ b/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} +) diff --git a/stretch_dashboard/README.md b/stretch_dashboard/README.md new file mode 100644 index 0000000..60172ea --- /dev/null +++ b/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. diff --git a/stretch_dashboard/Screenshot.png b/stretch_dashboard/Screenshot.png new file mode 100644 index 0000000..a504ad3 Binary files /dev/null and b/stretch_dashboard/Screenshot.png differ diff --git a/stretch_dashboard/package.xml b/stretch_dashboard/package.xml new file mode 100644 index 0000000..08572c6 --- /dev/null +++ b/stretch_dashboard/package.xml @@ -0,0 +1,23 @@ + + + stretch_dashboard + 0.2.0 + + A Python GUI plugin for displaying a dashboard that displays and interacts with the Stretch robot. + + David V. Lu!! + Apache License 2.0 + + catkin + + rospy + rqt_gui + rqt_robot_dashboard + sensor_msgs + std_msgs + std_srvs + + + + + diff --git a/stretch_dashboard/plugin.xml b/stretch_dashboard/plugin.xml new file mode 100644 index 0000000..685e692 --- /dev/null +++ b/stretch_dashboard/plugin.xml @@ -0,0 +1,20 @@ + + + + A Python GUI plugin for displaying a dashboard that displays and interacts with the Stretch robot. + + + + + folder + Plugins related to specific robots. + + + emblem-important + + A Python GUI plugin for displaying a dashboard that displays and interacts with the Stretch robot. + + + + diff --git a/stretch_dashboard/scripts/dashboard b/stretch_dashboard/scripts/dashboard new file mode 100755 index 0000000..bf6b215 --- /dev/null +++ b/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() diff --git a/stretch_dashboard/setup.py b/stretch_dashboard/setup.py new file mode 100644 index 0000000..fb07c6d --- /dev/null +++ b/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) diff --git a/stretch_dashboard/src/stretch_dashboard/__init__.py b/stretch_dashboard/src/stretch_dashboard/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/stretch_dashboard/src/stretch_dashboard/stretch_dashboard.py b/stretch_dashboard/src/stretch_dashboard/stretch_dashboard.py new file mode 100644 index 0000000..6e3bfe8 --- /dev/null +++ b/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))