00001
00002
00003
00004
00005
00006
00007 import os, sys, string, time, getopt, threading
00008 import httplib, urllib
00009
00010 import roslib; roslib.load_manifest('clearpath_sensors')
00011 import rospy
00012
00013 from clearpath_sensors.msg import Axis
00014
00015
00016 class StateThread(threading.Thread):
00017 def __init__(self, axis):
00018 threading.Thread.__init__(self)
00019
00020 self.axis = axis
00021 self.daemon = True
00022
00023 def run(self):
00024 r = rospy.Rate(10)
00025 msg = Axis()
00026
00027 while True:
00028 conn = httplib.HTTPConnection(self.axis.hostname)
00029 params = { 'query':'position' }
00030 conn.request("GET", "/axis-cgi/com/ptz.cgi?%s" % urllib.urlencode(params))
00031 response = conn.getresponse()
00032 if response.status == 200:
00033 body = response.read()
00034 params = dict([s.split('=',2) for s in body.splitlines()])
00035 msg.pan = float(params['pan'])
00036 msg.tilt = float(params['tilt'])
00037 msg.zoom = float(params['zoom'])
00038 msg.iris = float(params['iris'])
00039 msg.focus = 0.0
00040 if 'focus' in params:
00041 msg.focus = float(params['focus'])
00042 msg.autofocus = (params['autofocus'] == 'on')
00043 self.axis.pub.publish(msg)
00044 r.sleep()
00045
00046
00047 class AxisPTZ:
00048 def __init__(self, hostname, username, password):
00049 self.hostname = hostname
00050 self.username = username
00051 self.password = password
00052
00053 self.st = None
00054 self.pub = rospy.Publisher("state", Axis, self)
00055 self.sub = rospy.Subscriber("cmd", Axis, self.cmd)
00056
00057 def peer_subscribe(self, topic_name, topic_publish, peer_publish):
00058
00059 if self.st is None:
00060 self.st = StateThread(self)
00061 self.st.start()
00062
00063 def cmd(self, msg):
00064 conn = httplib.HTTPConnection(self.hostname)
00065 params = { 'pan': msg.pan, 'tilt': msg.tilt, 'zoom': msg.zoom, 'brightness': msg.brightness }
00066 if msg.autofocus:
00067 params['autofocus'] = 'on'
00068 else:
00069 params['autofocus'] = 'off'
00070 params['focus'] = msg.focus
00071 if msg.iris > 0.000001:
00072 rospy.logwarn("Iris value is read-only.")
00073 conn.request("GET", "/axis-cgi/com/ptz.cgi?%s" % urllib.urlencode(params))
00074
00075
00076 def main():
00077 rospy.init_node("axis")
00078
00079 arg_defaults = {
00080 'hostname': '192.168.0.90',
00081 'username': '',
00082 'password': '',
00083 }
00084 args = {}
00085 for name in arg_defaults:
00086 args[name] = rospy.get_param(rospy.search_param(name), arg_defaults[name])
00087
00088 AxisPTZ(**args)
00089 rospy.spin()
00090
00091
00092 if __name__ == "__main__":
00093 main()