ptu46: ptu_action_server.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 import roslib; roslib.load_manifest('ptu46')
00003 import rospy
00004 import ptu_control.msg
00005 import actionlib
00006 from sensor_msgs.msg import JointState
00007 import threading
00008 import numpy as np
00009 
00010 class PTUControl(object):
00011         pan      = 0
00012         tilt     = 0
00013         pan_vel  = 0
00014         tilt_vel = 0
00015         state_lock = threading.Lock()
00016 
00017         def __init__(self):
00018                 # setup some parameters
00019                 self.tsmin = rospy.get_param('/ptu/min_tilt_speed', 4.0)
00020                 self.tsmax = rospy.get_param('/ptu/max_tilt_speed', 140.0)
00021                 self.psmin = rospy.get_param('/ptu/min_pan_speed' , 4.0)
00022                 self.psmax = rospy.get_param('/ptu/max_pan_speed' , 140.0)
00023                 self.pstep = rospy.get_param('/ptu/pan_step', 0.00089759763795882463)
00024                 self.tstep = rospy.get_param('/ptu/tilt_step', 0.00089759763795882463)
00025 
00026                 # setup the subscribers and publishers
00027                 rospy.Subscriber('state', JointState, self.cb_ptu_state)
00028                 self.ptu_pub = rospy.Publisher('cmd', JointState)
00029                 self.as_goto = actionlib.SimpleActionServer('SetPTUState', \
00030                      ptu_control.msg.PtuGotoAction, execute_cb=self.cb_goto)
00031                 self.as_reset  = actionlib.SimpleActionServer('ResetPtu', \
00032                          ptu_control.msg.PtuResetAction, execute_cb=self.cb_reset)
00033         
00034         def cb_goto(self, msg):
00035                 pan, tilt, pan_vel, tilt_vel = np.radians((msg.pan, msg.tilt, msg.pan_vel, msg.tilt_vel))
00036 
00037                 if not pan_vel:
00038                         pan_vel = self.psmax
00039                 if not tilt_vel:
00040                         tilt_vel = self.tsmax
00041 
00042                 self._goto(pan, tilt, pan_vel, tilt_vel)
00043 
00044                 result = ptu_control.msg.PtuGotoResult()
00045                 result.state.position = self._get_state()
00046                 self.as_goto.set_succeeded(result)
00047                 
00048         def cb_reset(self, msg):
00049                 self._goto(0,0, self.psmax, self.tsmax)
00050                 result = ptu_control.msg.PtuResetResult()
00051                 self.as_reset.set_succeeded(result)
00052 
00053         def _goto(self, pan, tilt, pan_vel, tilt_vel):
00054                 rospy.loginfo('going to (%s, %s)' % (pan, tilt))
00055                 msg_out = JointState()
00056                 msg_out.header.stamp = rospy.Time.now()
00057                 msg_out.name = ['head_pan_joint', 'head_tilt_joint']
00058                 msg_out.position = [pan, tilt]
00059                 msg_out.velocity = [pan_vel, tilt_vel]
00060                 self.ptu_pub.publish(msg_out)
00061                 # wait for it to get there
00062                 wait_rate = rospy.Rate(10)
00063                 while not self._at_goal((pan, tilt)) and not rospy.is_shutdown():
00064                         wait_rate.sleep()
00065 
00066         def _at_goal(self, goal):
00067                 return all(np.abs(np.array(goal) - (self.pan, self.tilt)) <= np.degrees((self.pstep, self.tstep)))
00068 
00069         def cb_ptu_state(self, msg):
00070                 self.state_lock.acquire()
00071                 self.pan, self.tilt = msg.position
00072                 self.state_lock.release()
00073 
00074         def _get_state(self):
00075                 self.state_lock.acquire()
00076                 pt = np.degrees((self.pan, self.tilt))
00077                 self.state_lock.release()
00078                 return pt
00079 
00080 if __name__ == '__main__':
00081         rospy.init_node('ptu46_action_server')
00082         PTUControl()
00083         rospy.spin()