#!/usr/bin/python

import math
import time
from playerc import *

c = playerc_client(None, 'localhost', 6665)

if c.connect() != 0:
	raise Exception(playerc_error_str())

p = playerc_position2d(c,0)
if p.subscribe(PLAYERC_OPEN_MODE) != 0:
	raise Exception(playerc_error_str())

s = playerc_sonar(c,0)
if s.subscribe(PLAYERC_OPEN_MODE) != 0:
	raise Exception(playerc_error_str())

# Our robots take FOREVER for the sonar to start up.
# There's no way to say "are you ready yet?" so we just sleep
time.sleep(20)

#######################################################################
# Put your code after this comment
#######################################################################

while (1):
	#p.set_cmd_vel(0.0, 0.0, 20.0 * math.pi / 180.0, 1)
	if c.read() == None:
		raise Exception(playerc_error_str())


#######################################################################
# Keep your code above this comment
#######################################################################

s.unsubscribe()
p.unsubscribe()
c.disconnect()