def __init__(self): self.srv_current = rospy.Service('~set_current', SetFloat, self.current_cb) self.srv_relative_remaining_capacity = rospy.Service( '~set_relative_remaining_capacity', SetFloat, self.relative_remaining_capacity_cb) self.poll_frequency = rospy.get_param('~poll_frequency_hz', 20.0) self.pub_voltage = rospy.Publisher('~voltage', Float64, queue_size=1) self.pub_current = rospy.Publisher('~current', Float64, queue_size=1) self.pub_remaining_capacity = rospy.Publisher('~remaining_capacity', Float64, queue_size=1) self.pub_full_charge_capacity = rospy.Publisher( '~full_charge_capacity', Float64, queue_size=1) self.pub_temparature = rospy.Publisher('~temperature', Float64, queue_size=1) self.updater = Updater() self.updater.setHardwareID("bms") self.updater.add("cob_bms_dagnostics_updater", self.produce_diagnostics) self.voltage = 48.0 self.current = -8.0 self.remaining_capacity = 35.0 self.full_charge_capacity = 35.0 # Ah self.temperature = 25.0 rospy.Timer(rospy.Duration(1.0), self.publish_diagnostics) rospy.Timer(rospy.Duration(1.0 / self.poll_frequency), self.timer_cb) rospy.Timer(rospy.Duration(1.0 / self.poll_frequency), self.timer_consume_power_cb)
def __init__(self): super(self.__class__, self).__init__(defaults) self.from_param_server() if tf_config: self._init_tf() if with_diagnostics: from diagnostic_updater import Updater self["updater"] = Updater() self["updater"].setHardwareID("none")
def testDiagnosticUpdater(self): rclpy.init() node = rclpy.create_node('test_node') updater = Updater(node) c = TestClass() updater.add('wrapped', c.wrapped) cf = ClassFunction() updater.add(cf)
def main(): hostname = socket.gethostname() rospy.init_node('cpu_monitor_%s' % hostname.replace("-", "_")) updater = Updater() updater.setHardwareID(hostname) updater.add(CpuTask(rospy.get_param("~warning_percentage", 90))) rate = rospy.Rate(rospy.get_param("~rate", 1)) while not rospy.is_shutdown(): rate.sleep() updater.update()
def __init__(self, axis, api): """Create the thread. :param axis: The parameter source. :type axis: :py:class:`AxisPTZ` :param api: The VAPIX API instance that allows the thread to access positional data. :type api: :py:class:`VAPIX` """ threading.Thread.__init__(self) self.axis = axis self.api = api # Permit program to exit even if threads are still running by flagging # thread as a daemon: self.daemon = True self._diagnostic_updater = Updater() self._diagnostic_updater.setHardwareID(api.hostname) # BACKWARDS COMPATIBILITY LAYER self.cameraPosition = None # deprecated self.msg = Axis() # deprecated
def __init__(self): device = get_param('~device', 'auto') baudrate = get_param('~baudrate', 0) timeout = get_param('~timeout', 0.002) if device == 'auto': devs = mtdevice.find_devices() if devs: device, baudrate = devs[0] rospy.loginfo("Detected MT device on port %s @ %d bps" % (device, baudrate)) else: rospy.logerr("Fatal: could not find proper MT device.") rospy.signal_shutdown("Could not find proper MT device.") return if not baudrate: baudrate = mtdevice.find_baudrate(device) if not baudrate: rospy.logerr("Fatal: could not find proper baudrate.") rospy.signal_shutdown("Could not find proper baudrate.") return rospy.loginfo("MT node interface: %s at %d bd." % (device, baudrate)) self.mt = mtdevice.MTDevice(device, baudrate, timeout) # optional no rotation procedure for internal calibration of biases # (only mark iv devices) no_rotation_duration = get_param('~no_rotation_duration', 0) if no_rotation_duration: rospy.loginfo( "Starting the no-rotation procedure to estimate the " "gyroscope biases for %d s. Please don't move the IMU" " during this time." % no_rotation_duration) self.mt.SetNoRotation(no_rotation_duration) self.frame_id = get_param('~frame_id', '/base_imu') self.frame_local = get_param('~frame_local', 'ENU') self.stest_stat = DiagnosticStatus(name='mtnode: Self Test', level=1, message='No status information') self.xkf_stat = DiagnosticStatus(name='mtnode: XKF Valid', level=1, message='No status information') self.gps_stat = DiagnosticStatus(name='mtnode: GPS Fix', level=1, message='No status information') self.device_info = { 'product_code': self.mt.GetProductCode(), 'device_id': self.mt.GetDeviceID(), 'firmware_rev': self.mt.GetFirmwareRev() } self.device_info.update(self.mt.GetConfiguration()) self.updater = Updater() self.updater.setHardwareID(str(self.mt.GetDeviceID())) #self.updater.add("Self Test", self.diagnostic_self_test) #self.updater.add("XKF", self.diagnostic_xkf) #self.updater.add("GPS Fix", self.diagnostic_gps) self.updater.add("Device Info", self.diagnostic_device) self.imu_freq = TopicDiagnostic( "imu/data", self.updater, FrequencyStatusParam({ 'min': 0, 'max': 100 }, 0.1, 10), TimeStampStatusParam()) self.mag_freq = TopicDiagnostic( "imu/mag", self.updater, FrequencyStatusParam({ 'min': 0, 'max': 100 }, 0.1, 10), TimeStampStatusParam()) # publishers created at first use to reduce topic clutter self.imu_pub = None self.gps_pub = None self.vel_pub = None self.mag_pub = None self.temp_pub = None self.press_pub = None self.analog_in1_pub = None # decide type+header self.analog_in2_pub = None # decide type+header self.ecef_pub = None self.time_ref_pub = None # TODO pressure, ITOW from raw GPS? self.old_bGPS = 256 # publish GPS only if new # publish a string version of all data; to be parsed by clients self.str_pub = rospy.Publisher('imu_data_str', String, queue_size=10)
def init_diagnostics(self): updater = Updater() updater.add("Computation Time", self.update_ct_diagnostic) rospy.Timer(rospy.Duration(1), lambda evt: updater.update())
def __init__(self, api, axis, flip_vertically=False, flip_horizontally=False, mirror_horizontally=False): """Create the controller. :param api: The VAPIX instance to be used to communicate with the camera. :type api: :py:class:`axis_camera.VAPIX` :param axis: The parameter holding class. :type axis: axis.Axis :param flip_vertically: If True, flip the controls vertically (for ceiling mounted cameras). :type flip_vertically: bool :param flip_horizontally: If True, flip the controls horizontally (for ceiling mounted cameras). :type flip_horizontally: bool :param mirror_horizontally: If True, mirror the controls horizontally (for backwards mounted cameras). :type mirror_horizontally: bool """ self._api = api self._axis = axis self._flip_vertically = flip_vertically self._flip_horizontally = flip_horizontally self._mirror_horizontally = mirror_horizontally self._parameter_update_mutex = threading.Lock() self._executing_parameter_update = False self._reconfigure_mutex = threading.Lock() self._executing_reconfigure = False self._autofocus = True self._focus = 0 self._autoiris = True self._iris = 0 self._brightness = 5000 self._backlight = False self._ir_cut_filter = True self._dynamic_reconfigure_server = Server(CameraConfig, self._reconfigure, "axis_ptz_driver") self._diagnostic_updater = Updater() self._diagnostic_updater.setHardwareID(api.hostname) self._diagnostic_updater.add(FunctionDiagnosticTask("Last command status", self._diagnostic_last_command)) self._last_command_error = None timer = rospy.Timer(rospy.Duration(1), lambda (event): self._diagnostic_updater.update()) # set up the topics this node listens on # the callbacks are created as lambda wrappers around the Python API functions of this controller so that we # do not blow this file with more unnecessary methods if self._api.has_capabilities('AbsolutePan', 'AbsoluteTilt', 'AbsoluteZoom'): self._absolute_ptz_subscriber = rospy.Subscriber( "control/pan_tilt_zoom/absolute", PTZ, self._call_with_ptz_message(self.set_ptz), queue_size=100) if self._api.has_capabilities('RelativePan', 'RelativeTilt', 'RelativeZoom'): self._relative_ptz_subscriber = rospy.Subscriber( "control/pan_tilt_zoom/relative", PTZ, self._call_with_ptz_message(self.adjust_ptz), queue_size=100) if self._api.has_capabilities('ContinuousPan', 'ContinuousTilt', 'ContinuousZoom'): self._velocity_ptz_subscriber = rospy.Subscriber( "control/pan_tilt_zoom/velocity", PTZ, self._call_with_ptz_message(self.set_ptz_velocity), queue_size=100) if self._api.has_capabilities('AbsolutePan', 'AbsoluteTilt'): self._absolute_pan_tilt_subscriber = rospy.Subscriber( "control/pan_tilt/absolute", PTZ, self._call_with_pt_message(self.set_pan_tilt), queue_size=100) if self._api.has_capabilities('RelativePan', 'RelativeTilt'): self._relative_pan_tilt_subscriber = rospy.Subscriber( "control/pan_tilt/relative", PTZ, self._call_with_pt_message(self.adjust_pan_tilt), queue_size=100) if self._api.has_capabilities('ContinuousPan', 'ContinuousTilt'): self._velocity_pan_tilt_subscriber = rospy.Subscriber( "control/pan_tilt/velocity", PTZ, self._call_with_pt_message(self.set_pan_tilt_velocity), queue_size=100) if self._api.has_capability('AbsolutePan'): self._absolute_pan_subscriber = rospy.Subscriber( "control/pan/absolute", Float32, self._call_with_simple_message_data(self.set_pan), queue_size=100) if self._api.has_capability('AbsoluteTilt'): self._absolute_tilt_subscriber = rospy.Subscriber( "control/tilt/absolute", Float32, self._call_with_simple_message_data(self.set_tilt), queue_size=100) if self._api.has_capability('AbsoluteZoom'): self._absolute_zoom_subscriber = rospy.Subscriber( "control/zoom/absolute", Int32, self._call_with_simple_message_data(self.set_zoom), queue_size=100) if self._api.has_capability('RelativePan'): self._relative_pan_subscriber = rospy.Subscriber( "control/pan/relative", Float32, self._call_with_simple_message_data(self.adjust_pan), queue_size=100) if self._api.has_capability('RelativeTilt'): self._relative_tilt_subscriber = rospy.Subscriber( "control/tilt/relative", Float32, self._call_with_simple_message_data(self.adjust_tilt), queue_size=100) if self._api.has_capability('RelativeZoom'): self._relative_zoom_subscriber = rospy.Subscriber( "control/zoom/relative", Int32, self._call_with_simple_message_data(self.adjust_zoom), queue_size=100) if self._api.has_capability('ContinuousPan'): self._pan_velocity_subscriber = rospy.Subscriber( "control/pan/velocity", Int32, self._call_with_simple_message_data(self.set_pan_velocity), queue_size=100) if self._api.has_capability('ContinuousTilt'): self._tilt_velocity_subscriber = rospy.Subscriber( "control/tilt/velocity", Int32, self._call_with_simple_message_data(self.set_tilt_velocity), queue_size=100) if self._api.has_capability('ContinuousZoom'): self._zoom_velocity_subscriber = rospy.Subscriber( "control/zoom/velocity", Int32, self._call_with_simple_message_data(self.set_zoom_velocity), queue_size=100) if self._api.has_capabilities('AbsolutePan', 'AbsoluteTilt', 'AbsoluteZoom'): self._look_at_subscriber = rospy.Subscriber( "control/look_at", PointInRectangle, self._call_with_pir_message(self.look_at), queue_size=100) if self._api.has_capability('AutoFocus'): self._autofocus_subscriber = rospy.Subscriber( "control/camera/focus/auto", Bool, self._call_with_simple_message_data(self.set_autofocus), queue_size=100) if self._api.has_capability('AbsoluteFocus'): self._focus_subscriber = rospy.Subscriber( "control/camera/focus/absolute", Int32, self._call_with_simple_message_data(self.set_focus), queue_size=100) if self._api.has_capability('RelativeFocus'): self._focus_relative_subscriber = rospy.Subscriber( "control/camera/focus/relative", Int32, self._call_with_simple_message_data(self.adjust_focus), queue_size=100) if self._api.has_capability('ContinuousFocus'): self._focus_velocity_subscriber = rospy.Subscriber( "control/camera/focus/velocity", Int32, self._call_with_simple_message_data(self.set_focus_velocity), queue_size=100) if self._api.has_capability('AutoIris'): self._autoiris_subscriber = rospy.Subscriber( "control/camera/iris/auto", Bool, self._call_with_simple_message_data(self.set_autoiris), queue_size=100) if self._api.has_capability('AbsoluteIris'): self._iris_subscriber = rospy.Subscriber( "control/camera/iris/absolute", Int32, self._call_with_simple_message_data(self.set_iris), queue_size=100) if self._api.has_capability('RelativeIris'): self._iris_relative_subscriber = rospy.Subscriber( "control/camera/iris/relative", Int32, self._call_with_simple_message_data(self.adjust_iris), queue_size=100) if self._api.has_capability('ContinuousIris'): self._iris_velocity_subscriber = rospy.Subscriber( "control/camera/iris/velocity", Int32, self._call_with_simple_message_data(self.set_iris_velocity), queue_size=100) if self._api.has_capability('AbsoluteBrightness'): self._brightness_subscriber = rospy.Subscriber( "control/camera/brightness/absolute", Int32, self._call_with_simple_message_data(self.set_brightness), queue_size=100) if self._api.has_capability('RelativeBrightness'): self._brightness_relative_subscriber = rospy.Subscriber( "control/camera/brightness/relative", Int32, self._call_with_simple_message_data(self.adjust_brightness), queue_size=100) if self._api.has_capability('ContinuousBrightness'): self._brightness_velocity_subscriber = rospy.Subscriber( "control/camera/brightness/velocity", Int32, self._call_with_simple_message_data(self.set_brightness_velocity), queue_size=100) if self._api.has_capability('BackLight'): self._use_backlight_subscriber = rospy.Subscriber( "control/camera/backlight_compensation", Bool, self._call_with_simple_message_data(self.use_backlight_compensation), queue_size=100) if self._api.has_capabilities('IrCutFilter', 'AutoIrCutFilter'): self._use_ir_cut_filter_auto_subscriber = rospy.Subscriber( "control/camera/ir_cut_filter/auto", Bool, self._call_with_simple_message_data(self.set_ir_cut_filter_auto), queue_size=100) if self._api.has_capabilities('IrCutFilter'): self._use_ir_cut_filter_use_subscriber = rospy.Subscriber( "control/camera/ir_cut_filter/use", Bool, self._call_with_simple_message_data(self.set_ir_cut_filter_use), queue_size=100)