Example #1
0
    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)
Example #2
0
    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)
Example #3
0
    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))
Example #4
0
 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")
Example #5
0
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
Example #7
0
    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)
Example #8
0
 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()
Example #9
0
 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()
Example #10
0
    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)
Example #13
0
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)
Example #14
0
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)
Example #15
0
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()
Example #16
0
    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())
Example #19
0
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)
Example #20
0
    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)
Example #21
0
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()
Example #22
0
    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)
Example #23
0
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()
Example #24
0
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