feat: add focus and iris service

CMakeLists.txt

@@ -33,6 +33,8 @@ FILES
 add_service_files(DIRECTORY srv
 FILES
    set_ptz.srv
+  set_focus.srv
+  set_iris.srv
    set_digital_output.srv
 )
 

README.md

@@ -91,3 +91,48 @@ relative: false"
 * params pan and tilt as float (radians)
 * param zoom as float (proportional zoom between min_zoom_augment and max_zoom_augment)
 * param relative as bool (increases the current pan,tilt,zoom relative to the current values)
+
+### Focus and iris services
+
+The PTZ node also exposes two additional services to control optics parameters.
+These services are independent from continuous PTZ command publishing.
+
+* `~set_focus` (`axis_camera/set_focus`)
+  * request fields:
+    * `focus` (`float32`): manual focus value
+    * `autofocus` (`bool`): if `true`, camera autofocus is enabled and `focus` value is ignored by the camera
+* `~set_iris` (`axis_camera/set_iris`)
+  * request fields:
+    * `iris` (`float32`): manual iris value
+    * `autoiris` (`bool`): if `true`, camera autoiris is enabled and `iris` value is ignored by the camera
+
+Examples:
+
+Enable autofocus:
+```
+rosservice call /axis_camera_ptz/set_focus "focus: 0.0
+autofocus: true"
+```
+
+Set manual focus:
+```
+rosservice call /axis_camera_ptz/set_focus "focus: 1200.0
+autofocus: false"
+```
+
+Enable autoiris:
+```
+rosservice call /axis_camera_ptz/set_iris "iris: 0.0
+autoiris: true"
+```
+
+Set manual iris:
+```
+rosservice call /axis_camera_ptz/set_iris "iris: 500.0
+autoiris: false"
+```
+
+Notes:
+
+* Service names depend on node namespace. Run `rosservice list` to get exact names in your setup.
+* Current `focus`, `autofocus`, `iris` and `autoiris` values are published in `~camera_params` (`robotnik_msgs/Axis`).

src/axis_camera/axis_lib/axis_control.py

@@ -15,21 +15,44 @@ class ControlAxis():
     def __init__(self, hostname):
         self.hostname = hostname
 
-    def sendPTZCommand(self, pan, tilt, zoom):
+    def sendPTZCommand(self, pan=None, tilt=None, zoom=None, focus=None, autofocus=None, iris=None, autoiris=None):
         ret = {
             'exception': False,
             'error_msg': '',
-            'status': 0
+            'status': 0,
+            'body': '',
+            'url': ''
         }
 
         conn = httplib.HTTPConnection(self.hostname)
-        params = { 'pan': pan, 'tilt': tilt, 'zoom': zoom }
+        params = {}
+        if pan is not None:
+            params['pan'] = pan
+        if tilt is not None:
+            params['tilt'] = tilt
+        if zoom is not None:
+            params['zoom'] = zoom
+        if focus is not None:
+            params['focus'] = focus
+        if autofocus is not None:
+            params['autofocus'] = 'on' if autofocus else 'off'
+        if iris is not None:
+            params['iris'] = iris
+        if autoiris is not None:
+            params['autoiris'] = 'on' if autoiris else 'off'
 
         try:		   
             url = "/axis-cgi/com/ptz.cgi?camera=1&%s" % urllib_parse.urlencode(params)
 
             conn.request("GET", url)
-            ret['status'] = conn.getresponse().status
+            response = conn.getresponse()
+            ret['status'] = response.status
+            body = response.read()
+            if body:
+                try:
+                    ret['body'] = body.decode()
+                except AttributeError:
+                    ret['body'] = body
             ret['url'] = url
 
         except socket.error as e:
@@ -38,6 +61,8 @@ def sendPTZCommand(self, pan, tilt, zoom):
         except socket.timeout as e:
             ret['exception'] = True
             ret['error_msg'] = e
+        finally:
+            conn.close()
         return ret
 
     def getPTZState(self):
@@ -89,6 +114,8 @@ def getPTZState(self):
                     "autofocus" : autofocus,
                     "iris" : iris,
                     "autoiris" : autoiris,
+                    "supports_focus" : ('focus' in params or 'autofocus' in params),
+                    "supports_iris" : ('iris' in params or 'autoiris' in params),
                     "error_reading" : False,
                     "error_reading_msg" : ''
                 }    
