diff --git a/optical_tracker/launch/test_calibration.launch b/optical_tracker/launch/test_calibration.launch index 0081fdb..531bd5b 100644 --- a/optical_tracker/launch/test_calibration.launch +++ b/optical_tracker/launch/test_calibration.launch @@ -1,7 +1,8 @@ - + + diff --git a/optical_tracker/package.xml b/optical_tracker/package.xml index c8706ed..883ec86 100644 --- a/optical_tracker/package.xml +++ b/optical_tracker/package.xml @@ -18,7 +18,7 @@ cv2 numpy dmx_msgs - dmx_driver + dmx_manager catkin diff --git a/optical_tracker/src/calib.py b/optical_tracker/src/calib.py index c1a8b97..ec6af82 100755 --- a/optical_tracker/src/calib.py +++ b/optical_tracker/src/calib.py @@ -5,9 +5,14 @@ import math from pygame import key from geometry_msgs.msg import Point -from dmx_driver.srv import SetChannel, SetChannelRequest +from dmx_msgs.srv import GetChannel, GetChannelRequest from widgets import PanGauge, TiltGauge +DEGREES_PER_PAN = 540 / 255 +DEGREES_PER_TILT = 270 / 255 +RADIANS_PER_PAN = DEGREES_PER_PAN * math.pi / 180 +RADIANS_PER_TILT = DEGREES_PER_TILT * math.pi / 180 + class Calibration(object): def __init__(self): pygame.init() @@ -21,23 +26,25 @@ def __init__(self): } self.screen = pygame.display.get_surface() #self.position_subscriber = rospy.Subscriber("/position", Point, self.position_cb) - #self.set_channel = rospy.ServiceProxy("/set_channel", SetChannel) + self.get_channel = rospy.ServiceProxy("/get_channel", GetChannel) def calibrate(self): pan = 0 tilt = 0 - print "Calibrating..." + print "Calibrating... Press ESC to end." inc_pan = 0.05 inc_tilt = 0.05 while not rospy.is_shutdown(): pygame.event.pump() keys = key.get_pressed() - pan = pan + inc_pan - if pan > math.pi or pan < 0: - inc_pan = -inc_pan - tilt = tilt + inc_tilt - if tilt > math.pi or tilt < 0: - inc_tilt = -inc_tilt + # pan = pan + inc_pan + # if pan > math.pi or pan < 0: + # inc_pan = -inc_pan + # tilt = tilt + inc_tilt + # if tilt > math.pi or tilt < 0: + # inc_tilt = -inc_tilt + pan = self.get_normalized_pan() + tilt = self.get_normalized_tilt() self.screen.fill(pygame.Color(0, 0, 0)) self.widgets['pan'].set_pan(pan) self.widgets['pan'].draw(self.screen, 0, 0) @@ -49,18 +56,25 @@ def calibrate(self): self.rate.sleep() def run(self): - print "Starting calibration..." - while not rospy.is_shutdown(): - pygame.event.pump() - keys = key.get_pressed() - if keys[pygame.K_c]: - self.calibrate() - self.rate.sleep() + #print "Press C to start calibration." + #while not rospy.is_shutdown(): + # pygame.event.pump() + # keys = key.get_pressed() + # if keys[pygame.K_c]: + self.calibrate() + # self.rate.sleep() def position_cb(self, point): assert(isinstance(point, Point)) self.latest_point = point + def get_normalized_pan(self): + pan = self.get_channel(1).value * DEGREES_PER_PAN + 180 + return pan * math.pi / 180 + + def get_normalized_tilt(self): + tilt = -self.get_channel(3).value * DEGREES_PER_TILT + 45 + 90 + return tilt * math.pi / 180 if __name__ == '__main__': rospy.init_node("test_calibration")