import math
import time
from playerc import *

robo = playerc_client(None,'localhost', 6665)  
# connect client
connected=0
loop=0
while True:
    connected = robo.connect()
    print connected 
    loop += 1
    if (connected==0) or (loop>=100):
        break
    else:
        print loop
        time.sleep(0.2)
# check conncetion and display result
print connected 
if connected == 0:
    print 'connection established'   
    # create proxy for position 2d
    pos_proxy = playerc_position2d(robo,0)
    # create proxy for ranger 
    ran_proxy = playerc_ranger(robo,0)    
    #  subscribe to player
    pos_proxy.subscribe(PLAYERC_OPEN_MODE)
    #  subscribe to ranger
    ran_proxy.subscribe(PLAYERC_OPEN_MODE)    
    #get geom of position & laser
    pos_proxy.get_geom()
    ran_proxy.get_geom()
    robo.read()
    print 'Robot pose: (%.3f,%.3f,%.3f)' % (pos_proxy.px,pos_proxy.py,pos_proxy.pa)
    #print 'laser pose: (%.3f,%.3f,%.3f)' % (ran_proxy.px,ran_proxy.py,ran_proxy.pa)
    pos_proxy.set_cmd_vel(0.5,0.5,0.0,1)  
    robo.read()
    print 'laser range res %s' % ran_proxy.range_res
    print 'laser range count %s' % ran_proxy.ranges_count
    print 'laser max angle %s' % ran_proxy.max_angle
    print 'laser min angle %s' % ran_proxy.min_angle
    print 'laser max range %s' % ran_proxy.max_range
    print 'laser min range  %s' % ran_proxy.min_range
    print 'laser points %s' % ran_proxy.points
    print 'laser points count  %s' % ran_proxy.points_count    
    print 'laser element count %s' % ran_proxy.element_count
    print 'laser element poses  %s' % ran_proxy.element_poses  
    print 'laser element sizes  %s' % ran_proxy.element_sizes    
    #print 'laser angular res %s' % ran_proxy.angular_res
    #print 'laser device  poses  %s' % ran_proxy.device_pose      
    #print 'laser device  size  %s' % ran_proxy.device_size
    #print 'laser device  info  %s' % ran_proxy.info  
#   print 'laser device  intensities  %s' % ran_proxy.intensities 
#    print 'laser device  intensity count  %s' % ran_proxy.intensities_count          
    #print ran_proxy.ranges(1) 
    laserscanstr = 'Partial laser scan: '
    for j in range(0,5):
        if j >= ran_proxy.points_count :
            break
        laserscanstr += '%.3f ' % ran_proxy.ranges[j]
        print laserscanstr    
    
    #clean up
ran_proxy.unsubscribe()
pos_proxy.unsubscribe()
#robo.unsubscribe()
robo.disconnect()