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, node: Node, updater: Updater):
"""Initializes an gait diagnostic which analyzes gait and subgait states.
:type updater: diagnostic_updater.Updater
"""
self._goal_sub = node.create_subscription(
topic="/march/gait_selection/current_gait",
msg_type=CurrentGait,
callback=self._cb_goal,
qos_profile=10,
)
self._gait_msg = None
updater.add("Gait", self._diagnostics)
def __init__(self, node: Node, updater: Updater, joint_names: List[str]):
"""Initialize an MotorController diagnostic which analyzes MotorController states.
:type updater: diagnostic_updater.Updater
"""
self.node = node
self._sub = node.create_subscription(
msg_type=MotorControllerState,
topic="/march/motor_controller_states",
callback=self._cb,
qos_profile=10,
)
self._motor_controller_state = None
for i, joint_name in enumerate(joint_names):
updater.add(f"MotorController {joint_name}", self._diagnostic(i))
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 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 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 __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 __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 __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, 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
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)
class Axis(rospy.SubscribeListener):
"""The ROS-VAPIX interface for video streaming."""
def __init__(self, hostname, username, password, width, height, frame_id, camera_info_url, use_encrypted_password,
camera_id=1, auto_wakeup_camera=True, compression=0, fps=24, use_color=True,
use_square_pixels=False):
"""Create the ROS-VAPIX interface.
:param hostname: Hostname of the camera (without http://, can be an IP address).
:type hostname: basestring
:param username: If login is needed, provide a username here.
:type username: :py:obj:`basestring` | None
:param password: If login is needed, provide a password here.
:type password: :py:obj:`basestring` | None
:param width: Width of the requested video stream in pixels (can be changed later). Must be one of the supported
resolutions. If `None`, the resolution will be chosen by height only. If also `height` is `None`,
then the default camera resolution will be used.
:type width: int|None
:param height: Height of the requested video stream in pixels (can be changed later). Must be one of the
supported resolutions. If `None`, the resolution will be chosen by width only. If also `width` is
`None`, then the default camera resolution will be used.
:type height: int|None
:param frame_id: The ROS TF frame assigned to the camera.
:type frame_id: basestring
:param camera_info_url: The URL pointing to the camera calaibration, if available.
:type camera_info_url: basestring
:param use_encrypted_password: Whether to use Plain HTTP Auth (False) or Digest HTTP Auth (True).
:type use_encrypted_password: bool
:param camera_id: ID (number) of the camera. Can be 1 to 4.
:type camera_id: int
:param auto_wakeup_camera: If True, there will be a wakeup trial after first unsuccessful network command.
:type auto_wakeup_camera: bool
:param compression: Compression of the image (0 - no compression, 100 - max compression).
:type compression: int
:param fps: The desired frames per second.
:type fps: int
:param use_color: If True, send a color stream, otherwise send only grayscale image.
:type use_color: bool
:param use_square_pixels: If True, the resolution will be stretched to match 1:1 pixels.
By default, the pixels have a ratio of 11:12.
:type use_square_pixels: bool
:raises: :py:exc:`ValueError` if the requested resolution (either the `resolution`, or `width`+`height`
is not supported.
"""
# True every time the video parameters have changed and the URL has to be altered (set from other threads).
self.video_params_changed = False
self.__initializing = True
self._hostname = hostname
self._camera_id = camera_id
self.diagnostic_updater = Updater()
self.diagnostic_updater.setHardwareID(hostname)
self._api = None
# autodetect the VAPIX API and connect to it; try it forever
while self._api is None and not rospy.is_shutdown():
try:
self._api = VAPIX.get_api_for_camera(hostname, username, password, camera_id, use_encrypted_password)
except (IOError, ValueError):
rospy.loginfo("Retrying connection to VAPIX on host %s, camera %d in 2 seconds." %
(hostname, camera_id))
rospy.sleep(2)
if rospy.is_shutdown():
return
self._allowed_resolutions = self._get_allowed_resolutions()
rospy.loginfo("The following resolutions are available for camera %d:\n%s" %
(camera_id, "\n".join([str(res) for res in self._allowed_resolutions])))
rospy.set_param("~allowed_resolutions", [res.get_vapix_representation() for res in self._allowed_resolutions])
# Sometimes the camera falls into power saving mode and stops streaming.
# This setting allows the script to try to wake up the camera.
self._auto_wakeup_camera = auto_wakeup_camera
# dynamic-reconfigurable properties - definitions
self._width = None # deprecated
self._height = None # deprecated
self._resolution = None
self._compression = None
self._fps = None
self._use_color = None
self._use_square_pixels = None
# treat empty strings as None in width and height params
width = width if width != "" else None
height = height if height != "" else None
# dynamic-reconfigurable properties - defaults
if width is None and height is None:
# TODO change to perform default resolution detection from VAPIX
self.set_resolution(self._allowed_resolutions[0])
else:
resolution = self.find_resolution_by_size(width, height)
self.set_resolution(resolution.get_vapix_representation())
self.set_compression(compression)
self.set_fps(fps)
self.set_use_color(use_color)
self.set_use_square_pixels(use_square_pixels)
# only advertise the supported resolutions on dynamic reconfigure
change_enum_items(
VideoStreamConfig,
"resolution",
[{
'name': res.name if isinstance(res, CIFVideoResolution) else str(res),
'value': res.get_vapix_representation(),
'description': str(res)
} for res in self._allowed_resolutions],
self._resolution.get_vapix_representation()
)
# dynamic reconfigure server
self._video_stream_param_change_server = dynamic_reconfigure.server.Server(VideoStreamConfig,
self.reconfigure_video)
# camera info setup
self._frame_id = frame_id
self._camera_info_url = camera_info_url
# generate a valid camera name based on the hostname
self._camera_name = camera_info_manager.genCameraName(self._hostname)
self._camera_info = camera_info_manager.CameraInfoManager(cname=self._camera_name, url=self._camera_info_url)
self._camera_info.loadCameraInfo() # required before getCameraInfo()
# the thread used for streaming images (is instantiated when the first image subscriber subscribes)
self._streaming_thread = None
# the publishers are started/stopped lazily in peer_subscribe/peer_unsubscribe
self._video_publisher_frequency_diagnostic = FrequencyStatusParam({'min': self._fps, 'max': self._fps})
self._video_publisher = PausableDiagnosedPublisher(
self,
rospy.Publisher("image_raw/compressed", CompressedImage, self, queue_size=100),
self.diagnostic_updater, self._video_publisher_frequency_diagnostic, TimeStampStatusParam()
)
self._camera_info_publisher = PausableDiagnosedPublisher(
self,
rospy.Publisher("camera_info", CameraInfo, self, queue_size=100),
self.diagnostic_updater, self._video_publisher_frequency_diagnostic, TimeStampStatusParam()
)
self._snapshot_server = rospy.Service("take_snapshot", TakeSnapshot, self.take_snapshot)
self.diagnostic_updater.add(FunctionDiagnosticTask("Camera parameters", self._camera_diagnostic_callback))
# BACKWARDS COMPATIBILITY LAYER
self.username = username # deprecated
self.password = password # deprecated
self.use_encrypted_password = use_encrypted_password # deprecated
self.st = None # deprecated
self.pub = self._video_publisher # deprecated
self.caminfo_pub = self._camera_info_publisher # deprecated
self.__initializing = False
def __str__(self):
(width, height) = self._resolution.get_resolution(self._use_square_pixels)
return 'Axis driver on host %s, camera %d (%dx%d px @ %d FPS)' % \
(self._hostname, self._api.camera_id, width, height, self._fps)
def peer_subscribe(self, topic_name, topic_publish, peer_publish):
"""Lazy-start the image-publisher."""
if self._streaming_thread is None:
self._streaming_thread = ImageStreamingThread(self)
self._streaming_thread.start()
else:
self._streaming_thread.resume()
def peer_unsubscribe(self, topic_name, num_peers):
"""Lazy-stop the image-publisher when nobody is interested"""
if num_peers == 0:
self._streaming_thread.pause()
def take_snapshot(self, request):
"""Retrieve a snapshot from the camera.
:param request: The service request.
:type request: :py:class:`axis_camera.srv.TakeSnapshotRequest`
:return: The response containing the image.
:rtype: :py:class:`axis_camera.srv.TakeSnapshotResponse`
:raises: :py:exc:`IOError`, :py:exc:`urllib2.URLError`
"""
image_data = self._api.take_snapshot()
image = CompressedImage()
image.header.stamp = rospy.Time.now()
image.header.frame_id = self._frame_id
image.format = "jpeg"
image.data = image_data
response = TakeSnapshotResponse()
response.image = image
return response
def reconfigure_video(self, config, level):
"""Dynamic reconfigure callback for video parameters.
:param config: The requested configuration.
:type config: dict
:param level: Unused here.
:type level: int
:return: The config corresponding to what was really achieved.
:rtype: dict
"""
if self.__initializing:
# in the initialization phase, we want to give precedence to the values given to the constructor
config.compression = self._compression
config.fps = self._fps
config.use_color = self._use_color
config.use_square_pixels = self._use_square_pixels
config.resolution = self._resolution.get_vapix_representation()
else:
self.__try_set_value_from_config(config, 'compression', self.set_compression)
self.__try_set_value_from_config(config, 'fps', self.set_fps)
self.__try_set_value_from_config(config, 'use_color', self.set_use_color)
self.__try_set_value_from_config(config, 'use_square_pixels', self.set_use_square_pixels)
try:
self.set_resolution(config.resolution)
except ValueError:
config.resolution = self._resolution.get_vapix_representation()
return config
def __try_set_value_from_config(self, config, field, setter):
"""First, try to call `setter(config[field])`, and if this call doesn't succeed. set the field in config to
its value stored in this class.
:param config: The dynamic reconfigure config dictionary.
:type config: dict
:param field: The field name (both in :py:obj:`config` and in :py:obj:`self`).
:type field: basestring
:param setter: The setter to use to set the value.
:type setter: lambda function
"""
try:
setter(config[field])
except ValueError:
config[field] = getattr(self, field)
#################################
# DYNAMIC RECONFIGURE CALLBACKS #
#################################
def set_resolution(self, resolution_value):
"""Request a new resolution for the video stream.
:param resolution_value: The string of type `width`x`height` or a :py:class:`VideoResolution` object.
:type resolution_value: basestring|VideoResolution
:raises: :py:exc:`ValueError` if the resolution is unknown/unsupported.
"""
resolution = None
if isinstance(resolution_value, VideoResolution):
resolution = resolution_value
elif isinstance(resolution_value, basestring):
resolution = self._get_resolution_from_param_value(resolution_value)
if resolution is None:
raise ValueError("Unsupported resolution type specified: %r" % resolution_value)
if self._resolution is None or resolution != self._resolution:
self._resolution = resolution
self.video_params_changed = True
# deprecated values
self._width = resolution.get_resolution(self._use_square_pixels)[0]
self._height = resolution.get_resolution(self._use_square_pixels)[1]
def _get_resolution_from_param_value(self, value):
"""Return a :py:class:`VideoResolution` object corresponding to the given video resolution param string.
:param value: Value of the resolution parameter to parse (of form `width`x`height`).
:type value: basestring
:return: The :py:class:`VideoResolution` corresponding to the given resolution param string.
:rtype: :py:class:`VideoResolution`
:raises: :py:exc:`ValueError` if the resolution is unknown/unsupported.
"""
for resolution in self._allowed_resolutions:
if resolution.get_vapix_representation() == value:
return resolution
raise ValueError("%s is not a valid valid resolution." % value)
def find_resolution_by_size(self, width, height):
"""Return a :py:class:`VideoResolution` object with the given dimensions.
If there are more resolutions with the same size, any of them may be returned.
:param width: Image width in pixels. If `None`, resolutions will be matched only by height.
:type width: int|None
:param height: Image height in pixels. If `None`, resolutions will be matched only by width.
:type height: int|None
:return: The corresponding resolution object.
:rtype: :py:class:`VideoResolution`
:raises: :py:exc:`ValueError` if no resolution with the given dimensions can be found.
:raises: :py:exc:`ValueError` if both `width` and `height` are None.
"""
if width is None and height is None:
raise ValueError("Either width or height of the desired resolution must be specified.")
for resolution in self._allowed_resolutions:
size = resolution.get_resolution(use_square_pixels=False)
if (width is None or width == size[0]) and (height is None or height == size[1]):
return resolution
size = resolution.get_resolution(use_square_pixels=True)
if (width is None or width == size[0]) and (height is None or height == size[1]):
return resolution
raise ValueError("Cannot find a supported resolution with dimensions %sx%s" % (width, height))
def _get_allowed_resolutions(self):
"""Return a list of resolutions supported both by the camera.
:return: The supported resolutions list.
:rtype: list of :py:class:`VideoResolution`
"""
camera_resolutions = self._get_resolutions_supported_by_camera()
return camera_resolutions
def _get_resolutions_supported_by_camera(self):
"""Return a list of resolutions supported the camera.
:return: The supported resolutions list.
:rtype: list of :py:class:`VideoResolution`
"""
try:
names = self._api.parse_list_parameter_value(self._api.get_parameter("Properties.Image.Resolution"))
return [VideoResolution.parse_from_vapix_param_value(name, self._api) for name in names]
except (IOError, ValueError):
rospy.logwarn("Could not determine resolutions supported by the camera. Asssuming only CIF.")
return [CIFVideoResolution("CIF", 384, 288)]
def set_compression(self, compression):
"""Request the given compression level for the video stream.
:param compression: Compression of the image (0 - no compression, 100 - max compression).
:type compression: int
:raises: :py:exc:`ValueError` if the given compression level is outside the allowed range.
"""
if compression != self._compression:
self._compression = self.sanitize_compression(compression)
self.video_params_changed = True
@staticmethod
def sanitize_compression(compression):
"""Make sure the given value can be used as a compression level of the video stream.
:param compression: Compression of the image (0 - no compression, 100 - max compression).
:type compression: int
:return: The given compression converted to an int.
:rtype: int
:raises: :py:exc:`ValueError` if the given compression level is outside the allowed range.
"""
compression = int(compression)
if not (0 <= compression <= 100):
raise ValueError("%s is not a valid value for compression." % str(compression))
return compression
def set_fps(self, fps):
"""Request the given compression level for the video stream.
:param fps: The desired frames per second.
:type fps: int
:raises: :py:exc:`ValueError` if the given FPS is outside the allowed range.
"""
if fps != self._fps:
self._fps = self.sanitize_fps(fps)
self.video_params_changed = True
if hasattr(self, "_video_publisher_frequency_diagnostic"):
self._video_publisher_frequency_diagnostic.freq_bound['min'] = self._fps
self._video_publisher_frequency_diagnostic.freq_bound['max'] = self._fps
@staticmethod
def sanitize_fps(fps):
"""Make sure the given value can be used as FPS of the video stream.
:param fps: The desired frames per second.
:type fps: int
:return: The given FPS converted to an int.
:rtype: int
:raises: :py:exc:`ValueError` if the given FPS is outside the allowed range.
"""
fps = int(fps)
if not (1 <= fps <= 30):
raise ValueError("%s is not a valid value for FPS." % str(fps))
return fps
def set_use_color(self, use_color):
"""Request using/not using color in the video stream.
:param use_color: If True, send a color stream, otherwise send only grayscale image.
:type use_color: bool
:raises: :py:exc:`ValueError` if the given argument is not a bool.
"""
if use_color != self._use_color:
self._use_color = self.sanitize_bool(use_color, "use_color")
self.video_params_changed = True
def set_use_square_pixels(self, use_square_pixels):
"""Request using/not using square pixels.
:param use_square_pixels: If True, the resolution will be stretched to match 1:1 pixels.
By default, the pixels have a ratio of 11:12.
:type use_square_pixels: bool
:raises: :py:exc:`ValueError` if the given argument is not a bool.
"""
if use_square_pixels != self._use_square_pixels:
self._use_square_pixels = self.sanitize_bool(use_square_pixels, "use_square_pixels")
self.video_params_changed = True
@staticmethod
def sanitize_bool(value, field_name):
"""Convert the given value to a bool.
:param value: Either True, False,, "1", "0", 1 or 0.
:type value: :py:class:`basestring` | :py:class:`bool` | :py:class:`int`
:param field_name: Name of the field this value belongs to (just for debug messages).
:type field_name: basestring
:return: The bool value of the given value.
:rtype: :py:class:`bool`
:raises: :py:exc:`ValueError` if the given value is not supported in this conversion.
"""
if value not in (True, False, "1", "0", 1, 0):
raise ValueError("%s is not a valid value for %s." % (str(value), field_name))
# bool("0") returns True because it is a nonempty string
if value == "0":
return False
return bool(value)
def _camera_diagnostic_callback(self, diag_message):
assert isinstance(diag_message, DiagnosticStatusWrapper)
diag_message.summary(DiagnosticStatusWrapper.OK, "Video parameters")
diag_message.add("FPS", self._fps)
diag_message.add("Resolution", self._resolution)
diag_message.add("Compression", self._compression)
diag_message.add("Color image", self._use_color)
diag_message.add("Square pixels used", self._use_square_pixels)
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()
def __init__(self, hostname, username, password, width, height, frame_id, camera_info_url, use_encrypted_password,
camera_id=1, auto_wakeup_camera=True, compression=0, fps=24, use_color=True,
use_square_pixels=False):
"""Create the ROS-VAPIX interface.
:param hostname: Hostname of the camera (without http://, can be an IP address).
:type hostname: basestring
:param username: If login is needed, provide a username here.
:type username: :py:obj:`basestring` | None
:param password: If login is needed, provide a password here.
:type password: :py:obj:`basestring` | None
:param width: Width of the requested video stream in pixels (can be changed later). Must be one of the supported
resolutions. If `None`, the resolution will be chosen by height only. If also `height` is `None`,
then the default camera resolution will be used.
:type width: int|None
:param height: Height of the requested video stream in pixels (can be changed later). Must be one of the
supported resolutions. If `None`, the resolution will be chosen by width only. If also `width` is
`None`, then the default camera resolution will be used.
:type height: int|None
:param frame_id: The ROS TF frame assigned to the camera.
:type frame_id: basestring
:param camera_info_url: The URL pointing to the camera calaibration, if available.
:type camera_info_url: basestring
:param use_encrypted_password: Whether to use Plain HTTP Auth (False) or Digest HTTP Auth (True).
:type use_encrypted_password: bool
:param camera_id: ID (number) of the camera. Can be 1 to 4.
:type camera_id: int
:param auto_wakeup_camera: If True, there will be a wakeup trial after first unsuccessful network command.
:type auto_wakeup_camera: bool
:param compression: Compression of the image (0 - no compression, 100 - max compression).
:type compression: int
:param fps: The desired frames per second.
:type fps: int
:param use_color: If True, send a color stream, otherwise send only grayscale image.
:type use_color: bool
:param use_square_pixels: If True, the resolution will be stretched to match 1:1 pixels.
By default, the pixels have a ratio of 11:12.
:type use_square_pixels: bool
:raises: :py:exc:`ValueError` if the requested resolution (either the `resolution`, or `width`+`height`
is not supported.
"""
# True every time the video parameters have changed and the URL has to be altered (set from other threads).
self.video_params_changed = False
self.__initializing = True
self._hostname = hostname
self._camera_id = camera_id
self.diagnostic_updater = Updater()
self.diagnostic_updater.setHardwareID(hostname)
self._api = None
# autodetect the VAPIX API and connect to it; try it forever
while self._api is None and not rospy.is_shutdown():
try:
self._api = VAPIX.get_api_for_camera(hostname, username, password, camera_id, use_encrypted_password)
except (IOError, ValueError):
rospy.loginfo("Retrying connection to VAPIX on host %s, camera %d in 2 seconds." %
(hostname, camera_id))
rospy.sleep(2)
if rospy.is_shutdown():
return
self._allowed_resolutions = self._get_allowed_resolutions()
rospy.loginfo("The following resolutions are available for camera %d:\n%s" %
(camera_id, "\n".join([str(res) for res in self._allowed_resolutions])))
rospy.set_param("~allowed_resolutions", [res.get_vapix_representation() for res in self._allowed_resolutions])
# Sometimes the camera falls into power saving mode and stops streaming.
# This setting allows the script to try to wake up the camera.
self._auto_wakeup_camera = auto_wakeup_camera
# dynamic-reconfigurable properties - definitions
self._width = None # deprecated
self._height = None # deprecated
self._resolution = None
self._compression = None
self._fps = None
self._use_color = None
self._use_square_pixels = None
# treat empty strings as None in width and height params
width = width if width != "" else None
height = height if height != "" else None
# dynamic-reconfigurable properties - defaults
if width is None and height is None:
# TODO change to perform default resolution detection from VAPIX
self.set_resolution(self._allowed_resolutions[0])
else:
resolution = self.find_resolution_by_size(width, height)
self.set_resolution(resolution.get_vapix_representation())
self.set_compression(compression)
self.set_fps(fps)
self.set_use_color(use_color)
self.set_use_square_pixels(use_square_pixels)
# only advertise the supported resolutions on dynamic reconfigure
change_enum_items(
VideoStreamConfig,
"resolution",
[{
'name': res.name if isinstance(res, CIFVideoResolution) else str(res),
'value': res.get_vapix_representation(),
'description': str(res)
} for res in self._allowed_resolutions],
self._resolution.get_vapix_representation()
)
# dynamic reconfigure server
self._video_stream_param_change_server = dynamic_reconfigure.server.Server(VideoStreamConfig,
self.reconfigure_video)
# camera info setup
self._frame_id = frame_id
self._camera_info_url = camera_info_url
# generate a valid camera name based on the hostname
self._camera_name = camera_info_manager.genCameraName(self._hostname)
self._camera_info = camera_info_manager.CameraInfoManager(cname=self._camera_name, url=self._camera_info_url)
self._camera_info.loadCameraInfo() # required before getCameraInfo()
# the thread used for streaming images (is instantiated when the first image subscriber subscribes)
self._streaming_thread = None
# the publishers are started/stopped lazily in peer_subscribe/peer_unsubscribe
self._video_publisher_frequency_diagnostic = FrequencyStatusParam({'min': self._fps, 'max': self._fps})
self._video_publisher = PausableDiagnosedPublisher(
self,
rospy.Publisher("image_raw/compressed", CompressedImage, self, queue_size=100),
self.diagnostic_updater, self._video_publisher_frequency_diagnostic, TimeStampStatusParam()
)
self._camera_info_publisher = PausableDiagnosedPublisher(
self,
rospy.Publisher("camera_info", CameraInfo, self, queue_size=100),
self.diagnostic_updater, self._video_publisher_frequency_diagnostic, TimeStampStatusParam()
)
self._snapshot_server = rospy.Service("take_snapshot", TakeSnapshot, self.take_snapshot)
self.diagnostic_updater.add(FunctionDiagnosticTask("Camera parameters", self._camera_diagnostic_callback))
# BACKWARDS COMPATIBILITY LAYER
self.username = username # deprecated
self.password = password # deprecated
self.use_encrypted_password = use_encrypted_password # deprecated
self.st = None # deprecated
self.pub = self._video_publisher # deprecated
self.caminfo_pub = self._camera_info_publisher # deprecated
self.__initializing = False
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)
def init_diagnostics(self):
updater = Updater()
updater.add("Computation Time", self.update_ct_diagnostic)
rospy.Timer(rospy.Duration(1), lambda evt: 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)
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)
class XSensDriver(object):
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 reset_vars(self):
self.imu_msg = Imu()
self.imu_msg.orientation_covariance = (-1., ) * 9
self.imu_msg.angular_velocity_covariance = (-1., ) * 9
self.imu_msg.linear_acceleration_covariance = (-1., ) * 9
self.pub_imu = False
self.gps_msg = NavSatFix()
self.pub_gps = False
self.vel_msg = TwistStamped()
self.pub_vel = False
self.mag_msg = MagneticField()
self.mag_msg.magnetic_field_covariance = (0, ) * 9
self.pub_mag = False
self.temp_msg = Temperature()
self.temp_msg.variance = 0.
self.pub_temp = False
self.press_msg = FluidPressure()
self.press_msg.variance = 0.
self.pub_press = False
self.anin1_msg = UInt16()
self.pub_anin1 = False
self.anin2_msg = UInt16()
self.pub_anin2 = False
self.ecef_msg = PointStamped()
self.pub_ecef = False
self.pub_diag = False
def spin(self):
try:
while not rospy.is_shutdown():
self.spin_once()
self.reset_vars()
# Ctrl-C signal interferes with select with the ROS signal handler
# should be OSError in python 3.?
except select.error:
pass
def diagnostic_self_test(self, stat):
stat.summary(self.stest_stat.level, self.stest_stat.message)
def diagnostic_xkf(self, stat):
stat.summary(self.xkf_stat.level, self.xkf_stat.message)
def diagnostic_gps(self, stat):
stat.summary(self.gps_stat.level, self.gps_stat.message)
def diagnostic_device(self, stat):
stat.summary(DiagnosticStatus.OK, "Device Ok")
for (key, value) in self.device_info.iteritems():
stat.add(key, value)
def spin_once(self):
'''Read data from device and publishes ROS messages.'''
def convert_coords(x, y, z, source, dest=self.frame_local):
"""Convert the coordinates between ENU, NED, and NWU."""
if source == dest:
return x, y, z
# convert to ENU
if source == 'NED':
x, y, z = y, x, -z
elif source == 'NWU':
x, y, z = -y, x, z
# convert to desired
if dest == 'NED':
x, y, z = y, x, -z
elif dest == 'NWU':
x, y, z = y, -x, z
return x, y, z
def convert_quat(q, source, dest=self.frame_local):
"""Convert a quaternion between ENU, NED, and NWU."""
def q_mult((w0, x0, y0, z0), (w1, x1, y1, z1)):
"""Quaternion multiplication."""
w = w0 * w1 - x0 * x1 - y0 * y1 - z0 * z1
x = w0 * x1 + x0 * w1 + y0 * z1 - z0 * y1
y = w0 * y1 - x0 * z1 + y0 * w1 + z0 * x1
z = w0 * z1 + x0 * y1 - y0 * x1 + z0 * w1
return (w, x, y, z)
q_enu_ned = (0, 1. / sqrt(2), 1. / sqrt(2), 0)
q_enu_nwu = (1. / sqrt(2), 0, 0, -1. / sqrt(2))
q_ned_nwu = (0, -1, 0, 0)
q_ned_enu = (0, -1. / sqrt(2), -1. / sqrt(2), 0)
q_nwu_enu = (1. / sqrt(2), 0, 0, 1. / sqrt(2))
q_nwu_ned = (0, 1, 0, 0)
if source == 'ENU':
if dest == 'ENU':
return q
elif dest == 'NED':
return q_mult(q_enu_ned, q)
elif dest == 'NWU':
return q_mult(q_enu_nwu, q)
elif source == 'NED':
if dest == 'ENU':
return q_mult(q_ned_enu, q)
elif dest == 'NED':
return q
elif dest == 'NWU':
return q_mult(q_ned_nwu, q)
elif source == 'NWU':
if dest == 'ENU':
return q_mult(q_nwu_enu, q)
elif dest == 'NED':
return q_mult(q_nwu_ned, q)
elif dest == 'NWU':
return q
def publish_time_ref(secs, nsecs, source):
"""Publish a time reference."""
# Doesn't follow the standard publishing pattern since several time
# refs could be published simultaneously
if self.time_ref_pub is None:
self.time_ref_pub = rospy.Publisher('time_reference',
TimeReference,
queue_size=10)
time_ref_msg = TimeReference()
time_ref_msg.header = self.h
time_ref_msg.time_ref.secs = secs
time_ref_msg.time_ref.nsecs = nsecs
time_ref_msg.source = source
self.time_ref_pub.publish(time_ref_msg)
def stamp_from_itow(itow, y=None, m=None, d=None, ns=0, week=None):
"""Return (secs, nsecs) from GPS time of week ms information."""
if y is not None:
stamp_day = datetime.datetime(y, m, d)
elif week is not None:
epoch = datetime.datetime(1980, 1, 6) # GPS epoch
stamp_day = epoch + datetime.timedelta(weeks=week)
else:
today = datetime.date.today() # using today by default
stamp_day = datetime.datetime(today.year, today.month,
today.day)
iso_day = stamp_day.isoweekday() # 1 for Monday, 7 for Sunday
# stamp for the GPS start of the week (Sunday morning)
start_of_week = stamp_day - datetime.timedelta(days=iso_day)
# stamp at the millisecond precision
stamp_ms = start_of_week + datetime.timedelta(milliseconds=itow)
secs = time.mktime(
(stamp_ms.year, stamp_ms.month, stamp_ms.day, stamp_ms.hour,
stamp_ms.minute, stamp_ms.second, 0, 0, -1))
nsecs = stamp_ms.microsecond * 1000 + ns
if nsecs < 0: # ns can be negative
secs -= 1
nsecs += 1e9
return (secs, nsecs)
# MTData
def fill_from_RAW(raw_data):
'''Fill messages with information from 'raw' MTData block.'''
# don't publish raw imu data anymore
# TODO find what to do with that
rospy.loginfo("Got MTi data packet: 'RAW', ignored!")
def fill_from_RAWGPS(rawgps_data):
'''Fill messages with information from 'rawgps' MTData block.'''
if rawgps_data['bGPS'] < self.old_bGPS:
self.pub_gps = True
# LLA
self.gps_msg.latitude = rawgps_data['LAT'] * 1e-7
self.gps_msg.longitude = rawgps_data['LON'] * 1e-7
self.gps_msg.altitude = rawgps_data['ALT'] * 1e-3
# NED vel # TODO?
self.old_bGPS = rawgps_data['bGPS']
def fill_from_Temp(temp):
'''Fill messages with information from 'temperature' MTData block.
'''
self.pub_temp = True
self.temp_msg.temperature = temp
def fill_from_Calib(imu_data):
'''Fill messages with information from 'calibrated' MTData block.'''
try:
self.pub_imu = True
x, y, z = convert_coords(imu_data['gyrX'], imu_data['gyrY'],
imu_data['gyrZ'], o['frame'])
self.imu_msg.angular_velocity.x = x
self.imu_msg.angular_velocity.y = y
self.imu_msg.angular_velocity.z = z
self.imu_msg.angular_velocity_covariance = (radians(0.025), 0.,
0., 0.,
radians(0.025), 0.,
0., 0.,
radians(0.025))
self.pub_vel = True
self.vel_msg.twist.angular.x = x
self.vel_msg.twist.angular.y = y
self.vel_msg.twist.angular.z = z
except KeyError:
pass
try:
self.pub_imu = True
x, y, z = convert_coords(imu_data['accX'], imu_data['accY'],
imu_data['accZ'], o['frame'])
self.imu_msg.linear_acceleration.x = x
self.imu_msg.linear_acceleration.y = y
self.imu_msg.linear_acceleration.z = z
self.imu_msg.linear_acceleration_covariance = (0.0004, 0., 0.,
0., 0.0004, 0.,
0., 0., 0.0004)
except KeyError:
pass
try:
self.pub_mag = True
x, y, z = convert_coords(imu_data['magX'], imu_data['magY'],
imu_data['magZ'], o['frame'])
self.mag_msg.magnetic_field.x = x
self.mag_msg.magnetic_field.y = y
self.mag_msg.magnetic_field.z = z
except KeyError:
pass
def fill_from_Orient(orient_data):
'''Fill messages with information from 'orientation' MTData block.
'''
self.pub_imu = True
if 'quaternion' in orient_data:
w, x, y, z = orient_data['quaternion']
elif 'roll' in orient_data:
x, y, z, w = quaternion_from_euler(
radians(orient_data['roll']),
radians(orient_data['pitch']), radians(orient_data['yaw']))
elif 'matrix' in orient_data:
m = identity_matrix()
m[:3, :3] = orient_data['matrix']
x, y, z, w = quaternion_from_matrix(m)
self.imu_msg.orientation.x = x
self.imu_msg.orientation.y = y
self.imu_msg.orientation.z = z
self.imu_msg.orientation.w = w
self.imu_msg.orientation_covariance = (radians(1.), 0., 0., 0.,
radians(1.), 0., 0., 0.,
radians(9.))
def fill_from_Auxiliary(aux_data):
'''Fill messages with information from 'Auxiliary' MTData block.'''
try:
self.anin1_msg.data = o['Ain_1']
self.pub_anin1 = True
except KeyError:
pass
try:
self.anin2_msg.data = o['Ain_2']
self.pub_anin2 = True
except KeyError:
pass
def fill_from_Pos(position_data):
'''Fill messages with information from 'position' MTData block.'''
self.pub_gps = True
self.gps_msg.latitude = position_data['Lat']
self.gps_msg.longitude = position_data['Lon']
self.gps_msg.altitude = position_data['Alt']
def fill_from_Vel(velocity_data):
'''Fill messages with information from 'velocity' MTData block.'''
self.pub_vel = True
x, y, z = convert_coords(velocity_data['Vel_X'],
velocity_data['Vel_Y'],
velocity_data['Vel_Z'], o['frame'])
self.vel_msg.twist.linear.x = x
self.vel_msg.twist.linear.y = y
self.vel_msg.twist.linear.z = z
def fill_from_Stat(status):
'''Fill messages with information from 'status' MTData block.'''
self.pub_diag = True
if status & 0b0001:
self.stest_stat.level = DiagnosticStatus.OK
self.stest_stat.message = "Ok"
else:
self.stest_stat.level = DiagnosticStatus.ERROR
self.stest_stat.message = "Failed"
if status & 0b0010:
self.xkf_stat.level = DiagnosticStatus.OK
self.xkf_stat.message = "Valid"
else:
self.xkf_stat.level = DiagnosticStatus.WARN
self.xkf_stat.message = "Invalid"
if status & 0b0100:
self.gps_stat.level = DiagnosticStatus.OK
self.gps_stat.message = "Ok"
self.gps_msg.status.status = NavSatStatus.STATUS_FIX
self.gps_msg.status.service = NavSatStatus.SERVICE_GPS
else:
self.gps_stat.level = DiagnosticStatus.WARN
self.gps_stat.message = "No fix"
self.gps_msg.status.status = NavSatStatus.STATUS_NO_FIX
self.gps_msg.status.service = 0
def fill_from_Sample(ts):
'''Catch 'Sample' MTData blocks.'''
self.h.seq = ts
# MTData2
def fill_from_Temperature(o):
'''Fill messages with information from 'Temperature' MTData2 block.
'''
self.pub_temp = True
self.temp_msg.temperature = o['Temp']
def fill_from_Timestamp(o):
'''Fill messages with information from 'Timestamp' MTData2 block.'''
try:
# put timestamp from gps UTC time if available
y, m, d, hr, mi, s, ns, f = o['Year'], o['Month'], o['Day'],\
o['Hour'], o['Minute'], o['Second'], o['ns'], o['Flags']
if f & 0x4:
secs = time.mktime((y, m, d, hr, mi, s, 0, 0, 0))
publish_time_ref(secs, ns, 'UTC time')
except KeyError:
pass
try:
itow = o['TimeOfWeek']
secs, nsecs = stamp_from_itow(itow)
publish_time_ref(secs, nsecs, 'integer time of week')
except KeyError:
pass
try:
sample_time_fine = o['SampleTimeFine']
secs = int(sample_time_fine / 1000)
nsecs = 1e6 * (sample_time_fine % 1000)
publish_time_ref(secs, nsecs, 'sample time fine')
except KeyError:
pass
try:
sample_time_coarse = o['SampleTimeCoarse']
publish_time_ref(sample_time_coarse, 0, 'sample time coarse')
except KeyError:
pass
# TODO find what to do with other kind of information
pass
def fill_from_Orientation_Data(o):
'''Fill messages with information from 'Orientation Data' MTData2
block.'''
self.pub_imu = True
try:
x, y, z, w = o['Q1'], o['Q2'], o['Q3'], o['Q0']
except KeyError:
pass
try:
x, y, z, w = quaternion_from_euler(radians(o['Roll']),
radians(o['Pitch']),
radians(o['Yaw']))
except KeyError:
pass
try:
a, b, c, d, e, f, g, h, i = o['a'], o['b'], o['c'], o['d'],\
o['e'], o['f'], o['g'], o['h'], o['i']
m = identity_matrix()
m[:3, :3] = ((a, b, c), (d, e, f), (g, h, i))
x, y, z, w = quaternion_from_matrix(m)
except KeyError:
pass
w, x, y, z = convert_quat((w, x, y, z), o['frame'])
self.imu_msg.orientation.x = x
self.imu_msg.orientation.y = y
self.imu_msg.orientation.z = z
self.imu_msg.orientation.w = w
self.imu_msg.orientation_covariance = (radians(1.), 0., 0., 0.,
radians(1.), 0., 0., 0.,
radians(9.))
def fill_from_Pressure(o):
'''Fill messages with information from 'Pressure' MTData2 block.'''
self.press_msg.fluid_pressure = o['Pressure']
self.pub_press = True
def fill_from_Acceleration(o):
'''Fill messages with information from 'Acceleration' MTData2
block.'''
self.pub_imu = True
# FIXME not sure we should treat all in that same way
try:
x, y, z = o['Delta v.x'], o['Delta v.y'], o['Delta v.z']
except KeyError:
pass
try:
x, y, z = o['freeAccX'], o['freeAccY'], o['freeAccZ']
except KeyError:
pass
try:
x, y, z = o['accX'], o['accY'], o['accZ']
except KeyError:
pass
x, y, z = convert_coords(x, y, z, o['frame'])
self.imu_msg.linear_acceleration.x = x
self.imu_msg.linear_acceleration.y = y
self.imu_msg.linear_acceleration.z = z
self.imu_msg.linear_acceleration_covariance = (0.0004, 0., 0., 0.,
0.0004, 0., 0., 0.,
0.0004)
def fill_from_Position(o):
'''Fill messages with information from 'Position' MTData2 block.'''
try:
self.gps_msg.latitude = o['lat']
self.gps_msg.longitude = o['lon']
self.pub_gps = True
# altMsl is deprecated
alt = o.get('altEllipsoid', o.get('altMsl', 0))
self.gps_msg.altitude = alt
except KeyError:
pass
try:
x, y, z = o['ecefX'], o['ecefY'], o['ecefZ']
# TODO: ecef units not specified: might not be in meters!
self.ecef_msg.point.x = x
self.ecef_msg.point.y = y
self.ecef_msg.point.z = z
self.pub_ecef = True
except KeyError:
pass
def fill_from_GNSS(o):
'''Fill messages with information from 'GNSS' MTData2 block.'''
try: # PVT
# time block
itow, y, m, d, ns, f = o['itow'], o['year'], o['month'],\
o['day'], o['nano'], o['valid']
if f & 0x4:
secs, nsecs = stamp_from_itow(itow, y, m, d, ns)
publish_time_ref(secs, nsecs, 'GNSS time UTC')
# flags
fixtype = o['fixtype']
if fixtype == 0x00:
self.gps_msg.status.status = NavSatStatus.STATUS_NO_FIX # no fix
self.gps_msg.status.service = 0
else:
self.gps_msg.status.status = NavSatStatus.STATUS_FIX # unaugmented
self.gps_msg.status.service = NavSatStatus.SERVICE_GPS
# lat lon alt
self.gps_msg.latitude = o['lat']
self.gps_msg.longitude = o['lon']
self.gps_msg.altitude = o['height'] / 1e3
self.pub_gps = True
# TODO velocity?
# TODO 2D heading?
# TODO DOP?
except KeyError:
pass
# TODO publish Sat Info
def fill_from_Angular_Velocity(o):
'''Fill messages with information from 'Angular Velocity' MTData2
block.'''
try:
x, y, z = convert_coords(o['gyrX'], o['gyrY'], o['gyrZ'],
o['frame'])
self.imu_msg.angular_velocity.x = x
self.imu_msg.angular_velocity.y = y
self.imu_msg.angular_velocity.z = z
self.imu_msg.angular_velocity_covariance = (radians(0.025), 0.,
0., 0.,
radians(0.025), 0.,
0., 0.,
radians(0.025))
self.pub_imu = True
self.vel_msg.twist.angular.x = x
self.vel_msg.twist.angular.y = y
self.vel_msg.twist.angular.z = z
self.pub_vel = True
except KeyError:
pass
# TODO decide what to do with 'Delta q'
def fill_from_GPS(o):
'''Fill messages with information from 'GPS' MTData2 block.'''
# TODO DOP
try: # SOL
x, y, z = o['ecefX'], o['ecefY'], o['ecefZ']
self.ecef_msg.point.x = x * 0.01 # data is in cm
self.ecef_msg.point.y = y * 0.01
self.ecef_msg.point.z = z * 0.01
self.pub_ecef = True
vx, vy, vz = o['ecefVX'], o['ecefVY'], o['ecefVZ']
self.vel_msg.twist.linear.x = vx * 0.01 # data is in cm
self.vel_msg.twist.linear.y = vy * 0.01
self.vel_msg.twist.linear.z = vz * 0.01
self.pub_vel = True
itow, ns, week, f = o['iTOW'], o['fTOW'], o['Week'], o['Flags']
if (f & 0x0C) == 0xC:
secs, nsecs = stamp_from_itow(itow, ns=ns, week=week)
publish_time_ref(secs, nsecs, 'GPS Time')
# TODO there are other pieces of information that we could
# publish
except KeyError:
pass
try: # Time UTC
itow, y, m, d, ns, f = o['iTOW'], o['year'], o['month'],\
o['day'], o['nano'], o['valid']
if f & 0x4:
secs, nsecs = stamp_from_itow(itow, y, m, d, ns)
publish_time_ref(secs, nsecs, 'GPS Time UTC')
except KeyError:
pass
# TODO publish SV Info
def fill_from_SCR(o):
'''Fill messages with information from 'SCR' MTData2 block.'''
# TODO that's raw information
pass
def fill_from_Analog_In(o):
'''Fill messages with information from 'Analog In' MTData2 block.'''
try:
self.anin1_msg.data = o['analogIn1']
self.pub_anin1 = True
except KeyError:
pass
try:
self.anin2_msg.data = o['analogIn2']
self.pub_anin2 = True
except KeyError:
pass
def fill_from_Magnetic(o):
'''Fill messages with information from 'Magnetic' MTData2 block.'''
x, y, z = convert_coords(o['magX'], o['magY'], o['magZ'],
o['frame'])
self.mag_msg.magnetic_field.x = x
self.mag_msg.magnetic_field.y = y
self.mag_msg.magnetic_field.z = z
self.pub_mag = True
def fill_from_Velocity(o):
'''Fill messages with information from 'Velocity' MTData2 block.'''
x, y, z = convert_coords(o['velX'], o['velY'], o['velZ'],
o['frame'])
self.vel_msg.twist.linear.x = x
self.vel_msg.twist.linear.y = y
self.vel_msg.twist.linear.z = z
self.pub_vel = True
def fill_from_Status(o):
'''Fill messages with information from 'Status' MTData2 block.'''
try:
status = o['StatusByte']
fill_from_Stat(status)
except KeyError:
pass
try:
status = o['StatusWord']
fill_from_Stat(status)
except KeyError:
pass
def find_handler_name(name):
return "fill_from_%s" % (name.replace(" ", "_"))
# get data
try:
data = self.mt.read_measurement()
except mtdef.MTTimeoutException:
time.sleep(0.1)
return
# common header
self.h = Header()
self.h.stamp = rospy.Time.now()
self.h.frame_id = self.frame_id
# set default values
self.reset_vars()
# fill messages based on available data fields
for n, o in data.items():
try:
locals()[find_handler_name(n)](o)
except KeyError:
rospy.logwarn("Unknown MTi data packet: '%s', ignoring." % n)
# publish available information
if self.pub_imu:
self.imu_msg.header = self.h
if self.imu_pub is None:
self.imu_pub = rospy.Publisher('imu/data', Imu, queue_size=10)
self.imu_freq.tick(self.h.stamp)
self.imu_pub.publish(self.imu_msg)
if self.pub_gps:
self.gps_msg.header = self.h
if self.gps_pub is None:
self.gps_pub = rospy.Publisher('fix', NavSatFix, queue_size=10)
self.gps_pub.publish(self.gps_msg)
if self.pub_vel:
self.vel_msg.header = self.h
if self.vel_pub is None:
self.vel_pub = rospy.Publisher('velocity',
TwistStamped,
queue_size=10)
self.vel_pub.publish(self.vel_msg)
if self.pub_mag:
self.mag_msg.header = self.h
if self.mag_pub is None:
self.mag_pub = rospy.Publisher('imu/mag',
MagneticField,
queue_size=10)
self.mag_freq.tick(self.h.stamp)
self.mag_pub.publish(self.mag_msg)
if self.pub_temp:
self.temp_msg.header = self.h
if self.temp_pub is None:
self.temp_pub = rospy.Publisher('temperature',
Temperature,
queue_size=10)
self.temp_pub.publish(self.temp_msg)
if self.pub_press:
self.press_msg.header = self.h
if self.press_pub is None:
self.press_pub = rospy.Publisher('pressure',
FluidPressure,
queue_size=10)
self.press_pub.publish(self.press_msg)
if self.pub_anin1:
if self.analog_in1_pub is None:
self.analog_in1_pub = rospy.Publisher('analog_in1',
UInt16,
queue_size=10)
self.analog_in1_pub.publish(self.anin1_msg)
if self.pub_anin2:
if self.analog_in2_pub is None:
self.analog_in2_pub = rospy.Publisher('analog_in2',
UInt16,
queue_size=10)
self.analog_in2_pub.publish(self.anin2_msg)
if self.pub_ecef:
self.ecef_msg.header = self.h
if self.ecef_pub is None:
self.ecef_pub = rospy.Publisher('ecef',
PointStamped,
queue_size=10)
self.ecef_pub.publish(self.ecef_msg)
# publish string representation
self.str_pub.publish(str(data))
self.updater.update()
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)
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 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
