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 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)
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 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 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 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 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 __init__(self):
rospy.init_node('axis_ptz_teleop')
self.enable_button = rospy.get_param('~enable_button', 1)
self.state = Axis(pan=220)
self.joy = None
self.pub = rospy.Publisher('cmd', Axis)
rospy.Subscriber("joy", Joy, self.joy_callback)
def run(self):
r = rospy.Rate(1)
self.msg = Axis()
while True:
self.queryCameraPosition()
self.publishCameraState()
self.publishJointStates()
r.sleep()
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 __init__(self):
rospy.init_node('ptz_control_joystick')
self.enable_button = rospy.get_param('~enable_button', 2)
self.pan_axis = rospy.get_param('~teleop_axis_pan', 1)
self.tilt_axis = rospy.get_param('~teleop_axis_tilt', 2)
self.state = Axis(pan=220)
self.joy = None
self.pub = rospy.Publisher('cmd', Axis)
rospy.Subscriber("joy", Joy, self.joy_callback)
def initialiseVariables(self):
self.joy = None
self.msg = Axis() # instantiate Axis message
self.msg.autofocus = True # autofocus is on by default
# sensitivities[0..5] corresponding to fwd, left, up, tilt right,
# tilt forwards, anticlockwise twist
self.mirror = False
self.mirror_already_actioned = False # to stop mirror flip-flopping
self.sensitivities = [120, -60, 40, 0, 0, 30]
self.deadband = [0.2, 0.2, 0.2, 0.2, 0.4, 0.4]
def __init__(self):
rospy.init_node('axis_twist_teleop')
self.enable_button = rospy.get_param('~enable_button', 1)
self.zero_button = rospy.get_param('~zero_button', 2)
self.scale_pan = rospy.get_param('~scale_pan_deg', 10)
self.scale_tilt = rospy.get_param('~scale_tilt_deg', 10)
self.state = Axis(pan=180, tilt=0, zoom=1)
self.joy = None
self.cmd_pub = rospy.Publisher('cmd', Axis)
self.twist_pub = rospy.Publisher('twist', Twist)
rospy.Subscriber("/joy", Joy, self.joy_callback)
def __init__(self):
self.enable_button = rospy.get_param('~enable_button', 1)
self.enable_turbo_button = rospy.get_param('~enable_turbo_button', 3)
self.camera_reset_pose_1 = rospy.get_param('~camera_reset_pose_1', 4)
self.camera_reset_pose_2 = rospy.get_param('~camera_reset_pose_2', 5)
self.axis_pan = rospy.get_param('~axis_pan', 0)
self.axis_tilt = rospy.get_param('~axis_tilt', 1)
self.axis_zoom = rospy.get_param('~axis_zoom', 6)
self.pub_time = rospy.Time.now()
self.pub = rospy.Publisher('cmd', Axis, queue_size=1)
self.state = Axis(pan=10)
self.state.brightness = 5000
self.pub.publish(self.state)
rospy.Subscriber("joy", Joy, self.joy_callback, queue_size=1)
def __init__(self):
self.current_state = Axis()
#PUB
self.pub_pantilts = rospy.Publisher('/pan_tilts',
PanTilts,
queue_size=10)
self.pub = rospy.Publisher('/imgg/compressed',
CompressedImage,
queue_size=10)
#SUB
self.sub_cam = rospy.Subscriber('/axis/state', Axis, self.update_state)
self.sub = rospy.Subscriber('axis/image_raw/compressed',
CompressedImage, self.callback)
print("Detect node started")
def __init__(self):
self.current_state = Axis()
self.lastcall = time.time()
self.mode = 'TRACK'
self.sens_pan = 1
#PUB
self.pub_cam = rospy.Publisher('axis/cmd', Axis)
#SUB
self.sub_cam = rospy.Subscriber('/axis/state', Axis, self.update_state)
self.sub_pantilts = rospy.Subscriber('/pan_tilts', PanTilts,
self.callback)
#http://wiki.ros.org/ROS/Tutorials/WritingServiceClient%28python%29#rospy_tutorials.2BAC8-Tutorials.2BAC8-WritingServiceClient.CA-27047f5058d93f3c972525600be4e0b4132b06a5_13
self.ser_mode = rospy.Service('/camera_mod', ChangeTrackingMode,
self.change_mode)
print("Tracker started in mode : " + self.mode)
def __init__(self, pos_filename):
self.current_state = Axis()
#SUB
self.sub_cam = rospy.Subscriber('/axis/state', Axis, self.update_state)
#http://wiki.ros.org/ROS/Tutorials/WritingServiceClient%28python%29#rospy_tutorials.2BAC8-Tutorials.2BAC8-WritingServiceClient.CA-27047f5058d93f3c972525600be4e0b4132b06a5_13
self.ser = rospy.Service('/record', Empty, self.save_position)
self.positions = []
print('ICI')
print(os.getcwd())
with open(pos_filename, 'r') as f:
self.positions = [x.split(',') for x in f.read().split('\n')[1:-1]]
self.pos_index = 0
self.data = open('positions_pan_tilt.txt', 'a')
self.next_pos()
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 __init__(self):
rospy.init_node('axis_ptz_random_move')
self.pub = rospy.Publisher('/cmd', Axis)
a = Axis()
self.pub.publish(a)
else:
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)
def test_zoom(self, z):
print("Zooming %f " % z)
a = Axis()
#a.fields = Axis.SELECT_ZOOM
a.zoom = z
self.pub.publish(a)
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
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
from axis_camera.msg import Axis
import sys, select, termios, tty
import time
if __name__=="__main__":
rospy.init_node('_init_pose_axis')
pub = rospy.Publisher('cmd', Axis, queue_size=5)
state = Axis()
state.pan=0
state.tilt=-83
time.sleep(1)
pub.publish(state)
time.sleep(1)
def test_zoom(self, z):
print("Zooming %f " % z)
a = Axis()
#a.fields = Axis.SELECT_ZOOM
a.zoom = z
self.pub.publish(a)
plt.plot(t, y, 'o-')
plt.legend(['data'], loc='best')
plt.show(block = False)
def sigint_handler(sig, frame):
print("Ending...")
sys.exit()
#ROS Axis control
rospy.init_node('axis_spline')
axis_state = Axis(pan=0)
command_state = Float64()
#spline_pub = rospy.Publisher('cmd', Axis, queue_size=10)
spline_pub = rospy.Publisher('pan_joint_position_controller/command', Float64, queue_size=10)
r = rospy.Rate( 1/(max_time/samples))
# Set signal to exit using terminal
signal.signal(signal.SIGINT, sigint_handler)
count = int()
count = 0
while not rospy.is_shutdown():
for point in y:
#plt.plot(x, y, 'o', xnew, f(xnew), '-', xnew, f2(xnew), 'o')
plt.plot(x, y, '-', xnew, f2(xnew), 'o')
plt.legend(['data', 'cubic'], loc='best')
plt.show(block=False)
def sigint_handler(sig, frame):
print("Ending...")
sys.exit()
#ROS Axis control
rospy.init_node('axis_spline')
axis_state = Axis(pan=0)
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)
