def __init__(self, plugin_package="jsk_teleop_joy"):
self.state = self.STATE_INITIALIZATION
self.pre_status = None
self.history = StatusHistory(max_length=10)
self.menu_pub = rospy.Publisher("/overlay_menu", OverlayMenu)
self.controller_type = rospy.get_param('~controller_type', 'auto')
self.plugins = rospy.get_param('~plugins', {})
self.current_plugin_index = 0
self.selecting_plugin_index = 0
#you can specify the limit of the rate via ~diagnostic_period
self.diagnostic_updater = DiagnosticUpdater()
self.diagnostic_updater.setHardwareID("teleop_manager")
self.diagnostic_updater.add("State", self.stateDiagnostic)
self.diagnostic_updater.add("Plugin Status",
self.pluginStatusDiagnostic)
#self.diagnostic_updater.add("Joy Input", self.joyInputDiagnostic)
self.diagnostic_updater.update()
if self.controller_type == 'xbox':
self.JoyStatus = XBoxStatus
elif self.controller_type == 'ps3':
self.JoyStatus = PS3Status
elif self.controller_type == 'ps3wired':
self.JoyStatus = PS3WiredStatus
elif self.controller_type == 'ipega':
self.JoyStatus = IpegaStatus
elif self.controller_type == 'auto':
s = rospy.Subscriber('/joy', Joy, autoJoyDetect)
self.state = self.STATE_WAIT_FOR_JOY
error_message_published = False
r = rospy.Rate(1)
while not rospy.is_shutdown():
self.diagnostic_updater.update()
if AUTO_DETECTED_CLASS == "UNKNOWN":
if not error_message_published:
rospy.logfatal("unknown joy type")
error_message_published = True
r.sleep()
elif AUTO_DETECTED_CLASS:
self.JoyStatus = AUTO_DETECTED_CLASS
s.unregister()
break
else:
r.sleep()
self.diagnostic_updater.update()
self.plugin_manager = PluginManager(plugin_package)
self.loadPlugins()