@@ -102,6 +129,8 @@ def getPTZState(self):
         except ValueError as e:
             ptz_read["error_reading"]= True
             ptz_read["error_reading_msg"] = e
+        finally:
+            conn.close()
 
         return ptz_read
 

src/axis_camera/axis_ptz_node.py

@@ -40,6 +40,8 @@
 from std_srvs.srv import Empty
 from sensor_msgs.msg import JointState
 
+from axis_camera.srv import set_focus, set_focusResponse
+from axis_camera.srv import set_iris, set_irisResponse
 from robotnik_msgs.msg import Axis as AxisMsg
 from robotnik_msgs.msg import ptz
 from robotnik_msgs.msg import CameraParameters
@@ -104,8 +106,16 @@ def __init__(self, args):
         self.desired_pan = 0.0
         self.desired_tilt = 0.0
         self.desired_zoom = 0.0
+        self.desired_focus = 0.0
+        self.desired_autofocus = False
+        self.desired_iris = 0.0
+        self.desired_autoiris = False
         self.error_reading = False
         self.error_reading_msg = ''
+        self.focus_supported = True
+        self.iris_supported = True
+        self.focus_support_warned = False
+        self.iris_support_warned = False
 
         # Timer to get/release ptz control
         if(self.use_control_timeout):
@@ -128,15 +138,16 @@ def rosSetup(self):
         """
                 Sets the ros connections
         """
-        ns = rospy.get_namespace()
-        #self.pub = rospy.Publisher("~camera_params", AxisMsg, queue_size=10)
+        self.pub = rospy.Publisher("~camera_params", AxisMsg, queue_size=10)
         self.sub = rospy.Subscriber("~ptz_command", ptz, self.commandPTZCb)
         # Publish the joint state of the pan & tilt
         self.joint_state_publisher = rospy.Publisher(self.joint_states_topic, JointState, queue_size=10)
         # Publish camera zoom info
         self.zoom_parameter_pub = rospy.Publisher("~camera_parameters", CameraParameters, queue_size=10)
         # Services
         self.home_service = rospy.Service('~home_ptz', Empty, self.homeService)
+        self.focus_service = rospy.Service('~set_focus', set_focus, self.setFocusService)
+        self.iris_service = rospy.Service('~set_iris', set_iris, self.setIrisService)
 
         # Diagnostic Updater
         self.diagnostics_updater = diagnostic_updater.Updater()
@@ -275,6 +286,10 @@ def controlPTZ(self):
             self.desired_pan = self.invert_pan*self.current_ptz.pan 
             self.desired_tilt = self.invert_tilt*self.current_ptz.tilt
             self.desired_zoom = self.current_ptz.zoom
+            self.desired_focus = self.current_ptz.focus
+            self.desired_autofocus = self.current_ptz.autofocus
+            self.desired_iris = self.current_ptz.iris
+            self.desired_autoiris = self.current_ptz.autoiris
 
 
 
@@ -326,6 +341,110 @@ def sendPTZCommand(self, pan = None, tilt = None, zoom = None):
             rospy.logerr('%s:sendPTZCommand: error connecting the camera: %s '%(rospy.get_name(), control['error_msg']))
 
         self.t_last_command_sent = rospy.Time.now()
