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)
