libplayerc_py example

An example of using libplayerc_py. More...

An example of using libplayerc_py.

1 #!/usr/bin/env python
2 
3 #Copyright (c) 2006, Brian Gerkey
4 #All rights reserved.
5 #
6 #Redistribution and use in source and binary forms, with or without
7 #modification, are permitted provided that the following conditions are met:
8 #
9 # * Redistributions of source code must retain the above copyright notice,
10 # this list of conditions and the following disclaimer.
11 # * Redistributions in binary form must reproduce the above copyright notice,
12 # this list of conditions and the following disclaimer in the documentation
13 # and/or other materials provided with the distribution.
14 # * Neither the name of the Player Project nor the names of its contributors
15 # may be used to endorse or promote products derived from this software
16 # without specific prior written permission.
17 #
18 #THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
19 #ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
20 #WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
21 #DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
22 #ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
23 #(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
24 #LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
25 #ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
26 #(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
27 #SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28 
29 import math
30 from playerc import *
31 
32 # Create a client object
33 c = playerc_client(None, 'localhost', 6665)
34 # Connect it
35 if c.connect() != 0:
36  raise playerc_error_str()
37 
38 # Create a proxy for position2d:0
39 p = playerc_position2d(c,0)
40 if p.subscribe(PLAYERC_OPEN_MODE) != 0:
41  raise playerc_error_str()
42 
43 # Retrieve the geometry
44 if p.get_geom() != 0:
45  raise playerc_error_str()
46 print 'Robot size: (%.3f,%.3f)' % (p.size[0], p.size[1])
47 
48 # Create a proxy for laser:0
49 l = playerc_laser(c,0)
50 if l.subscribe(PLAYERC_OPEN_MODE) != 0:
51  raise playerc_error_str()
52 
53 # Retrieve the geometry
54 if l.get_geom() != 0:
55  raise playerc_error_str()
56 print 'Laser pose: (%.3f,%.3f,%.3f)' % (l.pose[0],l.pose[1],l.pose[2])
57 
58 # Start the robot turning CCW at 20 deg / sec
59 p.set_cmd_vel(0.0, 0.0, 20.0 * math.pi / 180.0, 1)
60 
61 for i in range(0,30):
62  if c.read() == None:
63  raise playerc_error_str()
64 
65  print 'Robot pose: (%.3f,%.3f,%.3f)' % (p.px,p.py,p.pa)
66  laserscanstr = 'Partial laser scan: '
67  for j in range(0,5):
68  if j >= l.scan_count:
69  break
70  laserscanstr += '%.3f ' % l.ranges[j]
71  print laserscanstr
72 
73 # Now turn the other way
74 p.set_cmd_vel(0.0, 0.0, -20.0 * math.pi / 180.0, 1)
75 
76 for i in range(0,30):
77  if c.read() == None:
78  raise playerc_error_str()
79 
80  print 'Robot pose: (%.3f,%.3f,%.3f)' % (p.px,p.py,p.pa)
81  laserscanstr = 'Partial laser scan: '
82  for j in range(0,5):
83  if j >= l.scan_count:
84  break
85  laserscanstr += '%.3f ' % l.ranges[j]
86  print laserscanstr
87 
88 # Now stop
89 p.set_cmd_vel(0.0, 0.0, 0.0, 1)
90 
91 # Clean up
92 p.unsubscribe()
93 l.unsubscribe()
94 c.disconnect()
95 

Last updated 12 September 2005 21:38:45