class PanTilt(object):
def __init__(self):
print "Ready to deliver pan-tilt angles"
rospy.init_node("pan_tilt_server")
self._pan_angle = 0
self._tilt_angle = 0
self._pantilt_obj = Pantilt("/dev/ttyUSB2", 9600)
self._pantilt_obj.open()
self.s2 = rospy.Subscriber("/pan_tilt", Pan_tilt, self.handle_pan_tilt_change)
rospy.spin()
def handle_pan_tilt_change(self, data):
self._pantilt_obj.set_angle(data.pan, data.titl)
class PanTilt(object):
def __init__(self):
print "Ready to deliver pan-tilt angles"
rospy.init_node('pan_tilt_server')
self._pan_angle = 0
self._tilt_angle = 0
self._pantilt_obj = Pantilt('/dev/ttyUSB2',9600)
self._pantilt_obj.open()
self.s1 = rospy.Service('pan_tilt_srv', Pan_tilt , self.handle_pan_tilt_srv)
self.s2 = rospy.Subscriber("/caminos", Camino, self.handle_pan_tilt_change)
self.amcl_sus = rospy.Subscriber("amcl_pose", PoseWithCovarianceStamped, self.__amcl_pose_handler)
rospy.spin()
def __amcl_pose_handler(self, data):
actual_position = data.pose.pose
self.x_act = actual_position.position.x
self.y_act = actual_position.position.y
self.ang_act = Quat((actual_position.orientation.x,actual_position.orientation.y,\
actual_position.orientation.z,actual_position.orientation.w))
def handle_pan_tilt_srv(self,req):
print "Returning (%f + %f)"%(self._pan_angle,self._tilt_angle,)
return Pan_tiltResponse((self._pan_angle, self._tilt_angle))
def handle_pan_tilt_change(self,camino):
# TODO: compute optimal velocity
#print camino
#_pantilt_obj.set_velocity(200,200)
#Ang of the robot: self.ang_act.ra
#Ang of the pan: camino.puntos[0].angulo_pan
for punto in camino.puntos:
print punto.angulo_pan
print self.ang_act.ra
#_pantilt_obj.set_angle(self._pan_angle,self._tilt_angle)
ang_mov_pan = self.ang_act.ra - punto.angulo_pan
if ang_mov_pan > 158 or ang_mov_pan < -158:
rospy.loginfo("Pan want to move more that limit")
if ang_mov_pan < 0:
self._pantilt_obj.set_angle(-158,0)
else:
self._pantilt_obj.set_angle(158,0)
else:
self._pantilt_obj.set_angle(ang_mov_pan,0)
self._pan_angle = ang_mov_pan
time.sleep(10)
def __init__(self):
print "Ready to deliver pan-tilt angles"
rospy.init_node("pan_tilt_server")
self._pan_angle = 0
self._tilt_angle = 0
self._pantilt_obj = Pantilt("/dev/ttyUSB2", 9600)
self._pantilt_obj.open()
self.s2 = rospy.Subscriber("/pan_tilt", Pan_tilt, self.handle_pan_tilt_change)
rospy.spin()
def __init__(self):
print "Ready to deliver pan-tilt angles"
rospy.init_node('pan_tilt_server')
self._pan_angle = 0
self._tilt_angle = 0
self._pantilt_obj = Pantilt('/dev/ttyUSB2',9600)
self._pantilt_obj.open()
self.s1 = rospy.Service('pan_tilt_srv', Pan_tilt , self.handle_pan_tilt_srv)
self.s2 = rospy.Subscriber("/caminos", Camino, self.handle_pan_tilt_change)
self.amcl_sus = rospy.Subscriber("amcl_pose", PoseWithCovarianceStamped, self.__amcl_pose_handler)
rospy.spin()
def move_panTilt(self):
global ang_pan
global ang_tilt
pantilt = Pantilt('/dev/ttyUSB2',9600) #Change port and baudrate
pantilt.open()
pan = ang_pan
tilt = ang_tilt
pantilt.set_angle(pan,tilt) #pan,tilt
time.sleep(3)
pantilt.close()
def __init__(self):
print "Ready to deliver pan-tilt angles"
rospy.init_node('pan_tilt_server')
self._pan_angle = 0
self._tilt_angle = 0
self._pantilt_obj = Pantilt('/dev/ttyUSB2',9600)
self._pantilt_obj.open()
self.x_act = 0
self.y_act = 0
self.ang_act = 0
#self.s1 = rospy.Service('pan_tilt_srv', Pan_tilt_mess , self.handle_pan_tilt_srv)
self.s2 = rospy.Subscriber("/pan_tilt", Pan_tilt_mess, self.handle_pan_tilt_change)
#self.amcl_sus = rospy.Subscriber("amcl_pose", PoseWithCovarianceStamped, self.__amcl_pose_handler)
self.amcl_sus = rospy.Subscriber("final_orientation", Orientacion, self.__orientacion_handler)
self.s3 = rospy.Subscriber("/pan_tilt_tele", Pan_tilt_mess, self.handle_tele_op_pan_tilt)
self.reset_cero()
rospy.spin()
max_h,max_w = img.shape
self.img_center = [max_h,max_w]
self.image2 = self.process_rgb_image(img)
#print self.image.shape
# Display the result
#cv2.imshow("RGB Image", img)
except CvBridgeError, e:
print e
if __name__ == "__main__":
image = Imagen()
pantilt_obj = Pantilt('/dev/ttyUSB2',9600)
ang_pan = 0
ang_tilt = 0
while True:
if image.image is not None and image.image2 is not None:
#cv2.imshow('Video1', image.image)
cv2.imshow('Video2', image.image2
if (image.img_center[0] - image.object_center[0]) > 40:
if image.img_center[0] > image.object_center[0]:
ang_pan += 1.5
else:
ang_pan -= 1.5
else:
ang_pan = 0
from pantilt import Pantilt
import time
pantilt = Pantilt('/dev/ttyUSB2',9600) #Change port and baudrate
pantilt.open()
# Maximo del tilt (cabeza de arriba) es de +7.5,-11
#Se mueve a la posicion indicada, es decir si se coloca 0, se mueve a la posicion 0 grados
# No es que gire los grados indicados
#pantilt.set_angle(0,0)
#time.sleep(2)
pantilt.set_angle(20,0)
time.sleep(2)
pantilt.set_angle(0,0)
time.sleep(2)
pantilt.close()
class PanTilt(object):
def __init__(self):
print "Ready to deliver pan-tilt angles"
rospy.init_node('pan_tilt_server')
self._pan_angle = 0
self._tilt_angle = 0
self._pantilt_obj = Pantilt('/dev/ttyUSB2',9600)
self._pantilt_obj.open()
self.x_act = 0
self.y_act = 0
self.ang_act = 0
#self.s1 = rospy.Service('pan_tilt_srv', Pan_tilt_mess , self.handle_pan_tilt_srv)
self.s2 = rospy.Subscriber("/pan_tilt", Pan_tilt_mess, self.handle_pan_tilt_change)
#self.amcl_sus = rospy.Subscriber("amcl_pose", PoseWithCovarianceStamped, self.__amcl_pose_handler)
self.amcl_sus = rospy.Subscriber("final_orientation", Orientacion, self.__orientacion_handler)
self.s3 = rospy.Subscriber("/pan_tilt_tele", Pan_tilt_mess, self.handle_tele_op_pan_tilt)
self.reset_cero()
rospy.spin()
def handle_tele_op_pan_tilt(self,data):
self._pantilt_obj.set_angle(data.pan,data.tilt)
def __amcl_pose_handler(self, data):
actual_position = data.pose.pose
self.x_act = actual_position.position.x
self.y_act = actual_position.position.y
self.ang_act = Quat((actual_position.orientation.x,actual_position.orientation.y,\
actual_position.orientation.z,actual_position.orientation.w))
def __orientacion_handler(self, data):
self.ang_act = Quat((data.x,data.y,data.z,data.w))
def handle_pan_tilt_srv(self,req):
print "Returning (%f + %f)"%(self._pan_angle,self._tilt_angle,)
return Pan_tiltResponse((self._pan_angle, self._tilt_angle))
def transformar_angulo(self,angulo):
if angulo < 180:
return angulo
else:
return angulo - 360
def handle_pan_tilt_change(self,data):
# TODO: compute optimal velocity
#print camino
#_pantilt_obj.set_velocity(200,200)
#Ang of the robot: self.ang_act.ra
#Ang of the pan: data.pan
if data.reset == 1:
self._pantilt_obj.set_angle(0,0)
else:
print "Angulo que llego en radianes:",data.pan
print "Angulo del robot en radianes:",radians(self.ang_act.ra)
print "Angulo que llego en grados:",degrees(data.pan)
print "Angulo del robot en grados:",self.ang_act.ra
#_pantilt_obj.set_angle(self._pan_angle,self._tilt_angle)
ang_mov_pan = (self.ang_act.ra - degrees(data.pan))
ang_mov_pan = self.transformar_angulo(ang_mov_pan)
if ang_mov_pan > 158 or ang_mov_pan < -158:
rospy.loginfo("Pan want to move more that limit")
if ang_mov_pan < 0:
self._pantilt_obj.set_angle(-158,0)
else:
self._pantilt_obj.set_angle(158,0)
else:
self._pantilt_obj.set_angle(ang_mov_pan,0)
self._pan_angle = radians(ang_mov_pan)
#time.sleep(0.5)
def reset_cero(self):
self._pantilt_obj.set_angle(0,0)