def run(self):
r = rospy.Rate(10)
msg = Axis()
while True:
url = '%s:%s@%s' % (self.axis.username, self.axis.password, self.axis.hostname)
auth = base64.encodestring("%s:%s" % (self.axis.username, self.axis.password))
headers = {"Authorization" : "Basic %s" % auth}
conn = httplib.HTTPConnection(self.axis.hostname)
params = { 'query':'position' }
conn.request("GET", "/axis-cgi/com/ptz.cgi?query=position",headers=headers) #, urllib.urlencode(params)
response = conn.getresponse()
#print(response.status)
if response.status == 200:
body = response.read()
params = dict([s.split('=',2) for s in body.splitlines()])
msg.pan = float(params['pan'])
# Flip pan orient if the camera is mounted backwards and facing down
if self.axis.flip:
msg.pan = 180 - msg.pan
if msg.pan > 180:
msg.pan -= 360
if msg.pan < -180:
msg.pan += 360
msg.tilt = float(params['tilt'])
msg.zoom = float(params['zoom'])
msg.iris = float(params['iris'])
msg.focus = 0.0
if 'focus' in params:
msg.focus = float(params['focus'])
msg.autofocus = (params['autofocus'] == 'on')
self.axis.pub.publish(msg)
r.sleep()
def run(self):
r = rospy.Rate(10)
msg = Axis()
while True:
if not self.axis.twist_timeout and (
(rospy.Time.now() - self.axis.last_request).to_sec() > 1.0):
self.axis.twist_timeout = True
self.axis.cmd_twist(Twist(), reset_timeout=False)
conn = httplib.HTTPConnection(self.axis.hostname)
params = {'query': 'position'}
conn.request("GET",
"/axis-cgi/com/ptz.cgi?%s" % urllib.urlencode(params))
response = conn.getresponse()
if response.status == 200:
body = response.read()
params = dict([s.split('=', 2) for s in body.splitlines()])
msg.pan = float(params['pan'])
# Flip pan orient if the camera is mounted backwards and facing down
if self.axis.flip:
msg.pan = 180 - msg.pan
if msg.pan > 180:
msg.pan -= 360
if msg.pan < -180:
msg.pan += 360
msg.tilt = float(params['tilt'])
msg.zoom = float(params['zoom'])
msg.iris = float(params['iris'])
msg.focus = 0.0
if 'focus' in params:
msg.focus = float(params['focus'])
msg.autofocus = (params['autofocus'] == 'on')
self.axis.pub.publish(msg)
r.sleep()
def run(self):
r = rospy.Rate(10)
msg = Axis()
while True:
if not self.axis.timeout and ((rospy.Time.now() - self.axis.last_request).to_sec() > 1.0):
self.axis.timeout = True
self.axis.cmd(Twist(),reset_timeout=False)
conn = httplib.HTTPConnection(self.axis.hostname)
params = { 'query':'position' }
conn.request("GET", "/axis-cgi/com/ptz.cgi?%s" % urllib.urlencode(params))
response = conn.getresponse()
if response.status == 200:
body = response.read()
params = dict([s.split('=',2) for s in body.splitlines()])
msg.pan = float(params['pan'])
# Flip pan orient if the camera is mounted backwards and facing down
if self.axis.flip:
msg.pan = 180 - msg.pan
if msg.pan > 180:
msg.pan -= 360
if msg.pan < -180:
msg.pan += 360
msg.tilt = float(params['tilt'])
msg.zoom = float(params['zoom'])
msg.iris = float(params['iris'])
msg.focus = 0.0
if 'focus' in params:
msg.focus = float(params['focus'])
msg.autofocus = (params['autofocus'] == 'on')
self.axis.pub.publish(msg)
r.sleep()
def callback(self, config, level):
#self.speedControl = config.speed_control
# create temporary message and fill with data from dynamic reconfigure
temp_msg = Axis()
temp_msg.pan = config.pan
temp_msg.tilt = config.tilt
temp_msg.zoom = config.zoom
temp_msg.focus = config.focus
temp_msg.brightness = config.brightness
temp_msg.autofocus = config.autofocus
# check sanity and apply values
self.cmd(temp_msg)
# read sanitized values and update GUI
config.pan = self.msg.pan
config.tilt = self.msg.tilt
config.zoom = self.msg.zoom
config.focus = self.msg.focus
config.brightness = self.msg.brightness
config.autofocus = self.msg.autofocus
# update GUI with sanitized values
return config
def dynamicReconfigCallback(self, config, level):
#self.speedControl = config.speed_control
# create temporary message and fill with data from dynamic reconfigure
temp_msg = Axis()
temp_msg.pan = config.pan
temp_msg.tilt = config.tilt
temp_msg.zoom = config.zoom
temp_msg.focus = config.focus
temp_msg.brightness = config.brightness
temp_msg.autofocus = config.autofocus
# check sanity and apply values
self.cmdCallback(temp_msg)
# read sanitized values and update GUI
config.pan = self.msg.pan
config.tilt = self.msg.tilt
config.zoom = self.msg.zoom
config.focus = self.msg.focus
config.brightness = self.msg.brightness
config.autofocus = self.msg.autofocus
# update GUI with sanitized values
return config
def callback(self, config, level):
"""Deprecated."""
#self.speedControl = config.speed_control
if self._executing_reconfigure or (hasattr(self, '_camera_controller') and (self._camera_controller._executing_parameter_update or self._camera_controller._executing_reconfigure)):
return config
with self._reconfigure_mutex:
self._executing_reconfigure = True
# create temporary message and fill with data from dynamic reconfigure
command = Axis()
command.pan = config.pan
command.tilt = config.tilt
command.zoom = config.zoom
command.focus = config.focus
command.brightness = config.brightness
command.autofocus = config.autofocus
# check sanity and apply values
self.cmd(command)
# read sanitized values and update GUI
config.pan = command.pan
config.tilt = command.tilt
config.zoom = command.zoom
config.focus = self._camera_controller._focus
config.brightness = self._camera_controller._brightness
config.autofocus = self._camera_controller._autofocus
self._executing_reconfigure = False
# update GUI with sanitized values
return config
def ptz():
global image_1
#rospy.init_node('axis_ptz_random_move')
# self.pub = rospy.Publisher('/cmd', Axis)
pub = rospy.Publisher('/cmd', Axis)
pub1 = rospy.Publisher('/chatter', Image)
rospy.init_node('ptzcmd', anonymous=True)
bridge=CvBridge()
Message=Axis()
sub=rospy.Subscriber('/image_raw/compressed', CompressedImage,callback,queue_size=1)
r = rospy.Rate(40)
while not rospy.is_shutdown():
pan1=float(raw_input("Enter pan:"))
tilt1=float(raw_input("Enter tilt:"))
zoom1=float(raw_input("Enter zoom:"))
brightness1=float(raw_input("Enter brightness:"))
#auto=True
Message.pan=pan1
Message.tilt=tilt1
Message.zoom=zoom1
Message.brightness=brightness1
#Message.autofocus=auto
pub.publish(Message)
r.sleep()
def test_pan_tilt(self, hpos, vpos):
a = Axis()
#a.fields = Axis.SELECT_PAN + Axis.SELECT_TILT
a.pan = hpos # Horizontal position
a.tilt = vpos # Vertical position
print("Moving to %f %f " % (a.pan, a.tilt))
self.pub.publish(a)
def test_pan_tilt(self, hpos, vpos):
a = Axis()
#a.fields = Axis.SELECT_PAN + Axis.SELECT_TILT
a.pan = hpos # Horizontal position
a.tilt = vpos # Vertical position
print("Moving to %f %f " % (a.pan, a.tilt))
self.pub.publish(a)
key = ''
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
return key
def vels(speed,turn):
return "currently:\tspeed %s\tturn %s " % (speed,turn)
if __name__=="__main__":
settings = termios.tcgetattr(sys.stdin)
rospy.init_node('_teleop')
pub = rospy.Publisher('cmd', Axis, queue_size=5)
state = Axis()
state.pan=0
state.tilt=-45
pub.publish(state)
try:
while(1):
key = getKey()
print key
if key == "i":
state.tilt+=10
if state.tilt>-5:
state.tilt=-5
print state
pub.publish(state)
elif key == "k":
spline_pub = rospy.Publisher('cmd', Axis, queue_size=10)
r = rospy.Rate(1)
# Set signal to exit using terminal
signal.signal(signal.SIGINT, sigint_handler)
while not rospy.is_shutdown():
for t in xnew:
count += 1
print(count)
spline_time_now = t
spline_elapsed_time = spline_time_now - spline_time_last
spline_time_last = spline_time_now
if spline_elapsed_time == 0:
spline_elapsed_time = 1
r = rospy.Rate(1 / spline_elapsed_time)
axis_state.pan = f2(t)
spline_pub.publish(axis_state)
print(axis_state.pan)
r.sleep()
print("END CODE")
rospy.sleep(1)
def detect_and_draw(self, imgmsg):
self.computationalTime = rospy.Time.now()
#print 'number of KeyPoint objects skp', len(self.skp)
br = CvBridge()
temp = br.imgmsg_to_cv(imgmsg, "bgr8")
im_height = temp.height
im_length = temp.width
cv.ShowImage("view",temp)
cv.WaitKey(10)
template = np.asarray(temp)
tkp = self.detector.detect(template)
tkp,td = self.descriptor.compute(template, tkp)
if (self.init == 1):
self.im_ref = template
self.skp = tkp
self.sd = td
self.init = 0
command_cam = Axis()
command_cam.pan = 0
command_cam.tilt = 0
command_cam.zoom = 1
self.command_cam_pub.publish(command_cam);
#print 'number of KeyPoint objects tkp', len(tkp)
#print 'number of KeyPoint objects skp', len(self.skp)
flann_params = dict(algorithm=1, trees=4)
flann = cv2.flann_Index(self.sd, flann_params)
idx, dist = flann.knnSearch(td, 1, params={})
del flann
dist = dist[:,0]/2500.0
dist = dist.reshape(-1,).tolist()
idx = idx.reshape(-1).tolist()
indices = range(len(dist))
indices.sort(key=lambda i: dist[i])
dist = [dist[i] for i in indices]
idx = [idx[i] for i in indices]
h1, w1 = self.im_ref.shape[:2]
h2, w2 = template.shape[:2]
skp_final = []
for i, dis in itertools.izip(idx, dist):
if dis < self.threshold and self.skp[i].pt[1]*1.0 < 5*h1/6.0:
skp_final.append(self.skp[i])
else:
break
tkp_final = []
for i, dis in itertools.izip(range(len(idx)), dist):
if dis < self.threshold and self.skp[idx[i]].pt[1]*1.0 < 4*h1/6.0:
tkp_final.append(tkp[indices[i]])
else:
break
nWidth = w1+w2
nHeight = max(h1, h2)
hdif = (h1-h2)/2
newimg = np.zeros((nHeight, nWidth, 3), np.uint8)
newimg[hdif:hdif+h2, :w2] = template
newimg[:h1, w2:w1+w2] = self.im_ref
tkp_final
skp_final
#print 'number of KeyPoint objects in skp_final', len(skp_final)
#print 'number of KeyPoint objects in tkp_final', len(tkp_final)
for i in range(min(len(tkp_final), len(skp_final))):
pt_a = (int(tkp_final[i].pt[0]), int(tkp_final[i].pt[1]+hdif))
pt_b = (int(skp_final[i].pt[0]+w2), int(skp_final[i].pt[1]))
cv2.circle(newimg, pt_a, int(tkp_final[i].size),(160,32,240),1)
cv2.circle(newimg, pt_b, int(skp_final[i].size),(160,32,240),1)
cv2.line(newimg, pt_a, pt_b, (255, 0, 0))
cv.ShowImage("sift",cv.fromarray(newimg))
kp_array = sift_keypoints_array()
#kp_array.header = imgmsg.header
kp_array.header.frame_id = imgmsg.header.frame_id
kp_array.header.stamp = rospy.Time(0) #Time().now()
kp_array.skp = [sift_keypoint(*k.pt) for k in skp_final]
kp_array.tkp = [sift_keypoint(*k.pt) for k in tkp_final]
self.kp_pub.publish(kp_array)
self.computationalTime = rospy.Time.now() - self.computationalTime
self.Time_pub.publish(self.computationalTime)
key=cv.WaitKey(10) & 0xFF
if key == ord('d'):
self.im_ref = template
self.skp = tkp
self.sd = td
