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()
class JoyManager():
STATE_INITIALIZATION = 1
STATE_RUNNING = 2
STATE_WAIT_FOR_JOY = 3
MODE_PLUGIN = 0
MODE_MENU = 1
mode = 0
plugin_instances = []
def stateDiagnostic(self, stat):
if self.state == self.STATE_INITIALIZATION:
stat.summary(DiagnosticStatus.WARN,
"initializing JoyManager")
elif self.state == self.STATE_RUNNING:
stat.summary(DiagnosticStatus.OK,
"running")
stat.add("Joy stick type", str(self.JoyStatus))
elif self.state == self.STATE_WAIT_FOR_JOY:
stat.summary(DiagnosticStatus.WARN,
"waiting for joy message to detect joy stick type")
return stat
def pluginStatusDiagnostic(self, stat):
if len(self.plugin_instances) == 0:
stat.summary(DiagnosticStatus.ERROR, "no plugin is loaded")
else:
stat.summary(DiagnosticStatus.OK,
"%d plugins are loaded" % (len(self.plugin_instances)))
stat.add("instances", ", ".join([p.name for p in self.plugin_instances]))
return stat
def __init__(self):
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
#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 == '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('jsk_teleop_joy')
self.loadPlugins()
def loadPlugins(self):
self.plugin_manager.loadPlugins()
self.plugin_instances = self.plugin_manager.loadPluginInstances(self.plugins)
def switchPlugin(self, index):
self.current_plugin_index = index
if len(self.plugin_instances) <= self.current_plugin_index:
self.current_plugin_index = 0
elif self.current_plugin_index < 0:
self.current_plugin_index = len(self.plugin_instances)
self.current_plugin.disable()
self.current_plugin = self.plugin_instances[self.current_plugin_index]
self.current_plugin.enable()
def nextPlugin(self):
rospy.loginfo('switching to next plugin')
self.switchPlugin(self, self.current_plugin_index + 1)
def start(self):
self.publishMenu(0, close=True) # close menu anyway
self.diagnostic_updater.force_update()
if len(self.plugin_instances) == 0:
rospy.logfatal('no valid plugins are loaded')
return False
self.current_plugin = self.plugin_instances[0]
self.current_plugin.enable()
self.joy_subscriber = rospy.Subscriber('/joy', Joy, self.joyCB)
self.state = self.STATE_RUNNING
return True
def publishMenu(self, index, close=False):
menu = OverlayMenu()
menu.menus = [p.name for p in self.plugin_instances]
menu.current_index = index
menu.title = "Joystick"
if close:
menu.action = OverlayMenu.ACTION_CLOSE
self.menu_pub.publish(menu)
def processMenuMode(self, status, history):
if history.new(status, "down"):
self.selecting_plugin_index = self.selecting_plugin_index + 1
if self.selecting_plugin_index >= len(self.plugin_instances):
self.selecting_plugin_index = 0
self.publishMenu(self.selecting_plugin_index)
elif history.new(status, "up"):
self.selecting_plugin_index = self.selecting_plugin_index - 1
if self.selecting_plugin_index < 0:
self.selecting_plugin_index = len(self.plugin_instances) - 1
self.publishMenu(self.selecting_plugin_index)
elif history.new(status, "cross") or history.new(status, "center"):
self.publishMenu(self.selecting_plugin_index, close=True)
self.mode = self.MODE_PLUGIN
elif history.new(status, "circle"):
self.publishMenu(self.selecting_plugin_index, close=True)
self.mode = self.MODE_PLUGIN
self.switchPlugin(self.selecting_plugin_index)
def joyCB(self, msg):
status = self.JoyStatus(msg)
if self.mode == self.MODE_MENU:
self.processMenuMode(status, self.history)
else:
if self.history.new(status, "center"):
self.selecting_plugin_index = self.current_plugin_index
self.publishMenu(self.current_plugin_index)
self.mode = self.MODE_MENU
else:
self.current_plugin.joyCB(status, self.history)
self.pre_status = status
self.history.add(status)
self.diagnostic_updater.update()
class FakeBMS(object):
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 current_cb(self, req):
self.current = round(req.data,2)
res_current = SetFloatResponse(True, "Set current to {}".format(self.current))
return res_current
def relative_remaining_capacity_cb(self, req):
self.remaining_capacity = round(((req.data * self.full_charge_capacity)/100.0), 3)
res_capacity = SetFloatResponse(True, "Set relative remaining capacity to {}".format(self.remaining_capacity))
return res_capacity
def publish_diagnostics(self, event):
self.updater.update()
def produce_diagnostics(self, stat):
stat.summary(DiagnosticStatus.OK, "Fake Driver: Ready")
stat.add("current[A]", self.current)
stat.add("voltage[V]", self.voltage)
stat.add("temperature[Celsius]", self.temperature)
stat.add("remaining_capacity[Ah]", self.remaining_capacity)
stat.add("full_charge_capacity[Ah]", self.full_charge_capacity)
return stat
def timer_cb(self, event):
self.pub_voltage.publish(self.voltage)
self.pub_current.publish(self.current)
self.pub_remaining_capacity.publish(self.remaining_capacity)
self.pub_full_charge_capacity.publish(self.full_charge_capacity)
self.pub_temparature.publish(self.temperature)
def timer_consume_power_cb(self, event):
# emulate the battery usage based on the current values
self.remaining_capacity += (self.current/self.poll_frequency)/3600.0
self.remaining_capacity = round(self.remaining_capacity,3)
if self.remaining_capacity <= 0.0:
self.remaining_capacity = 0.0
if self.remaining_capacity >= self.full_charge_capacity:
self.remaining_capacity = round(self.full_charge_capacity,3)
class JoyManager():
STATE_INITIALIZATION = 1
STATE_RUNNING = 2
STATE_WAIT_FOR_JOY = 3
MODE_PLUGIN = 0
MODE_MENU = 1
mode = 0
plugin_instances = []
def stateDiagnostic(self, stat):
if self.state == self.STATE_INITIALIZATION:
stat.summary(DiagnosticStatus.WARN, "initializing JoyManager")
elif self.state == self.STATE_RUNNING:
stat.summary(DiagnosticStatus.OK, "running")
stat.add("Joy stick type", str(self.JoyStatus))
elif self.state == self.STATE_WAIT_FOR_JOY:
stat.summary(DiagnosticStatus.WARN,
"waiting for joy message to detect joy stick type")
return stat
def pluginStatusDiagnostic(self, stat):
if len(self.plugin_instances) == 0:
stat.summary(DiagnosticStatus.ERROR, "no plugin is loaded")
else:
stat.summary(
DiagnosticStatus.OK,
"%d plugins are loaded" % (len(self.plugin_instances)))
stat.add("instances",
", ".join([p.name for p in self.plugin_instances]))
return stat
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()
def loadPlugins(self):
self.plugin_manager.loadPlugins()
self.plugin_instances = self.plugin_manager.loadPluginInstances(
self.plugins, self)
def switchPlugin(self, index):
self.current_plugin_index = index
if len(self.plugin_instances) <= self.current_plugin_index:
self.current_plugin_index = 0
elif self.current_plugin_index < 0:
self.current_plugin_index = len(self.plugin_instances)
self.current_plugin.disable()
self.current_plugin = self.plugin_instances[self.current_plugin_index]
self.current_plugin.enable()
def nextPlugin(self):
rospy.loginfo('switching to next plugin')
self.switchPlugin(self, self.current_plugin_index + 1)
def start(self):
self.publishMenu(0, close=True) # close menu anyway
self.diagnostic_updater.force_update()
if len(self.plugin_instances) == 0:
rospy.logfatal('no valid plugins are loaded')
return False
self.current_plugin = self.plugin_instances[0]
self.current_plugin.enable()
self.joy_subscriber = rospy.Subscriber('/joy', Joy, self.joyCB)
self.state = self.STATE_RUNNING
return True
def publishMenu(self, index, close=False):
menu = OverlayMenu()
menu.menus = [p.name for p in self.plugin_instances]
menu.current_index = index
menu.title = "Joystick"
if close:
menu.action = OverlayMenu.ACTION_CLOSE
self.menu_pub.publish(menu)
def forceToPluginMenu(self, publish_menu=False):
self.selecting_plugin_index = self.current_plugin_index
if publish_menu:
self.publishMenu(self.current_plugin_index)
self.mode = self.MODE_MENU
def processMenuMode(self, status, history):
if history.new(status, "down") or history.new(status,
"left_analog_down"):
self.selecting_plugin_index = self.selecting_plugin_index + 1
if self.selecting_plugin_index >= len(self.plugin_instances):
self.selecting_plugin_index = 0
self.publishMenu(self.selecting_plugin_index)
elif history.new(status, "up") or history.new(status,
"left_analog_up"):
self.selecting_plugin_index = self.selecting_plugin_index - 1
if self.selecting_plugin_index < 0:
self.selecting_plugin_index = len(self.plugin_instances) - 1
self.publishMenu(self.selecting_plugin_index)
elif history.new(status, "cross") or history.new(status, "center"):
self.publishMenu(self.selecting_plugin_index, close=True)
self.mode = self.MODE_PLUGIN
elif history.new(status, "circle"):
self.publishMenu(self.selecting_plugin_index, close=True)
self.mode = self.MODE_PLUGIN
self.switchPlugin(self.selecting_plugin_index)
else:
self.publishMenu(self.selecting_plugin_index)
def joyCB(self, msg):
status = self.JoyStatus(msg)
if self.mode == self.MODE_MENU:
self.processMenuMode(status, self.history)
else:
if self.history.new(status, "center"):
self.forceToPluginMenu()
else:
self.current_plugin.joyCB(status, self.history)
self.pre_status = status
self.history.add(status)
self.diagnostic_updater.update()
class PositionStreamingThread(threading.Thread):
"""
This class handles publication of the positional state of the camera to a ROS message and to joint_states
(and using robot_state_publisher also to TF).
The thread doesn't support pausing, because it doesn't make sense if we want TF data.
"""
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 run(self):
"""Run the thread."""
frequency = self.axis._state_publishing_frequency
rate = rospy.Rate(frequency)
state_publisher = DiagnosedPublisher(
rospy.Publisher("camera/ptz", PTZ, queue_size=100),
self._diagnostic_updater,
FrequencyStatusParam({
'min': frequency,
'max': frequency
},
tolerance=0.2), TimeStampStatusParam())
joint_states_publisher = DiagnosedPublisher(
rospy.Publisher("camera/joint_states", JointState, queue_size=100),
self._diagnostic_updater,
FrequencyStatusParam({
'min': frequency,
'max': frequency
},
tolerance=0.2), TimeStampStatusParam())
parameter_updates_publisher = rospy.Publisher(
"axis_ptz/parameter_updates", Config, queue_size=100)
while not rospy.is_shutdown():
# publish position forever
try:
# get camera position from API
camera_position = self.api.get_camera_position(
) # we use deprecated param values
message_time = rospy.Time.now()
# Create and publish the PTZ message
message = self._create_camera_position_message(
camera_position, message_time)
state_publisher.publish(message)
# Create and publish JointState
joint_states_message = self._create_camera_joint_states_message(
camera_position, message_time)
joint_states_publisher.publish(joint_states_message)
# Publish the parameter updates (because of backwards compatibility)
parameter_updates_message = self._create_parameter_updates_message(
camera_position)
parameter_updates_publisher.publish(parameter_updates_message)
# set the parameters (because of backwards compatibility)
rospy.set_param("axis_ptz/pan", camera_position['pan'])
rospy.set_param("axis_ptz/tilt", camera_position['tilt'])
rospy.set_param("axis_ptz/zoom", camera_position['zoom'])
# Create and publish the deprecated Axis message
self.cameraPosition = camera_position # backwards compatibility
self.publishCameraState() # backwards compatibility
except (IOError, ValueError, RuntimeError) as e:
rospy.logwarn(
"Could not determine current camera position. Waiting 1 s. Cause: %s"
% repr(e.args))
rospy.sleep(1)
self._diagnostic_updater.update()
try:
rate.sleep()
except rospy.ROSTimeMovedBackwardsException:
rospy.logwarn("Detected jump back in time.")
def _create_camera_position_message(self, camera_position, timestamp):
"""Convert the camera_position dictionary to a PTZ message.
:param camera_position: The camera position. Should contain keys 'pan', 'tilt', and probably also 'zoom'.
:type camera_position: dict
:param timestamp: The time we relate the camera position to.
:type timestamp: :py:class:`rospy.Time`
:return: The PTZ message.
:rtype: :py:class:`axis_camera.msg.PTZ`
"""
message = PTZ()
message.header.stamp = timestamp
message.header.frame_id = self.axis._frame_id
message.pan = camera_position['pan']
message.tilt = camera_position['tilt']
if 'zoom' in camera_position:
message.zoom = camera_position['zoom']
if self.axis.flip:
self._correct_flipped_pan_tilt_in_message(message)
return message
def _create_camera_joint_states_message(self, camera_position, timestamp):
"""
Convert the camera_position dictionary to a JointState message.
:param camera_position: The camera position. Should contain keys 'pan', 'tilt', and probably also 'zoom'.
:type camera_position: dict
:param timestamp: The time we relate the camera position to.
:type timestamp: :py:class:`rospy.Time`
:return: The JointState message.
:rtype: :py:class:`sensor_msgs.msg.JointState`
"""
message = JointState()
message.header.stamp = timestamp
message.header.frame_id = self.axis._frame_id
message.name = ["axis_pan_j", "axis_tilt_j"]
message.position = [
radians(camera_position['pan']),
radians(camera_position['tilt'])
]
# TODO message.velocity???
# TODO flipping?
return message
def _create_parameter_updates_message(self, camera_position):
"""
Create a parameter_updates message to update the deprecated dynamic_reconigurable pan, tilt and zoom params.
:param camera_position: The camera position. Should contain keys 'pan', 'tilt', and probably also 'zoom'.
:type camera_position: dict
:return: The Config message.
:rtype: :py:class:`axis_camera.cfg.Config`
"""
message = Config()
message.doubles.append(
DoubleParameter(name="pan", value=camera_position['pan']))
message.doubles.append(
DoubleParameter(name="tilt", value=camera_position['tilt']))
message.ints.append(
IntParameter(name="zoom", value=camera_position['zoom']))
return message
@staticmethod
def _correct_flipped_pan_tilt_in_message(message):
"""
If flipping the image is required, do the flipping on a PTZ message fields.
:param message: A PTZ or Axis message.
:type message: :py:class:`axis_camera.msg.PTZ` | :py:class:`axis_camera.msg.Axis`
"""
message.pan = 180 - message.pan
if message.pan > 180:
message.pan -= 360
elif message.pan < -180:
message.pan += 360
message.tilt = -message.tilt
# BACKWARDS COMPATIBILITY LAYER
def queryCameraPosition(self): # deprecated
"""Deprecated."""
pass # is done in the run method
def publishCameraState(self): # deprecated
"""Deprecated."""
if self.cameraPosition is not None:
self.msg.pan = float(self.cameraPosition['pan'])
if self.axis.flip:
self.adjustForFlippedOrientation()
self.msg.tilt = float(self.cameraPosition['tilt'])
self.msg.zoom = float(self.cameraPosition['zoom'])
self.msg.iris = 0.0
if 'iris' in self.cameraPosition:
self.msg.iris = float(self.cameraPosition['iris'])
self.msg.focus = 0.0
if 'focus' in self.cameraPosition:
self.msg.focus = float(self.cameraPosition['focus'])
if 'autofocus' in self.cameraPosition:
self.msg.autofocus = (self.cameraPosition['autofocus'] == 'on')
self.axis.pub.publish(self.msg)
def adjustForFlippedOrientation(self): # deprecated
"""Deprecated."""
self._correct_flipped_pan_tilt_in_message(self.msg)
class PositionStreamingThread(threading.Thread):
"""
This class handles publication of the positional state of the camera to a ROS message and to joint_states
(and using robot_state_publisher also to TF).
The thread doesn't support pausing, because it doesn't make sense if we want TF data.
"""
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 run(self):
"""Run the thread."""
frequency = self.axis._state_publishing_frequency
rate = rospy.Rate(frequency)
state_publisher = DiagnosedPublisher(
rospy.Publisher("camera/ptz", PTZ, queue_size=100),
self._diagnostic_updater, FrequencyStatusParam({'min': frequency, 'max': frequency}, tolerance=0.2), TimeStampStatusParam()
)
joint_states_publisher = DiagnosedPublisher(
rospy.Publisher("camera/joint_states", JointState, queue_size=100),
self._diagnostic_updater, FrequencyStatusParam({'min': frequency, 'max': frequency}, tolerance=0.2), TimeStampStatusParam()
)
parameter_updates_publisher = rospy.Publisher("axis_ptz/parameter_updates", Config, queue_size=100)
while not rospy.is_shutdown():
# publish position forever
try:
# get camera position from API
camera_position = self.api.get_camera_position() # we use deprecated param values
message_time = rospy.Time.now()
# Create and publish the PTZ message
message = self._create_camera_position_message(camera_position, message_time)
state_publisher.publish(message)
# Create and publish JointState
joint_states_message = self._create_camera_joint_states_message(camera_position, message_time)
joint_states_publisher.publish(joint_states_message)
# Publish the parameter updates (because of backwards compatibility)
parameter_updates_message = self._create_parameter_updates_message(camera_position)
parameter_updates_publisher.publish(parameter_updates_message)
# set the parameters (because of backwards compatibility)
rospy.set_param("axis_ptz/pan", camera_position['pan'])
rospy.set_param("axis_ptz/tilt", camera_position['tilt'])
rospy.set_param("axis_ptz/zoom", camera_position['zoom'])
# Create and publish the deprecated Axis message
self.cameraPosition = camera_position # backwards compatibility
self.publishCameraState() # backwards compatibility
except (IOError, ValueError, RuntimeError) as e:
rospy.logwarn("Could not determine current camera position. Waiting 1 s. Cause: %s" % repr(e.args))
rospy.sleep(1)
self._diagnostic_updater.update()
try:
rate.sleep()
except rospy.ROSTimeMovedBackwardsException:
rospy.logwarn("Detected jump back in time.")
def _create_camera_position_message(self, camera_position, timestamp):
"""Convert the camera_position dictionary to a PTZ message.
:param camera_position: The camera position. Should contain keys 'pan', 'tilt', and probably also 'zoom'.
:type camera_position: dict
:param timestamp: The time we relate the camera position to.
:type timestamp: :py:class:`rospy.Time`
:return: The PTZ message.
:rtype: :py:class:`axis_camera.msg.PTZ`
"""
message = PTZ()
message.header.stamp = timestamp
message.header.frame_id = self.axis._frame_id
message.pan = camera_position['pan']
message.tilt = camera_position['tilt']
if 'zoom' in camera_position:
message.zoom = camera_position['zoom']
if self.axis.flip:
self._correct_flipped_pan_tilt_in_message(message)
return message
def _create_camera_joint_states_message(self, camera_position, timestamp):
"""
Convert the camera_position dictionary to a JointState message.
:param camera_position: The camera position. Should contain keys 'pan', 'tilt', and probably also 'zoom'.
:type camera_position: dict
:param timestamp: The time we relate the camera position to.
:type timestamp: :py:class:`rospy.Time`
:return: The JointState message.
:rtype: :py:class:`sensor_msgs.msg.JointState`
"""
message = JointState()
message.header.stamp = timestamp
message.header.frame_id = self.axis._frame_id
message.name = ["axis_pan_j", "axis_tilt_j"]
message.position = [radians(camera_position['pan']), radians(camera_position['tilt'])]
# TODO message.velocity???
# TODO flipping?
return message
def _create_parameter_updates_message(self, camera_position):
"""
Create a parameter_updates message to update the deprecated dynamic_reconigurable pan, tilt and zoom params.
:param camera_position: The camera position. Should contain keys 'pan', 'tilt', and probably also 'zoom'.
:type camera_position: dict
:return: The Config message.
:rtype: :py:class:`axis_camera.cfg.Config`
"""
message = Config()
message.doubles.append(DoubleParameter(name="pan", value=camera_position['pan']))
message.doubles.append(DoubleParameter(name="tilt", value=camera_position['tilt']))
message.ints.append(IntParameter(name="zoom", value=camera_position['zoom']))
return message
@staticmethod
def _correct_flipped_pan_tilt_in_message(message):
"""
If flipping the image is required, do the flipping on a PTZ message fields.
:param message: A PTZ or Axis message.
:type message: :py:class:`axis_camera.msg.PTZ` | :py:class:`axis_camera.msg.Axis`
"""
message.pan = 180 - message.pan
if message.pan > 180:
message.pan -= 360
elif message.pan < -180:
message.pan += 360
message.tilt = -message.tilt
# BACKWARDS COMPATIBILITY LAYER
def queryCameraPosition(self): # deprecated
"""Deprecated."""
pass # is done in the run method
def publishCameraState(self): # deprecated
"""Deprecated."""
if self.cameraPosition is not None:
self.msg.pan = float(self.cameraPosition['pan'])
if self.axis.flip:
self.adjustForFlippedOrientation()
self.msg.tilt = float(self.cameraPosition['tilt'])
self.msg.zoom = float(self.cameraPosition['zoom'])
self.msg.iris = 0.0
if 'iris' in self.cameraPosition:
self.msg.iris = float(self.cameraPosition['iris'])
self.msg.focus = 0.0
if 'focus' in self.cameraPosition:
self.msg.focus = float(self.cameraPosition['focus'])
if 'autofocus' in self.cameraPosition:
self.msg.autofocus = (self.cameraPosition['autofocus'] == 'on')
self.axis.pub.publish(self.msg)
def adjustForFlippedOrientation(self): # deprecated
"""Deprecated."""
self._correct_flipped_pan_tilt_in_message(self.msg)
class FakeBMS(object):
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 current_cb(self, req):
self.current = round(req.data, 2)
res_current = SetFloatResponse(
True, "Set current to {}".format(self.current))
return res_current
def relative_remaining_capacity_cb(self, req):
self.remaining_capacity = round(
((req.data * self.full_charge_capacity) / 100.0), 3)
res_capacity = SetFloatResponse(
True, "Set relative remaining capacity to {}".format(
self.remaining_capacity))
return res_capacity
def publish_diagnostics(self, event):
self.updater.update()
def produce_diagnostics(self, stat):
stat.summary(DiagnosticStatus.OK, "Fake Driver: Ready")
stat.add("current[A]", self.current)
stat.add("voltage[V]", self.voltage)
stat.add("temperature[Celsius]", self.temperature)
stat.add("remaining_capacity[Ah]", self.remaining_capacity)
stat.add("full_charge_capacity[Ah]", self.full_charge_capacity)
return stat
def timer_cb(self, event):
self.pub_voltage.publish(self.voltage)
self.pub_current.publish(self.current)
self.pub_remaining_capacity.publish(self.remaining_capacity)
self.pub_full_charge_capacity.publish(self.full_charge_capacity)
self.pub_temparature.publish(self.temperature)
def timer_consume_power_cb(self, event):
# emulate the battery usage based on the current values
self.remaining_capacity += (self.current /
self.poll_frequency) / 3600.0
self.remaining_capacity = round(self.remaining_capacity, 3)
if self.remaining_capacity <= 0.0:
self.remaining_capacity = 0.0
if self.remaining_capacity >= self.full_charge_capacity:
self.remaining_capacity = round(self.full_charge_capacity, 3)
def init_diagnostics(self):
updater = Updater()
updater.add("Computation Time", self.update_ct_diagnostic)
rospy.Timer(rospy.Duration(1), lambda evt: updater.update())
class AxisCameraController(object):
"""A class serving as the Python and ROS controller of the Axis camera.
It provides numerous topics/methods using which the camera can be controlled via VAPIX.
Each topic is only subscribed if the camera reports capabilities required for executing the topic's functionality.
Currently provided topics are:
- control/pan_tilt_zoom/absolute
- control/pan_tilt_zoom/relative
- control/pan_tilt_zoom/velocity
- control/pan_tilt/absolute
- control/pan_tilt/relative
- control/pan_tilt/velocity
- control/pan/absolute
- control/tilt/absolute
- control/zoom/absolute
- control/pan/relative
- control/tilt/relative
- control/zoom/relative
- control/pan/velocity
- control/tilt/velocity
- control/zoom/velocity
- control/look_at
- control/camera/focus/auto
- control/camera/focus/absolute
- control/camera/focus/relative
- control/camera/focus/velocity
- control/camera/iris/auto
- control/camera/iris/absolute
- control/camera/iris/relative
- control/camera/iris/velocity
- control/camera/brightness/absolute
- control/camera/brightness/relative
- control/camera/brightness/velocity
- control/camera/backlight_compensation
- control/camera/ir_cut_filter/auto
- control/camera/ir_cut_filter/use
"""
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)
def set_ptz(self, pan, tilt, zoom):
"""Command the PTZ unit with an absolute pose taking into account flips and mirroring.
:param pan: The desired pan. In None, pan is not commanded at all.
The value is automatically normalized to <-180,+180>
:type pan: float
:param tilt: The desired tilt. In None, tilt is not commanded at all.
The value is automatically normalized to <-180,+180>
:type tilt: float
:param zoom: The desired zoom level. In None, zoom is not commanded at all.
:type pan: int
:return: The pan, tilt and zoom values that were really applied (e.g. the cropped and normalized input)
:rtype: tuple
:raises: :py:class:`RuntimeError` if the camera doesn't have capabilities to execute the given command.
:raises: :py:class:`IOError`, :py:class:`urllib2.URLError` on network communication error
.. note:: This call doesn't wait for the command to finish execution (which might take a while if e.g. a move
from -170 to +170 pan is requested).
.. note:: Since the pan and tilt values are periodic and normalization takes place in this function, you can
simply call this command in loops like e.g. ``for pan in range(0,3600,30): move_ptz_absolute(pan)``
to rotate the camera 10 times.
"""
(pan, tilt) = self._apply_flip_and_mirror_absolute(pan, tilt)
return self._api.move_ptz_absolute(pan, tilt, zoom)
def adjust_ptz(self, pan, tilt, zoom):
"""Command the PTZ unit with a relative pose shift taking into account flips and mirroring.
:param pan: The pan change. In None or 0, pan remains unchanged.
The value is automatically normalized to <-360,+360>. May be negative.
:type pan: float
:param tilt: The tilt change. In None or 0, tilt remains unchanged.
The value is automatically normalized to <-360,+360>. May be negative.
:type tilt: float
:param zoom: The zoom change. In None or 0, zoom remains unchanged. May be negative.
:type pan: int
:return: The pan, tilt and zoom change values that were really applied (e.g. the cropped and normalized input)
:rtype: tuple
:raises: :py:class:`RuntimeError` if the camera doesn't have capabilities to execute the given command.
:raises: :py:class:`IOError`, :py:class:`urllib2.URLError` on network communication error
.. note:: This call doesn't wait for the command to finish execution (which might take a while if e.g. a move
by 300 deg pan is requested).
"""
(pan, tilt) = self._apply_flip_and_mirror_relative(pan, tilt)
return self._api.move_ptz_relative(pan, tilt, zoom)
def set_ptz_velocity(self, pan, tilt, zoom):
"""Command the PTZ unit velocity in terms of pan, tilt and zoom taking into account flips and mirroring.
:param pan: Pan speed. In None or 0, pan remains unchanged. Pan speed is aperiodic (can be higher than 360).
May be negative.
:type pan: int
:param tilt: Tilt speed. In None or 0, tilt remains unchanged. Tilt speed is aperiodic (can be higher than 360).
May be negative.
:type tilt: int
:param zoom: Zoom speed. In None or 0, zoom remains unchanged. May be negative.
:type pan: int
:return: The pan, tilt and zoom change values that were really applied (e.g. the cropped and normalized input)
:rtype: tuple
:raises: :py:class:`RuntimeError` if the camera doesn't have capabilities to execute the given command.
:raises: :py:class:`IOError`, :py:class:`urllib2.URLError` on network communication error
.. note:: This call doesn't wait for the command to finish execution.
.. note:: It is not clearly stated in the API, but practically any following absolute/relative pose command
sets velocity to zero.
"""
(pan, tilt) = self._apply_flip_and_mirror_velocity(pan, tilt)
return self._api.set_ptz_velocity(pan, tilt, zoom)
def set_pan_tilt(self, pan, tilt):
"""Command the PTZ unit with an absolute pan and tilt taking into account flips and mirroring.
:param pan: The desired pan. In None, pan is not commanded at all.
The value is automatically normalized to <-180,+180>
:type pan: float
:param tilt: The desired tilt. In None, tilt is not commanded at all.
The value is automatically normalized to <-180,+180>
:type tilt: float
:return: The pan and tilt values that were really applied (e.g. the cropped and normalized input)
:rtype: tuple
:raises: :py:class:`RuntimeError` if the camera doesn't have capabilities to execute the given command.
:raises: :py:class:`IOError`, :py:class:`urllib2.URLError` on network communication error
.. note:: This call doesn't wait for the command to finish execution (which might take a while if e.g. a move
from -170 to +170 pan is requested).
.. note:: Since the pan and tilt values are periodic and normalization takes place in this function, you can
simply call this command in loops like e.g. ``for pan in range(0,3600,30): move_ptz_absolute(pan)``
to rotate the camera 10 times.
"""
(pan, tilt) = self._apply_flip_and_mirror_absolute(pan, tilt)
return self._api.move_ptz_absolute(pan=pan, tilt=tilt)[slice(0, 2)]
def adjust_pan_tilt(self, pan, tilt):
"""Command the PTZ unit with a relative pan and tilt taking into account flips and mirroring.
:param pan: The pan change. In None or 0, pan remains unchanged.
The value is automatically normalized to <-360,+360>. May be negative.
:type pan: float
:param tilt: The tilt change. In None or 0, tilt remains unchanged.
The value is automatically normalized to <-360,+360>. May be negative.
:type tilt: float
:return: The pan and tilt change values that were really applied (e.g. the cropped and normalized input)
:rtype: tuple
:raises: :py:class:`RuntimeError` if the camera doesn't have capabilities to execute the given command.
:raises: :py:class:`IOError`, :py:class:`urllib2.URLError` on network communication error
.. note:: This call doesn't wait for the command to finish execution (which might take a while if e.g. a move
by 300 deg pan is requested).
"""
(pan, tilt) = self._apply_flip_and_mirror_relative(pan, tilt)
return self._api.move_ptz_relative(pan=pan, tilt=tilt)[slice(0, 2)]
def set_pan_tilt_velocity(self, pan, tilt):
"""Command the PTZ unit velocity in terms of pan, tilt and zoom taking into account flips and mirroring.
:param pan: Pan speed. In None or 0, pan remains unchanged. Pan speed is aperiodic (can be higher than 360).
May be negative.
:type pan: int
:param tilt: Tilt speed. In None or 0, tilt remains unchanged. Tilt speed is aperiodic (can be higher than 360).
May be negative.
:type tilt: int
:return: The pan and tilt velocity values that were really applied (e.g. the cropped and normalized input)
:rtype: tuple
:raises: :py:class:`RuntimeError` if the camera doesn't have capabilities to execute the given command.
:raises: :py:class:`IOError`, :py:class:`urllib2.URLError` on network communication error
.. note:: This call doesn't wait for the command to finish execution.
.. note:: It is not clearly stated in the API, but practically any following absolute/relative pose command
sets velocity to zero.
"""
(pan, tilt) = self._apply_flip_and_mirror_velocity(pan, tilt)
return self._api.set_ptz_velocity(pan=pan, tilt=tilt)[slice(0, 2)]
def set_pan(self, pan):
"""Command an absolute pan taking into account flips and mirroring.
:param pan: The desired pan. The value is automatically normalized to <-180,+180>
:type pan: float
:return: The pan value that was really applied (e.g. the cropped and normalized input)
:rtype: float
:raises: :py:class:`RuntimeError` if the camera doesn't have capabilities to execute the given command.
:raises: :py:class:`IOError`, :py:class:`urllib2.URLError` on network communication error
.. note:: This call doesn't wait for the command to finish execution (which might take a while if e.g. a move
from -170 to +170 pan is requested).
.. note:: Since the pan and tilt values are periodic and normalization takes place in this function, you can
simply call this command in loops like e.g. ``for pan in range(0,3600,30): move_ptz_absolute(pan)``
to rotate the camera 10 times.
"""
(pan, tilt) = self._apply_flip_and_mirror_absolute(pan, 0)
return self._api.move_ptz_absolute(pan=pan)[0]
def set_tilt(self, tilt):
"""Command an absolute tilt taking into account flips and mirroring.
:param tilt: The desired tilt. The value is automatically normalized to <-180,+180>
:type tilt: float
:return: The tilt value that was really applied (e.g. the cropped and normalized input)
:rtype: float
:raises: :py:class:`RuntimeError` if the camera doesn't have capabilities to execute the given command.
:raises: :py:class:`IOError`, :py:class:`urllib2.URLError` on network communication error
.. note:: This call doesn't wait for the command to finish execution (which might take a while if e.g. a move
from -170 to +170 pan is requested).
.. note:: Since the pan and tilt values are periodic and normalization takes place in this function, you can
simply call this command in loops like e.g. ``for pan in range(0,3600,30): move_ptz_absolute(pan)``
to rotate the camera 10 times.
"""
(pan, tilt) = self._apply_flip_and_mirror_absolute(0, tilt)
return self._api.move_ptz_absolute(tilt=tilt)[1]
def set_zoom(self, zoom):
"""Command an absolute zoom.
:param zoom: The desired zoom level.
:type zoom: int
:return: The zoom value that was really applied (e.g. the cropped and normalized input)
:rtype: int
:raises: :py:class:`RuntimeError` if the camera doesn't have capabilities to execute the given command.
:raises: :py:class:`IOError`, :py:class:`urllib2.URLError` on network communication error
.. note:: This call doesn't wait for the command to finish execution (which might take a while if e.g. a move
from -170 to +170 pan is requested).
.. note:: Since the pan and tilt values are periodic and normalization takes place in this function, you can
simply call this command in loops like e.g. ``for pan in range(0,3600,30): move_ptz_absolute(pan)``
to rotate the camera 10 times.
"""
return self._api.move_ptz_absolute(zoom=zoom)[2]
def adjust_pan(self, pan):
"""Command a relative pan taking into account flips and mirroring.
:param pan: The pan change. The value is automatically normalized to <-360,+360>. May be negative.
:type pan: float
:return: The pan change value that was really applied (e.g. the cropped and normalized input)
:rtype: float
:raises: :py:class:`RuntimeError` if the camera doesn't have capabilities to execute the given command.
:raises: :py:class:`IOError`, :py:class:`urllib2.URLError` on network communication error
.. note:: This call doesn't wait for the command to finish execution (which might take a while if e.g. a move
by 300 deg pan is requested).
"""
(pan, _) = self._apply_flip_and_mirror_relative(pan, 0)
return self._api.move_ptz_relative(pan=pan)[0]
def adjust_tilt(self, tilt):
"""Command a relative tilt taking into account flips and mirroring.
:param tilt: The tilt change. The value is automatically normalized to <-360,+360>. May be negative.
:type tilt: float
:return: The tilt change value that was really applied (e.g. the cropped and normalized input)
:rtype: float
:raises: :py:class:`RuntimeError` if the camera doesn't have capabilities to execute the given command.
:raises: :py:class:`IOError`, :py:class:`urllib2.URLError` on network communication error
.. note:: This call doesn't wait for the command to finish execution (which might take a while if e.g. a move
by 300 deg pan is requested).
"""
(_, tilt) = self._apply_flip_and_mirror_relative(0, tilt)
return self._api.move_ptz_relative(tilt=tilt)[1]
def adjust_zoom(self, zoom):
"""Command a relative zoom.
:param zoom: The zoom change. In None or 0, zoom remains unchanged. May be negative.
:type zoom: int
:return: The zoom change value that was really applied (e.g. the cropped and normalized input)
:rtype: int
:raises: :py:class:`RuntimeError` if the camera doesn't have capabilities to execute the given command.
:raises: :py:class:`IOError`, :py:class:`urllib2.URLError` on network communication error
.. note:: This call doesn't wait for the command to finish execution (which might take a while if e.g. a move
by 300 deg pan is requested).
"""
return self._api.move_ptz_relative(zoom=zoom)[2]
def set_pan_velocity(self, pan):
"""Command pan velocity.
:param pan: Pan speed. Pan speed is aperiodic (can be higher than 360). May be negative.
:type pan: int
:return: The pan velocity that was really applied (e.g. the cropped and normalized input)
:rtype: int
:raises: :py:class:`RuntimeError` if the camera doesn't have capabilities to execute the given command.
:raises: :py:class:`IOError`, :py:class:`urllib2.URLError` on network communication error
.. note:: This call doesn't wait for the command to finish execution.
.. note:: It is not clearly stated in the API, but practically any following absolute/relative pose command
sets velocity to zero.
"""
(pan, _) = self._apply_flip_and_mirror_velocity(pan, 0)
return self._api.set_ptz_velocity(pan=pan)[0]
def set_tilt_velocity(self, tilt):
"""Command tilt velocity.
:param tilt: Tilt speed. Tilt speed is aperiodic (can be higher than 360). May be negative.
:type tilt: int
:return: The tilt velocity that was really applied (e.g. the cropped and normalized input)
:rtype: int
:raises: :py:class:`RuntimeError` if the camera doesn't have capabilities to execute the given command.
:raises: :py:class:`IOError`, :py:class:`urllib2.URLError` on network communication error
.. note:: This call doesn't wait for the command to finish execution.
.. note:: It is not clearly stated in the API, but practically any following absolute/relative pose command
sets velocity to zero.
"""
(_, tilt) = self._apply_flip_and_mirror_velocity(0, tilt)
return self._api.set_ptz_velocity(tilt=tilt)[1]
def set_zoom_velocity(self, zoom):
"""Command zoom velocity.
:param zoom: Zoom speed. May be negative.
:type zoom: int
:return: The zoom velocity that was really applied (e.g. the cropped and normalized input)
:rtype: int
:raises: :py:class:`RuntimeError` if the camera doesn't have capabilities to execute the given command.
:raises: :py:class:`IOError`, :py:class:`urllib2.URLError` on network communication error
.. note:: This call doesn't wait for the command to finish execution.
.. note:: It is not clearly stated in the API, but practically any following absolute/relative pose command
sets velocity to zero.
"""
return self._api.set_ptz_velocity(zoom=zoom)[2]
def look_at(self, x, y, image_width, image_height):
"""Point the camera center to a point with the given coordinates in the camera image.
:param x: X coordinate of the look-at point.
:type x: int
:param y: X coordinate of the look-at point.
:type y: int
:param image_width: Width of the image in pixels.
:type image_width: int
:param image_height: Height of the image in pixels.
:type image_height: int
:raises: :py:class:`RuntimeError` if the camera doesn't have capabilities to execute the given command.
:raises: :py:class:`IOError`, :py:class:`urllib2.URLError` on network communication error
"""
self._api.look_at(x, y, image_width, image_height)
def set_autofocus(self, use):
"""Command the camera to use/stop using autofocus.
:param use: True: use autofocus; False: do not use it.
:type use: bool
:return: use
:rtype: :py:class:`bool`
:raises: :py:class:`RuntimeError` if the camera doesn't have capabilities to execute the given command.
:raises: :py:class:`IOError`, :py:class:`urllib2.URLError` on network communication error
"""
self._api.use_autofocus(use)
self._autofocus = use
self._send_parameter_update("autofocus", use)
return use
def set_focus(self, focus, set_also_autofocus=True):
"""Set focus to the desired value (implies turning off autofocus).
:param focus: The desired focus value.
:type focus: int
:param set_also_autofocus: If True and autofocus is on, turn it off first.
:type set_also_autofocus: bool
:return: The focus value that was really applied (e.g. the cropped and normalized input)
:rtype: int
:raises: :py:class:`RuntimeError` if the camera doesn't have capabilities to execute the given command.
:raises: :py:class:`IOError`, :py:class:`urllib2.URLError` on network communication error
"""
focus = self._api.set_focus(focus)
self._focus = focus
self._send_parameter_update("focus", focus)
if set_also_autofocus:
self._autofocus = False
self._send_parameter_update("autofocus", False)
return focus
def adjust_focus(self, amount, set_also_autofocus=True):
"""Add the desired amount to the focus value (implies turning off autofocus).
:param amount: The desired focus change amount.
:type amount: int
:param set_also_autofocus: If True and autofocus is on, turn it off first.
:type set_also_autofocus: bool
:return: The focus change amount that was really applied (e.g. the cropped and normalized input)
:rtype: int
:raises: :py:class:`RuntimeError` if the camera doesn't have capabilities to execute the given command.
:raises: :py:class:`IOError`, :py:class:`urllib2.URLError` on network communication error
"""
amount = self._api.adjust_focus(amount)
self._focus += amount
self._send_parameter_update("focus", self._focus)
if set_also_autofocus:
self._autofocus = False
self._send_parameter_update("autofocus", False)
return amount
def set_focus_velocity(self, velocity):
"""Set the focus "speed" (implies turning off autofocus).
:param velocity: The desired focus velocity.
:type velocity: int
:return: The focus velocity that was really applied (e.g. the cropped and normalized input)
:rtype: int
:raises: :py:class:`RuntimeError` if the camera doesn't have capabilities to execute the given command.
:raises: :py:class:`IOError`, :py:class:`urllib2.URLError` on network communication error
"""
return self._api.set_focus_velocity(velocity)
# TODO self.focus updating
def set_autoiris(self, use):
"""Command the camera to use/stop using auto iris adjustment.
:param use: True: use auto iris adjustment; False: do not use it.
:type use: bool
:return: use
:rtype: :py:class:`bool`
:raises: :py:class:`RuntimeError` if the camera doesn't have capabilities to execute the given command.
:raises: :py:class:`IOError`, :py:class:`urllib2.URLError` on network communication error
"""
self._api.use_autoiris(use)
self._autoiris = use
self._send_parameter_update("autoiris", use)
return use
def set_iris(self, iris, set_also_autoiris=True):
"""Set iris to the desired value (implies turning off autoiris).
:param iris: The desired iris value.
:type iris: int
:param set_also_autoiris: If True and autoiris is on, turn it off first.
:type set_also_autoiris: bool
:return: The iris value that was really applied (e.g. the cropped and normalized input)
:rtype: int
:raises: :py:class:`RuntimeError` if the camera doesn't have capabilities to execute the given command.
:raises: :py:class:`IOError`, :py:class:`urllib2.URLError` on network communication error
"""
iris = self._api.set_iris(iris)
self._iris = iris
self._send_parameter_update("iris", iris)
if set_also_autoiris:
self._autoiris = False
self._send_parameter_update("autoiris", False)
return iris
def adjust_iris(self, amount, set_also_autoiris=True):
"""Add the desired amount to the iris value (implies turning off autoiris).
:param amount: The desired iris change amount.
:type amount: int
:param set_also_autoiris: If True and autoiris is on, turn it off first.
:type set_also_autoiris: bool
:return: The iris change amount that was really applied (e.g. the cropped and normalized input)
:rtype: int
:raises: :py:class:`RuntimeError` if the camera doesn't have capabilities to execute the given command.
:raises: :py:class:`IOError`, :py:class:`urllib2.URLError` on network communication error
"""
amount = self._api.adjust_iris(amount)
self._iris += amount
self._send_parameter_update("iris", self._iris)
if set_also_autoiris:
self._autoiris = False
self._send_parameter_update("autoiris", False)
return amount
def set_iris_velocity(self, velocity):
"""Set the iris "speed" (implies turning off autoiris).
:param velocity: The desired iris velocity.
:type velocity: int
:return: The iris velocity that was really applied (e.g. the cropped and normalized input)
:rtype: int
:raises: :py:class:`RuntimeError` if the camera doesn't have capabilities to execute the given command.
:raises: :py:class:`IOError`, :py:class:`urllib2.URLError` on network communication error
"""
return self._api.set_iris_velocity(velocity)
# TODO self.iris updating
def set_brightness(self, brightness):
"""Set brightness to the desired value.
:param brightness: The desired brightness value.
:type brightness: int
:return: The brightness value that was really applied (e.g. the cropped and normalized input)
:rtype: int
:raises: :py:class:`RuntimeError` if the camera doesn't have capabilities to execute the given command.
:raises: :py:class:`IOError`, :py:class:`urllib2.URLError` on network communication error
.. note:: The brightness setting has no effect on Axis 214 PTZ.
"""
brightness = self._api.set_brightness(brightness)
self._brightness = brightness
self._send_parameter_update("brightness", brightness)
return brightness
def adjust_brightness(self, amount):
"""Add the desired amount to the brightness value.
:param amount: The desired brightness change amount.
:type amount: int
:return: The brightness change amount that was really applied (e.g. the cropped and normalized input)
:rtype: int
:raises: :py:class:`RuntimeError` if the camera doesn't have capabilities to execute the given command.
:raises: :py:class:`IOError`, :py:class:`urllib2.URLError` on network communication error
.. note:: The brightness setting has no effect on Axis 214 PTZ.
"""
amount = self._api.adjust_brightness(amount)
self._brightness += amount
self._send_parameter_update("brightness", self._brightness)
return amount
def set_brightness_velocity(self, velocity):
"""Set the brightness "speed".
:param velocity: The desired brightness velocity.
:type velocity: int
:return: The brightness velocity that was really applied (e.g. the cropped and normalized input)
:rtype: :py:class:`int`
:raises: :py:class:`RuntimeError` if the camera doesn't have capabilities to execute the given command.
:raises: :py:class:`IOError`, :py:class:`urllib2.URLError` on network communication error
.. note:: The brightness setting has no effect on Axis 214 PTZ.
"""
return self._api.set_brightness_velocity(velocity)
# TODO self.brightness updating
def use_backlight_compensation(self, use):
"""Command the camera to use/stop using backlight compensation (requires autoiris=on set before).
:param use: True: use backlight compensation; False: do not use it.
:type use: bool
:return: use
:rtype: :py:class:`bool`
:raises: :py:class:`RuntimeError` if the camera doesn't have capabilities to execute the given command.
:raises: :py:class:`IOError`, :py:class:`urllib2.URLError` on network communication error
"""
if self._autoiris: # the compensation needs autoiris to be active
self._api.use_backlight_compensation(use)
self._backlight = use
else:
use = False
self._send_parameter_update("backlight", use)
return use
def set_ir_cut_filter_auto(self, use):
"""Command the camera to use auto infrared filter (requires autoiris=on set before).
:param use: True: use automatic infrared filter;
False: use always.
:type use: bool
:return: True if the filter is always used, None if it is set to auto.
:rtype: :py:class:`bool`
:raises: :py:class:`RuntimeError` if the camera doesn't have capabilities to execute the given command.
:raises: :py:class:`IOError`, :py:class:`urllib2.URLError` on network communication error
"""
use = (None if use else True)
# auto IR filter requires autoiris to be active
if self._autoiris or use is not None:
self._api.use_ir_cut_filter(use)
self._ir_cut_filter = use
else:
use = True
self._send_parameter_update("ircutfilter", "auto" if (use is None) else "on")
return use
def set_ir_cut_filter_use(self, use):
"""Command the camera to use/stop using infrared filter.
:param use: Whether to use the filter.
:type use: bool
:return: use
:rtype: :py:class:`bool`
:raises: :py:class:`RuntimeError` if the camera doesn't have capabilities to execute the given command.
:raises: :py:class:`IOError`, :py:class:`urllib2.URLError` on network communication error
"""
self._api.use_ir_cut_filter(use)
self._ir_cut_filter = use
self._send_parameter_update("ircutfilter", "on" if use else "off")
return use
def _reconfigure(self, config, level, subname):
"""Dynamic reconfigure callback.
:param config: The config to be used.
:type config: dict
:return: The config with really used values.
:rtype: dict
"""
if self._executing_reconfigure or self._executing_parameter_update or self._axis._executing_reconfigure:
return config
with self._reconfigure_mutex:
self._executing_reconfigure = True
try:
if config.autofocus != self._autofocus:
if self._api.has_capability('AutoFocus'):
self.set_autofocus(config.autofocus)
else:
rospy.loginfo("Camera on host %s doesn't support autofocus." % self._api.hostname)
except (IOError, ValueError, RuntimeError) as e:
rospy.logwarn("Could not apply dynamic reconfigure for autofocus. Cause: %r" % e)
try:
if config.focus != self._focus:
if self._api.has_capability('AbsoluteFocus'):
self.set_focus(config.focus)
else:
rospy.loginfo("Camera on host %s doesn't support absolute focus control." % self._api.hostname)
except (IOError, ValueError, RuntimeError) as e:
rospy.logwarn("Could not apply dynamic reconfigure for focus. Cause: %r" % e)
try:
if config.autoiris != self._autoiris:
if self._api.has_capability('AutoIris'):
self.set_autoiris(config.autoiris)
else:
rospy.loginfo("Camera on host %s doesn't support autoiris." % self._api.hostname)
except (IOError, ValueError, RuntimeError) as e:
rospy.logwarn("Could not apply dynamic reconfigure for autoiris. Cause: %r" % e)
try:
if config.iris != self._iris:
if self._api.has_capability('AbsoluteIris'):
self.set_iris(config.iris)
else:
rospy.loginfo("Camera on host %s doesn't support absolute iris control." % self._api.hostname)
except (IOError, ValueError, RuntimeError) as e:
rospy.logwarn("Could not apply dynamic reconfigure for iris. Cause: %r" % e)
try:
if config.brightness != self._brightness:
# there is no capability for brightness
self.set_brightness(config.brightness)
except (IOError, ValueError, RuntimeError) as e:
rospy.logwarn("Could not apply dynamic reconfigure for brightness. Cause: %r" % e)
try:
if config.backlight != self._backlight:
if self._api.has_capability('BackLight'):
self.use_backlight_compensation(config.backlight)
else:
rospy.loginfo("Camera on host %s doesn't support backlight compensation." % self._api.hostname)
except (IOError, ValueError, RuntimeError) as e:
rospy.logwarn("Could not apply dynamic reconfigure for backlight compenstaion. Cause: %r" % e)
try:
ir = None if config.ircutfilter == "auto" else (config.ircutfilter == "on")
if ir != self._ir_cut_filter:
if self._api.has_capability('IrCutFilter'):
if ir is None or self._ir_cut_filter is None:
self.set_ir_cut_filter_auto(ir is None)
if ir is not None:
self.set_ir_cut_filter_use(ir)
else:
rospy.loginfo("Camera on host %s doesn't support IR cut filter control." % self._api.hostname)
except (IOError, ValueError, RuntimeError) as e:
rospy.logwarn("Could not apply dynamic reconfigure for IR cut filter. Cause: %r" % e)
config.autofocus = self._autofocus
config.focus = self._focus
config.autoiris = self._autoiris
config.iris = self._iris
config.brightness = self._brightness
config.backlight = self._backlight
config.ircutfilter = ("auto" if self._ir_cut_filter is None else ("on" if self._ir_cut_filter else "off"))
self._executing_reconfigure = False
return config
# Helper functions
def _send_parameter_update(self, parameter, value):
"""Notify the parameter change via dynamic reconfigure.
:param parameter: The parameter that has changed.
:type parameter: basestring
:param value: New value of the parameter.
:type value: Any
"""
if self._executing_parameter_update:
return
with self._parameter_update_mutex:
update = dict()
update[parameter] = value
self._executing_parameter_update = True
if hasattr(self._axis, 'srv') and self._axis.srv is not None and not self._axis._executing_reconfigure:
# axis.srv is instantiated after camera controller
self._axis.srv.update_configuration(update)
if hasattr(self, '_dynamic_reconfigure_server') and self._dynamic_reconfigure_server is not None \
and not self._executing_reconfigure:
# this server can also be initialized later
self._dynamic_reconfigure_server.update_configuration(update)
self._executing_parameter_update = False
def _apply_flip_and_mirror_absolute(self, pan, tilt):
"""Apply flipping and mirroring to absolute pan and tilt command.
:param pan: The input pan.
:type pan: float
:param tilt: The input tilt.
:type tilt: float
:return: The corrected pan and tilt.
:rtype: tuple
"""
if self._flip_vertically:
tilt = -tilt
if self._flip_horizontally:
pan = 180 - pan
if self._mirror_horizontally:
pan = -pan
return pan, tilt
def _apply_flip_and_mirror_relative(self, pan, tilt):
"""Apply flipping and mirroring to relative pan and tilt command.
:param pan: The input pan.
:type pan: float
:param tilt: The input tilt.
:type tilt: float
:return: The corrected pan and tilt.
:rtype: tuple
"""
if self._flip_vertically:
tilt = -tilt
if self._flip_horizontally:
pan = -pan
if self._mirror_horizontally:
pan = -pan
return pan, tilt
def _apply_flip_and_mirror_velocity(self, pan, tilt):
"""Apply flipping and mirroring to velocity pan and tilt command.
:param pan: The input pan.
:type pan: float
:param tilt: The input tilt.
:type tilt: float
:return: The corrected pan and tilt.
:rtype: tuple
"""
if self._flip_vertically:
tilt = -tilt
if self._flip_horizontally:
pan = -pan
if self._mirror_horizontally:
pan = -pan
return pan, tilt
def _record_diagnostics(self, func):
"""Wrap a function call and record diagnostics about if it has thrown an exception or not.
:param func: The function to be wrapped.
:type func: function
:return: The wrapped function.
:rtype: function
"""
def new_func(msg):
try:
func(msg)
self._last_command_error = None
except Exception, e:
self._last_command_error = e
raise e
return new_func