+
+    def setFocusService(self, req):
+        response = set_focusResponse()
+
+        if not self.ptz_syncronized:
+            response.ret = False
+            response.message = 'PTZ not synchronized yet'
+            rospy.logwarn('%s:setFocusService: %s', rospy.get_name(), response.message)
+            return response
+
+        if not self.focus_supported:
+            response.ret = False
+            response.message = 'Focus control not supported by this camera'
+            rospy.logwarn('%s:setFocusService: %s', rospy.get_name(), response.message)
+            return response
+
+        if not req.autofocus and not math.isfinite(req.focus):
+            response.ret = False
+            response.message = 'Invalid focus value'
+            rospy.logwarn('%s:setFocusService: %s', rospy.get_name(), response.message)
+            return response
+
+        focus_value = None if req.autofocus else req.focus
+        control = self.controller.sendPTZCommand(focus=focus_value, autofocus=req.autofocus)
+        if not self._commandSucceeded(control, 'focus'):
+            response.ret = False
+            response.message = self._formatCommandError(control, 'focus')
+            return response
+
+        self.desired_autofocus = req.autofocus
+        if focus_value is not None:
+            self.desired_focus = focus_value
+
+        response.ret = True
+        response.message = 'Focus command sent'
+        return response
+
+    def setIrisService(self, req):
+        response = set_irisResponse()
+
+        if not self.ptz_syncronized:
+            response.ret = False
+            response.message = 'PTZ not synchronized yet'
+            rospy.logwarn('%s:setIrisService: %s', rospy.get_name(), response.message)
+            return response
+
+        if not self.iris_supported:
+            response.ret = False
+            response.message = 'Iris control not supported by this camera'
+            rospy.logwarn('%s:setIrisService: %s', rospy.get_name(), response.message)
+            return response
+
+        if not req.autoiris and not math.isfinite(req.iris):
+            response.ret = False
+            response.message = 'Invalid iris value'
+            rospy.logwarn('%s:setIrisService: %s', rospy.get_name(), response.message)
+            return response
+
+        iris_value = None if req.autoiris else req.iris
+        control = self.controller.sendPTZCommand(iris=iris_value, autoiris=req.autoiris)
+        if not self._commandSucceeded(control, 'iris'):
+            response.ret = False
+            response.message = self._formatCommandError(control, 'iris')
+            return response
+
+        self.desired_autoiris = req.autoiris
+        if iris_value is not None:
+            self.desired_iris = iris_value
+
+        response.ret = True
+        response.message = 'Iris command sent'
+        return response
+
+    def _commandSucceeded(self, control, feature_name):
+        if control['status'] == 204 and not control['exception']:
+            return True
+
+        message = self._formatCommandError(control, feature_name)
+        if control['exception']:
+            rospy.logerr('%s:%s command failed: %s', rospy.get_name(), feature_name, message)
+        else:
+            rospy.logwarn('%s:%s command failed: %s', rospy.get_name(), feature_name, message)
+        return False
+
+    def _formatCommandError(self, control, feature_name):
+        if control['exception']:
+            return str(control['error_msg'])
+
+        body = control.get('body', '')
+        if body:
+            return 'HTTP %s: %s' % (control['status'], body.strip())
+        return 'HTTP %s while sending %s command to %s%s' % (control['status'], feature_name, self.hostname, control.get('url', ''))
+
+    def _updateOptionalSupport(self, ptz_read):
+        self.focus_supported = ptz_read.get('supports_focus', False)
+        self.iris_supported = ptz_read.get('supports_iris', False)
+
+        if not self.focus_supported and not self.focus_support_warned:
+            rospy.logwarn('%s:getPTZState: camera does not report focus/autofocus support', rospy.get_name())
+            self.focus_support_warned = True
+
+        if not self.iris_supported and not self.iris_support_warned:
+            rospy.logwarn('%s:getPTZState: camera does not report iris/autoiris support', rospy.get_name())
+            self.iris_support_warned = True
 
     def getPTZState(self):
         """
@@ -335,6 +454,7 @@ def getPTZState(self):
         # First time saves the current values
         ptz_read = self.controller.getPTZState()
         if not ptz_read["error_reading"]:
+            self._updateOptionalSupport(ptz_read)
 
             self.current_ptz.pan =  self.invert_pan * self.normalize_angle( ptz_read["pan"] - self.pan_offset)
             self.current_ptz.tilt = self.invert_tilt * (ptz_read["tilt"] - self.tilt_offset)
@@ -349,6 +469,10 @@ def getPTZState(self):
                 self.desired_pan = self.invert_pan*self.current_ptz.pan 
                 self.desired_tilt = self.invert_tilt*self.current_ptz.tilt
                 self.desired_zoom = self.current_ptz.zoom
+                self.desired_focus = self.current_ptz.focus
+                self.desired_autofocus = self.current_ptz.autofocus
+                self.desired_iris = self.current_ptz.iris
+                self.desired_autoiris = self.current_ptz.autoiris
                 rospy.loginfo('%s:getPTZState: PTZ state syncronized!', rospy.get_name())
                 self.ptz_syncronized = True
 
@@ -416,7 +540,7 @@ def publishROS(self):
 
         self.zoom_parameter_pub.publish(zoom_parameters)
         # Publishes the current PTZ values
-        #self.pub.publish(self.current_ptz)
+        self.pub.publish(self.current_ptz)
 
         # Publish the joint state
         msg = JointState()

srv/set_focus.srv

@@ -0,0 +1,5 @@
+float32 focus
+bool autofocus
+---
+bool ret
+string message

srv/set_iris.srv

@@ -0,0 +1,5 @@
+float32 iris
+bool autoiris
+---
+bool ret
+string message