def setup(self, context):
self.name = "hrpsys dashbaord"
self._console = ConsoleDashWidget(self.context, minimal=False)
self._monitor = MonitorDashWidget(self.context)
self._log = HrpsysLogMenu()
rtcd_start_command = rospy.get_param('hrpsys_rtcd_start_command', None)
rtcd_stop_command = rospy.get_param('hrpsys_rtcd_stop_command', None)
if rtcd_stop_command or rtcd_start_command:
self._rtcd = HrpsysRTCDMenu(rtcd_start_command, rtcd_stop_command)
else:
self._rtcd = None
ros_bridge_start_command = rospy.get_param(
'hrpsys_ros_bridge_start_command', None)
ros_bridge_stop_command = rospy.get_param(
'hrpsys_ros_bridge_stop_command', None)
if ros_bridge_stop_command or ros_bridge_start_command:
self._ros_bridge = HrpsysROSBridgeMenu(ros_bridge_start_command,
ros_bridge_stop_command)
else:
self._ros_bridge = None
self._power = None
#self._power = HrpsysPowerMenu()
self._servo = HrpsysServoMenu(
rospy.get_param('hrpsys_servo_start_command', None),
rospy.get_param('hrpsys_servo_stop_command', None))
self._dashboard_agg_sub = rospy.Subscriber(
'diagnostics_agg', diagnostic_msgs.msg.DiagnosticArray,
self.dashboard_callback)
def setup(self, context):
self.name = 'PR2 Dashboard'
self.max_icon_size = QSize(50, 30)
self.message = None
parser = argparse.ArgumentParser(prog='pr2_dashboard', add_help=False)
PR2Dashboard.add_arguments(parser)
args = parser.parse_args(context.argv())
self._motor_namespace = args.motor_namespace
self._dashboard_message = None
self._last_dashboard_message_time = 0.0
self._raw_byte = None
self.digital_outs = [0, 0, 0]
self._console = ConsoleDashWidget(self.context, minimal=False)
self._monitor = MonitorDashWidget(self.context)
self._motors = PR2Motors(self.on_reset_motors, self.on_halt_motors)
self._breakers = [
PR2BreakerButton('Left Arm', 0),
PR2BreakerButton('Base', 1),
PR2BreakerButton('Right Arm', 2)
]
self._runstop = PR2Runstops('RunStops')
self._batteries = [PR2Battery(self.context)]
self._dashboard_agg_sub = rospy.Subscriber('dashboard_agg',
DashboardState,
self.dashboard_callback)
def setup(self, context):
self.name = 'Scitos Dashboard'
self.max_icon_size = QSize(50, 30)
self.message = None
self._dashboard_message = None
self._last_dashboard_message_time = 0.0
self._raw_byte = None
self.digital_outs = [0, 0, 0]
self._console = ConsoleDashWidget(self.context, minimal=False)
self._monitor = MonitorDashWidget(self.context)
self._mileage = ScitosMileage()
self._drive = ScitosDrive(self.reset_motorstop_cb, self.freerun_cb,
self.enable_motorstop_cb)
self._batteries = ScitosBattery()
self._freerun_state = True
self._motor_state_sub = rospy.Subscriber('/motor_status', MotorStatus,
self.motor_status_callback)
self._battery_state_sub = rospy.Subscriber('/battery_state',
BatteryState,
self.battery_callback)
self._mileage_sub = rospy.Subscriber('/mileage', Float32,
self.mileage_callback)
self._motor_stale_timer = Timer(0, self._drive.set_stale)
self._motor_stale_timer.start()
self._battery_stale_timer = Timer(0, self._batteries.set_stale)
self._battery_stale_timer.start()
self._mileage_stale_timer = Timer(0, self._mileage.set_stale)
self._mileage_stale_timer.start()
def setup(self, context):
self.name = 'Youbot Dashboard'
self.max_icon_size = QSize(50, 30)
self.message = None
self._dashboard_message = None
self._last_dashboard_message_time = 0.0
self._raw_byte = None
self.digital_outs = [0, 0, 0]
self._console = ConsoleDashWidget(self.context, minimal=False)
self._monitor = MonitorDashWidget(self.context)
self._base_motors = YoubotMotors("base",
self.on_base_motors_on_clicked,
self.on_base_motors_off_clicked)
self._arm_1_motors = YoubotMotors("arm",
self.on_arm_1_motors_on_clicked,
self.on_arm_1_motors_off_clicked)
self._ethercat = YoubotEthercat('EtherCAT', self.on_reconnect_clicked)
self._batteries = [PR2Battery(self.context)]
self._dashboard_agg_sub = rospy.Subscriber('dashboard_agg',
DashboardState,
self.dashboard_callback)
def setup(self, context):
self.name = 'Irp6 Dashboard (%s)'%rosenv.get_master_uri()
# Diagnostics
self._monitor = MonitorDashWidget(self.context)
# Rosout
self._console = ConsoleDashWidget(self.context, minimal=False)
## Motors
self._irp6p_motors_button = Irp6pMotors(self.context)
self._irp6ot_motors_button = Irp6otMotors(self.context)
self._conveyor_motors_button = ConveyorMotors(self.context)
self._stop_button = Irp6Stop('Irp6Stop',self.emergency_stop)
self._agg_sub = rospy.Subscriber('diagnostics', DiagnosticArray, self.new_diagnostic_message_diagnostics)
def setup(self, context):
self.name = 'PR2 Dashboard'
self.max_icon_size = QSize(50, 30)
self.message = None
self._dashboard_message = None
self._last_dashboard_message_time = 0.0
self._raw_byte = None
self.digital_outs = [0, 0, 0]
self._console = ConsoleDashWidget(self.context, minimal=False)
self._monitor = MonitorDashWidget(self.context)
self._motors = PR2Motors(self.on_reset_motors, self.on_halt_motors)
self._breakers = [
PR2BreakerButton('Left Arm', 0),
PR2BreakerButton('Base', 1),
PR2BreakerButton('Right Arm', 2)
]
self._runstop = PR2Runstops('RunStops')
self._batteries = [PR2Battery(self.context)]
self._dashboard_agg_sub = rospy.Subscriber('dashboard_agg',
DashboardState,
self.dashboard_callback)
def setup(self, context):
self.name = "hrpsys dashbaord"
self._console = ConsoleDashWidget(self.context, minimal=False)
self._monitor = MonitorDashWidget(self.context)
self._log = HrpsysLogMenu()
rtcd_start_command = rospy.get_param("hrpsys_rtcd_start_command", None)
rtcd_stop_command = rospy.get_param("hrpsys_rtcd_stop_command", None)
if rtcd_stop_command or rtcd_start_command:
self._rtcd = HrpsysRTCDMenu(rtcd_start_command, rtcd_stop_command)
else:
self._rtcd = None
ros_bridge_start_command = rospy.get_param("hrpsys_ros_bridge_start_command", None)
ros_bridge_stop_command = rospy.get_param("hrpsys_ros_bridge_stop_command", None)
if ros_bridge_stop_command or ros_bridge_start_command:
self._ros_bridge = HrpsysROSBridgeMenu(ros_bridge_start_command, ros_bridge_stop_command)
else:
self._ros_bridge = None
self._power = None
# self._power = HrpsysPowerMenu()
self._servo = HrpsysServoMenu(
rospy.get_param("hrpsys_servo_start_command", None), rospy.get_param("hrpsys_servo_stop_command", None)
)
self._dashboard_agg_sub = rospy.Subscriber(
"diagnostics_agg", diagnostic_msgs.msg.DiagnosticArray, self.dashboard_callback
)
def setup(self, context):
self.name = 'NAO Dashboard (%s)'%rosenv.get_master_uri()
self.max_icon_size = QSize(50, 30)
self.message = None
self._dashboard_message = None
self._last_dashboard_message_time = 0.0
self._raw_byte = None
self.digital_outs = [0, 0, 0]
icons_path = path.join(roslib.packages.get_pkg_dir('nao_dashboard'), "icons/")
self._robot_combobox = QComboBox()
self._robot_combobox.setSizeAdjustPolicy(QComboBox.AdjustToContents)
self._robot_combobox.setInsertPolicy(QComboBox.InsertAlphabetically)
self._robot_combobox.setEditable(True)
gobject.threads_init()
dbus.glib.threads_init()
self.robots = []
self.sys_bus = dbus.SystemBus()
self.avahi_server = dbus.Interface(self.sys_bus.get_object(avahi.DBUS_NAME, '/'), avahi.DBUS_INTERFACE_SERVER)
self.sbrowser = dbus.Interface(self.sys_bus.get_object(avahi.DBUS_NAME, self.avahi_server.ServiceBrowserNew(avahi.IF_UNSPEC,
avahi.PROTO_INET, '_naoqi._tcp', 'local', dbus.UInt32(0))), avahi.DBUS_INTERFACE_SERVICE_BROWSER)
self.sbrowser.connect_to_signal("ItemNew", self.avahiNewItem)
self.sbrowser.connect_to_signal("ItemRemove", self.avahiItemRemove)
# Diagnostics
self._monitor = MonitorDashWidget(self.context)
# Rosout
self._console = ConsoleDashWidget(self.context, minimal=False)
## Joint temperature
self._temp_joint_button = StatusControl('Joint temperature', 'temperature_joints')
## CPU temperature
self._temp_head_button = StatusControl('CPU temperature', 'temperature_head')
## Motors
self._motors_button = Motors(self.context)
## Battery State
self._power_state_ctrl = PowerStateControl('NAO Battery')
self._agg_sub = rospy.Subscriber('diagnostics_agg', DiagnosticArray, self.new_diagnostic_message)
self._last_dashboard_message_time = 0.0
def setup(self, context):
self.name = 'NAO Dashboard (%s)'%rosenv.get_master_uri()
self._robot_combobox = AvahiWidget()
# Diagnostics
self._monitor = MonitorDashWidget(self.context)
# Rosout
self._console = ConsoleDashWidget(self.context, minimal=False)
## Joint temperature
self._temp_joint_button = StatusControl('Joint temperature', 'temperature_joints')
## CPU temperature
self._temp_head_button = StatusControl('CPU temperature', 'temperature_head')
## Motors
self._motors_button = Motors(self.context)
## Battery State
self._power_state_ctrl = PowerStateControl('NAO Battery')
self._agg_sub = rospy.Subscriber('diagnostics_agg', DiagnosticArray, self.new_diagnostic_message)
class NAOqiDashboard(Dashboard):
def setup(self, context):
self.name = 'NAOqi Dashboard (%s)' % rosenv.get_master_uri()
self.robot_prefix = ''
# get robot info
rospy.wait_for_service("/naoqi_driver/get_robot_config")
try:
get_robot_info = rospy.ServiceProxy(
"/naoqi_driver/get_robot_config", GetRobotInfo)
resp = get_robot_info.call(GetRobotInfoRequest())
if resp.info.type == 0:
self.robot_prefix = "nao_robot"
elif resp.info.type == 2:
self.robot_prefix = "pepper_robot"
except rospy.ServiceException, e:
print "Unable to call naoqi_driver/get_robot_config:%s", e
#print "gonna work with robot prefix ", self.robot_prefix
self._robot_combobox = AvahiWidget()
# Diagnostics
self._monitor = MonitorDashWidget(self.context)
# Rosout
self._console = ConsoleDashWidget(self.context, minimal=False)
## Joint temperature
self._temp_joint_button = StatusControl('Joint temperature',
'temperature_joints')
## CPU temperature
self._temp_head_button = StatusControl('CPU temperature',
'temperature_head')
## Motors
self._motors_button = Motors(self.context, self.robot_prefix)
## Postures
self._postures = PostureWidget(self.robot_prefix)
## Battery State
self._power_state_ctrl = PowerStateControl('Battery')
self._agg_sub = rospy.Subscriber('/diagnostics_agg', DiagnosticArray,
self.new_diagnostic_message)
def setup(self, context):
self.name = 'Irp6 Dashboard (%s)'%rosenv.get_master_uri()
# Diagnostics
self._monitor = MonitorDashWidget(self.context)
# Rosout
self._console = ConsoleDashWidget(self.context, minimal=False)
## Motors
self._irp6p_motors_button = Irp6pMotors(self.context)
self._irp6ot_motors_button = Irp6otMotors(self.context)
self._conveyor_motors_button = ConveyorMotors(self.context)
self._agg_sub = rospy.Subscriber('diagnostics', DiagnosticArray, self.new_diagnostic_message_diagnostics)
class PR2Dashboard(Dashboard):
"""
Dashboard for PR2s
:param context: the plugin context
:type context: qt_gui.plugin.Plugin
"""
def setup(self, context):
self.name = 'PR2 Dashboard'
self.max_icon_size = QSize(50, 30)
self.message = None
parser = argparse.ArgumentParser(prog='pr2_dashboard', add_help=False)
PR2Dashboard.add_arguments(parser)
args = parser.parse_args(context.argv())
self._motor_namespace = args.motor_namespace
self._dashboard_message = None
self._last_dashboard_message_time = 0.0
self._raw_byte = None
self.digital_outs = [0, 0, 0]
self._console = ConsoleDashWidget(self.context, minimal=False)
self._monitor = MonitorDashWidget(self.context)
self._motors = PR2Motors(self.on_reset_motors, self.on_halt_motors)
self._breakers = [
PR2BreakerButton('Left Arm', 0),
PR2BreakerButton('Base', 1),
PR2BreakerButton('Right Arm', 2)
]
self._runstop = PR2Runstops('RunStops')
self._batteries = [PR2Battery(self.context)]
self._dashboard_agg_sub = rospy.Subscriber('dashboard_agg',
DashboardState,
self.dashboard_callback)
def get_widgets(self):
return [[self._monitor, self._console, self._motors], self._breakers,
[self._runstop], self._batteries]
def dashboard_callback(self, msg):
"""
callback to process dashboard_agg messages
:param msg: dashboard_agg DashboardState message
:type msg: pr2_msgs.msg.DashboardState
"""
self._dashboard_message = msg
self._last_dashboard_message_time = rospy.get_time()
if (msg.motors_halted_valid):
if (not msg.motors_halted.data):
self._motors.set_ok()
self._motors.setToolTip(self.tr("Motors: Running"))
else:
self._motors.set_error()
self._motors.setToolTip(self.tr("Motors: Halted"))
else:
self._motors.set_stale()
self._motors.setToolTip(self.tr("Motors: Stale"))
if (msg.power_state_valid):
self._batteries[0].set_power_state(msg.power_state)
else:
self._batteries[0].set_stale()
if (msg.power_board_state_valid):
[
breaker.set_power_board_state_msg(msg.power_board_state)
for breaker in self._breakers
]
if msg.power_board_state.run_stop:
self._runstop.set_ok()
self._runstop.setToolTip(
self.tr("Physical Runstop: OK\nWireless Runstop: OK"))
elif msg.power_board_state.wireless_stop:
self._runstop.set_physical_engaged()
self._runstop.setToolTip(
self.tr("Physical Runstop: Pressed\nWireless Runstop: OK"))
if not msg.power_board_state.wireless_stop:
self._runstop.set_wireless_engaged()
self._runstop.setToolTip(
self.
tr("Physical Runstop: Unknown\nWireless Runstop: Pressed"))
else:
[breaker.reset() for breaker in self._breakers]
self._runstop.set_stale()
self._runstop.setToolTip(
self.tr("Physical Runstop: Stale\nWireless Runstop: Stale"))
def on_reset_motors(self):
# if any of the breakers is not enabled ask if they'd like to enable them
if (self._dashboard_message is not None
and self._dashboard_message.power_board_state_valid):
all_breakers_enabled = reduce(lambda x, y: x and y, [
state == PowerBoardState.STATE_ON for state in
self._dashboard_message.power_board_state.circuit_state
])
if (not all_breakers_enabled):
if (QMessageBox.question(
self._breakers[0], self.tr('Enable Breakers?'),
self.
tr("Resetting the motors may not work because not all breakers are enabled. Enable all the breakers first?"
), QMessageBox.Yes | QMessageBox.No,
QMessageBox.Yes) == QMessageBox.Yes):
[breaker.set_enable() for breaker in self._breakers]
reset = rospy.ServiceProxy(
"{}/reset_motors".format(self._motor_namespace),
std_srvs.srv.Empty)
try:
reset()
except rospy.ServiceException as e:
QMessageBox.critical(
self._breakers[0], "Error",
"Failed to reset the motors: service call failed with error: %s"
% (e))
def on_halt_motors(self):
halt = rospy.ServiceProxy(
"{}/halt_motors".format(self._motor_namespace), std_srvs.srv.Empty)
try:
halt()
except rospy.ServiceException as e:
QMessageBox.critical(
self._motors, "Error",
"Failed to halt the motors: service call failed with error: %s"
% (e))
def shutdown_dashboard(self):
self._dashboard_agg_sub.unregister()
@staticmethod
def add_arguments(parser):
group = parser.add_argument_group('Options for pr2_dashboard')
group.add_argument('--motor-namespace',
dest='motor_namespace',
type=str,
default='pr2_ethercat')
def save_settings(self, plugin_settings, instance_settings):
self._console.save_settings(plugin_settings, instance_settings)
self._monitor.save_settings(plugin_settings, instance_settings)
def restore_settings(self, plugin_settings, instance_settings):
self._console.restore_settings(plugin_settings, instance_settings)
self._monitor.restore_settings(plugin_settings, instance_settings)
class NAODashboard(Dashboard):
def setup(self, context):
self.name = 'NAO Dashboard (%s)'%rosenv.get_master_uri()
self._robot_combobox = AvahiWidget()
# Diagnostics
self._monitor = MonitorDashWidget(self.context)
# Rosout
self._console = ConsoleDashWidget(self.context, minimal=False)
## Joint temperature
self._temp_joint_button = StatusControl('Joint temperature', 'temperature_joints')
## CPU temperature
self._temp_head_button = StatusControl('CPU temperature', 'temperature_head')
## Motors
self._motors_button = Motors(self.context)
## Battery State
self._power_state_ctrl = PowerStateControl('NAO Battery')
self._agg_sub = rospy.Subscriber('diagnostics_agg', DiagnosticArray, self.new_diagnostic_message)
def get_widgets(self):
return [ [self._robot_combobox],
[self._monitor, self._console, self._temp_joint_button, self._temp_head_button,
self._motors_button],
[self._power_state_ctrl]
]
def shutdown_dashboard(self):
self._agg_sub.unregister()
def new_diagnostic_message(self, msg):
"""
callback to process dashboard_agg messages
:param msg: dashboard_agg DashboardState message
:type msg: pr2_msgs.msg.DashboardState
"""
self._dashboard_message = msg
for status in msg.status:
if status.name == '/Nao/Joints':
highestTemp = ""
lowestStiff = -1.0
highestStiff = -1.0
hotJoints = ""
for kv in status.values:
if kv.key == 'Highest Temperature':
highestTemp = " (" + kv.value + "deg C)"
elif kv.key == 'Highest Stiffness':
highestStiff = float(kv.value)
elif kv.key == 'Lowest Stiffness without Hands':
lowestStiff = float(kv.value)
elif kv.key == 'Hot Joints':
hotJoints = str(kv.value)
self.set_buttonStatus(self._temp_joint_button, status, "Joints: ", "%s %s"%(highestTemp, hotJoints))
if(lowestStiff < 0.0 or highestStiff < 0.0):
self._motors_button.set_stale()
elif(lowestStiff > 0.9):
self._motors_button.set_error()
elif(highestStiff < 0.05):
self._motors_button.set_ok()
else:
self._motors_button.set_warn()
elif status.name == '/Nao/CPU':
self.set_buttonStatus(self._temp_head_button, status, "CPU temperature: ")
elif status.name == '/Nao/Battery/Battery':
if status.level == 3:
self._power_state_ctrl.set_stale()
else:
self._power_state_ctrl.set_power_state(status.values)
def set_buttonStatus(self, button, status, statusPrefix = "", statusSuffix = ""):
statusString = "Unknown"
if status.level == DiagnosticStatus.OK:
button.update_state(0)
statusString = "OK"
elif status.level == DiagnosticStatus.WARN:
button.update_state(1)
statusString = "Warn"
elif status.level == DiagnosticStatus.ERROR:
button.update_state(2)
statusString = "Error"
elif status.level == 3:
button.update_state(3)
statusString = "Stale"
button.setToolTip(statusPrefix + statusString + statusSuffix)
def save_settings(self, plugin_settings, instance_settings):
self._console.save_settings(plugin_settings, instance_settings)
self._monitor.save_settings(plugin_settings, instance_settings)
def restore_settings(self, plugin_settings, instance_settings):
self._console.restore_settings(plugin_settings, instance_settings)
self._monitor.restore_settings(plugin_settings, instance_settings)
class Irp6Dashboard(Dashboard):
def setup(self, context):
self.name = 'Irp6 Dashboard (%s)'%rosenv.get_master_uri()
# Diagnostics
self._monitor = MonitorDashWidget(self.context)
# Rosout
self._console = ConsoleDashWidget(self.context, minimal=False)
## Motors
self._irp6p_motors_button = Irp6pMotors(self.context)
self._irp6ot_motors_button = Irp6otMotors(self.context)
self._conveyor_motors_button = ConveyorMotors(self.context)
self._stop_button = Irp6Stop('Irp6Stop',self.emergency_stop)
self._agg_sub = rospy.Subscriber('diagnostics', DiagnosticArray, self.new_diagnostic_message_diagnostics)
# self._agg_sub = rospy.Subscriber('diagnostics_agg', DiagnosticArray, self.new_diagnostic_message_diagnostics_agg)
def emergency_stop(self):
self._irp6p_motors_button.emergency_stop()
self._irp6ot_motors_button.emergency_stop()
self._conveyor_motors_button.emergency_stop()
print 'emergency stop commanded'
def get_widgets(self):
return [
[self._monitor, self._console, self._irp6ot_motors_button, self._irp6p_motors_button, self._conveyor_motors_button, self._stop_button]
]
def shutdown_dashboard(self):
self._agg_sub.unregister()
def new_diagnostic_message_diagnostics(self, msg):
"""
callback to process dashboard_agg messages
"""
self._dashboard_message = msg
for status in msg.status:
if status.name == 'Irp6p Hardware Interface':
self._irp6p_motors_button.interpret_diagnostic_message(status)
elif status.name == 'Irp6ot Hardware Interface':
self._irp6ot_motors_button.interpret_diagnostic_message(status)
elif status.name == 'Conveyor Hardware Interface':
self._conveyor_motors_button.interpret_diagnostic_message(status)
def new_diagnostic_message_diagnostics_agg(self, msg):
"""
callback to process dashboard_agg messages
"""
self._dashboard_message = msg
for status in msg.status:
if status.name == '/HardwareInterfaces/Irp6p/Irp6p Hardware Interface':
if (status.level != 3):
self._irp6p_motors_button.interpret_diagnostic_message(status)
elif status.name == '/HardwareInterfaces/Irp6ot/Irp6ot Hardware Interface':
if (status.level != 3):
self._irp6ot_motors_button.interpret_diagnostic_message(status)
elif status.name == '/HardwareInterfaces/Conveyor/Conveyor Hardware Interface':
if (status.level != 3):
self._conveyor_motors_button.interpret_diagnostic_message(status)
def save_settings(self, plugin_settings, instance_settings):
self._console.save_settings(plugin_settings, instance_settings)
self._monitor.save_settings(plugin_settings, instance_settings)
def restore_settings(self, plugin_settings, instance_settings):
self._console.restore_settings(plugin_settings, instance_settings)
self._monitor.restore_settings(plugin_settings, instance_settings)
class HrpsysDashboard(Dashboard):
"""
Dashbaord for hrpsys.
it includes:
* diagnostics
* rosout
* log download
* power
* servo
* rtcd
* ros_bridge
* battery
"""
def __init__(self, context):
splash_image = rospy.get_param('hrpsys_splash_image', None)
if splash_image:
self.splash = HrpsysSplashScreen(splash_image)
else:
self.splash = None
super(HrpsysDashboard, self).__init__(context)
def setup(self, context):
self.name = "hrpsys dashbaord"
self._console = ConsoleDashWidget(self.context, minimal=False)
self._monitor = MonitorDashWidget(self.context)
self._log = HrpsysLogMenu()
rtcd_start_command = rospy.get_param('hrpsys_rtcd_start_command', None)
rtcd_stop_command = rospy.get_param('hrpsys_rtcd_stop_command', None)
if rtcd_stop_command or rtcd_start_command:
self._rtcd = HrpsysRTCDMenu(rtcd_start_command, rtcd_stop_command)
else:
self._rtcd = None
ros_bridge_start_command = rospy.get_param('hrpsys_ros_bridge_start_command', None)
ros_bridge_stop_command = rospy.get_param('hrpsys_ros_bridge_stop_command', None)
if ros_bridge_stop_command or ros_bridge_start_command:
self._ros_bridge = HrpsysROSBridgeMenu(ros_bridge_start_command, ros_bridge_stop_command)
else:
self._ros_bridge = None
self._power = None
#self._power = HrpsysPowerMenu()
self._servo = HrpsysServoMenu(rospy.get_param('hrpsys_servo_start_command', None),
rospy.get_param('hrpsys_servo_stop_command', None))
self._dashboard_agg_sub = rospy.Subscriber('diagnostics_agg', diagnostic_msgs.msg.DiagnosticArray, self.dashboard_callback)
def get_widgets(self):
widgets = []
if self._rtcd:
widgets.append(self._rtcd)
if self._ros_bridge:
widgets.append(self._ros_bridge)
if self._power:
widgets.append(self._power)
if self._servo:
widgets.append(self._servo)
return [[self._monitor, self._console, self._log], widgets]
def dashboard_callback(self, msg):
servo_mode = None
power_mode = None
log_mode = None
for status in msg.status:
if status.name == "/Mode/Operating Mode":
servo_mode = status.message
if status.name == "/Mode/Power Mode":
power_mode = status.message
if status.name == "/Hrpsys/hrpEC Profile (log)":
log_mode = status.message
if servo_mode:
if servo_mode == 'Servo On':
self._servo.update_state(1)
elif servo_mode == 'Servo Off':
self._servo.update_state(2)
else:
self._servo.update_state(0)
if self._power:
if power_mode:
if power_mode == 'Power On':
self._power.update_state(1)
elif power_mode =='Power Off':
self._power.update_state(2)
else:
self._power.update_state(0)
if log_mode:
if re.search("^Running", log_mode):
self._log.update_state(1)
else:
self._log.update_state(2)
else:
self._log.update_state(0)
def shutdown_dashboard(self):
self._dashboard_agg_sub.unregister()
def save_settings(self, plugin_settings, instance_settings):
self._console.save_settings(plugin_settings, instance_settings)
self._monitor.save_settings(plugin_settings, instance_settings)
def restore_settings(self, plugin_settings, instance_settings):
self._console.restore_settings(plugin_settings, instance_settings)
self._monitor.restore_settings(plugin_settings, instance_settings)
class HrpsysDashboard(Dashboard):
"""
Dashbaord for hrpsys.
it includes:
* diagnostics
* rosout
* log download
* power
* servo
* rtcd
* ros_bridge
* battery
"""
def __init__(self, context):
splash_image = rospy.get_param('hrpsys_splash_image', None)
if splash_image:
self.splash = HrpsysSplashScreen(splash_image)
else:
self.splash = None
super(HrpsysDashboard, self).__init__(context)
def setup(self, context):
self.name = "hrpsys dashbaord"
self._console = ConsoleDashWidget(self.context, minimal=False)
self._monitor = MonitorDashWidget(self.context)
self._log = HrpsysLogMenu()
rtcd_start_command = rospy.get_param('hrpsys_rtcd_start_command', None)
rtcd_stop_command = rospy.get_param('hrpsys_rtcd_stop_command', None)
if rtcd_stop_command or rtcd_start_command:
self._rtcd = HrpsysRTCDMenu(rtcd_start_command, rtcd_stop_command)
else:
self._rtcd = None
ros_bridge_start_command = rospy.get_param(
'hrpsys_ros_bridge_start_command', None)
ros_bridge_stop_command = rospy.get_param(
'hrpsys_ros_bridge_stop_command', None)
if ros_bridge_stop_command or ros_bridge_start_command:
self._ros_bridge = HrpsysROSBridgeMenu(ros_bridge_start_command,
ros_bridge_stop_command)
else:
self._ros_bridge = None
self._power = None
#self._power = HrpsysPowerMenu()
self._servo = HrpsysServoMenu(
rospy.get_param('hrpsys_servo_start_command', None),
rospy.get_param('hrpsys_servo_stop_command', None))
self._dashboard_agg_sub = rospy.Subscriber(
'diagnostics_agg', diagnostic_msgs.msg.DiagnosticArray,
self.dashboard_callback)
def get_widgets(self):
widgets = []
if self._rtcd:
widgets.append(self._rtcd)
if self._ros_bridge:
widgets.append(self._ros_bridge)
if self._power:
widgets.append(self._power)
if self._servo:
widgets.append(self._servo)
return [[self._monitor, self._console, self._log], widgets]
def dashboard_callback(self, msg):
servo_mode = None
power_mode = None
log_mode = None
for status in msg.status:
if status.name == "/Mode/Operating Mode":
servo_mode = status.message
if status.name == "/Mode/Power Mode":
power_mode = status.message
if status.name == "/Hrpsys/hrpEC Profile (log)":
log_mode = status.message
if servo_mode:
if servo_mode == 'Servo On':
self._servo.update_state(1)
elif servo_mode == 'Servo Off':
self._servo.update_state(2)
else:
self._servo.update_state(0)
if self._power:
if power_mode:
if power_mode == 'Power On':
self._power.update_state(1)
elif power_mode == 'Power Off':
self._power.update_state(2)
else:
self._power.update_state(0)
if log_mode:
if re.search("^Running", log_mode):
self._log.update_state(1)
else:
self._log.update_state(2)
else:
self._log.update_state(0)
def shutdown_dashboard(self):
self._dashboard_agg_sub.unregister()
def save_settings(self, plugin_settings, instance_settings):
self._console.save_settings(plugin_settings, instance_settings)
self._monitor.save_settings(plugin_settings, instance_settings)
def restore_settings(self, plugin_settings, instance_settings):
self._console.restore_settings(plugin_settings, instance_settings)
self._monitor.restore_settings(plugin_settings, instance_settings)
class NAODashboard(Dashboard):
def setup(self, context):
self.name = 'NAO Dashboard (%s)'%rosenv.get_master_uri()
self.max_icon_size = QSize(50, 30)
self.message = None
self._dashboard_message = None
self._last_dashboard_message_time = 0.0
self._raw_byte = None
self.digital_outs = [0, 0, 0]
icons_path = path.join(roslib.packages.get_pkg_dir('nao_dashboard'), "icons/")
self._robot_combobox = QComboBox()
self._robot_combobox.setSizeAdjustPolicy(QComboBox.AdjustToContents)
self._robot_combobox.setInsertPolicy(QComboBox.InsertAlphabetically)
self._robot_combobox.setEditable(True)
gobject.threads_init()
dbus.glib.threads_init()
self.robots = []
self.sys_bus = dbus.SystemBus()
self.avahi_server = dbus.Interface(self.sys_bus.get_object(avahi.DBUS_NAME, '/'), avahi.DBUS_INTERFACE_SERVER)
self.sbrowser = dbus.Interface(self.sys_bus.get_object(avahi.DBUS_NAME, self.avahi_server.ServiceBrowserNew(avahi.IF_UNSPEC,
avahi.PROTO_INET, '_naoqi._tcp', 'local', dbus.UInt32(0))), avahi.DBUS_INTERFACE_SERVICE_BROWSER)
self.sbrowser.connect_to_signal("ItemNew", self.avahiNewItem)
self.sbrowser.connect_to_signal("ItemRemove", self.avahiItemRemove)
# Diagnostics
self._monitor = MonitorDashWidget(self.context)
# Rosout
self._console = ConsoleDashWidget(self.context, minimal=False)
## Joint temperature
self._temp_joint_button = StatusControl('Joint temperature', 'temperature_joints')
## CPU temperature
self._temp_head_button = StatusControl('CPU temperature', 'temperature_head')
## Motors
self._motors_button = Motors(self.context)
## Battery State
self._power_state_ctrl = PowerStateControl('NAO Battery')
self._agg_sub = rospy.Subscriber('diagnostics_agg', DiagnosticArray, self.new_diagnostic_message)
self._last_dashboard_message_time = 0.0
def get_widgets(self):
return [ [self._robot_combobox],
[self._monitor, self._console, self._temp_joint_button, self._temp_head_button,
self._motors_button],
[self._power_state_ctrl]
]
def shutdown_dashboard(self):
self._agg_sub.unregister()
def new_diagnostic_message(self, msg):
"""
callback to process dashboard_agg messages
:param msg: dashboard_agg DashboardState message
:type msg: pr2_msgs.msg.DashboardState
"""
self._dashboard_message = msg
self._last_dashboard_message_time = rospy.get_time()
for status in msg.status:
if status.name == '/Nao/Joints':
highestTemp = ""
lowestStiff = -1.0
highestStiff = -1.0
hotJoints = ""
for kv in status.values:
if kv.key == 'Highest Temperature':
highestTemp = " (" + kv.value + "deg C)"
elif kv.key == 'Highest Stiffness':
highestStiff = float(kv.value)
elif kv.key == 'Lowest Stiffness without Hands':
lowestStiff = float(kv.value)
elif kv.key == 'Hot Joints':
hotJoints = str(kv.value)
self.set_buttonStatus(self._temp_joint_button, status, "Joints: ", "%s %s"%(highestTemp, hotJoints))
#if(lowestStiff < 0.0 or highestStiff < 0.0):
#self._motors_button.set_stale()
#self._motors_button.SetToolTip(wx.ToolTip("Stale"))
#elif(lowestStiff > 0.9):
#self._motors_button.set_error()
#self._motors_button.SetToolTip(wx.ToolTip("Stiffness on"))
#elif(highestStiff < 0.05):
#self._motors_button.set_ok()
#self._motors_button.SetToolTip(wx.ToolTip("Stiffness off"))
#else:
#self._motors_button.set_warn()
#self._motors_button.SetToolTip(wx.ToolTip("Stiffness partially on (between %f and %f)" % (lowestStiff, highestStiff)))
elif status.name == '/Nao/CPU':
self.set_buttonStatus(self._temp_head_button, status, "CPU temperature: ")
elif status.name == '/Nao/Battery/Battery':
if status.level == 3:
self._power_state_ctrl.set_stale()
else:
self._power_state_ctrl.set_power_state(status.values)
def set_buttonStatus(self, button, status, statusPrefix = "", statusSuffix = ""):
statusString = "Unknown"
if status.level == DiagnosticStatus.OK:
button.update_state(0)
statusString = "OK"
elif status.level == DiagnosticStatus.WARN:
button.update_state(1)
statusString = "Warn"
elif status.level == DiagnosticStatus.ERROR:
button.update_state(2)
statusString = "Error"
elif status.level == 3:
button.update_state(3)
statusString = "Stale"
button.setToolTip(statusPrefix + statusString + statusSuffix)
def avahiNewItem(self, interface, protocol, name, stype, domain, flags):
self.avahi_server.ResolveService(interface, protocol, name, stype,
domain, avahi.PROTO_INET, dbus.UInt32(0),
reply_handler=self.service_resolved, error_handler=self.print_error)
pass
def avahiItemRemove(self, interface, protocol, name, stype, domain, flags):
print "Remove"
for robot in self.robots:
if robot['name'] == str(name) and robot['address'] == str(address) and robot['port'] == int(port):
self.robots.remove(robot)
updateRobotCombobox();
def service_resolved(self, interface, protocol, name, type, domain, host, aprotocol, address, port, txt, flags):
self.robots.append({'name': str(name), 'address': str(address), 'port': int(port)})
self.updateRobotCombobox()
def updateRobotCombobox(self):
selected = self._robot_combobox.currentText()
for i in range(self._robot_combobox.count()):
self._robot_combobox.removeItem(i)
id = -1
for robot in self.robots:
text = str(robot)
text = "%s (%s:%d)" % (robot['name'], robot['address'], robot['port'])
self._robot_combobox.addItem(text, '%s:%d' % (robot['address'], robot['port']))
if(text == selected):
id = self._robot_combobox.count()-1;
if(self._robot_combobox.count() == 1):
self._robot_combobox.setCurrentIndex(0)
elif(id > -1):
self._robot_combobox.setCurrentIndex(id)
def print_error(self, *args):
print 'error_handler'
print args
def save_settings(self, plugin_settings, instance_settings):
self._console.save_settings(plugin_settings, instance_settings)
self._monitor.save_settings(plugin_settings, instance_settings)
def restore_settings(self, plugin_settings, instance_settings):
self._console.restore_settings(plugin_settings, instance_settings)
self._monitor.restore_settings(plugin_settings, instance_settings)
class Irp6Dashboard(Dashboard):
def setup(self, context):
self.name = 'Irp6 Dashboard (%s)' % rosenv.get_master_uri()
# Diagnostics
self._monitor = MonitorDashWidget(self.context)
# Rosout
self._console = ConsoleDashWidget(self.context, minimal=False)
## Motors
self._irp6p_motors_button = Irp6pMotors(self.context)
self._irp6ot_motors_button = Irp6otMotors(self.context)
self._conveyor_motors_button = ConveyorMotors(self.context)
self._agg_sub = rospy.Subscriber(
'diagnostics', DiagnosticArray,
self.new_diagnostic_message_diagnostics)
# self._agg_sub = rospy.Subscriber('diagnostics_agg', DiagnosticArray, self.new_diagnostic_message_diagnostics_agg)
def get_widgets(self):
return [[
self._monitor, self._console, self._irp6ot_motors_button,
self._irp6p_motors_button, self._conveyor_motors_button
]]
def shutdown_dashboard(self):
self._agg_sub.unregister()
def new_diagnostic_message_diagnostics(self, msg):
"""
callback to process dashboard_agg messages
"""
self._dashboard_message = msg
for status in msg.status:
if status.name == 'Irp6p Hardware Interface':
self._irp6p_motors_button.interpret_diagnostic_message(status)
elif status.name == 'Irp6ot Hardware Interface':
self._irp6ot_motors_button.interpret_diagnostic_message(status)
elif status.name == 'Conveyor Hardware Interface':
self._conveyor_motors_button.interpret_diagnostic_message(
status)
def new_diagnostic_message_diagnostics_agg(self, msg):
"""
callback to process dashboard_agg messages
"""
self._dashboard_message = msg
for status in msg.status:
if status.name == '/HardwareInterfaces/Irp6p/Irp6p Hardware Interface':
if (status.level != 3):
self._irp6p_motors_button.interpret_diagnostic_message(
status)
elif status.name == '/HardwareInterfaces/Irp6ot/Irp6ot Hardware Interface':
if (status.level != 3):
self._irp6ot_motors_button.interpret_diagnostic_message(
status)
elif status.name == '/HardwareInterfaces/Conveyor/Conveyor Hardware Interface':
if (status.level != 3):
self._conveyor_motors_button.interpret_diagnostic_message(
status)
def save_settings(self, plugin_settings, instance_settings):
self._console.save_settings(plugin_settings, instance_settings)
self._monitor.save_settings(plugin_settings, instance_settings)
def restore_settings(self, plugin_settings, instance_settings):
self._console.restore_settings(plugin_settings, instance_settings)
self._monitor.restore_settings(plugin_settings, instance_settings)
