~csiro-asl/csiro-asl-ros-drivers/trunk

« back to all changes in this revision

Viewing changes to csiro_asl_drivers/mdl_slm/nodes/mdlslm.py

  • Committer: Fred Pauling
  • Date: 2011-08-11 04:46:37 UTC
  • Revision ID: frederick.pauling@csiro.au-20110811044637-v2dj9y273yy7yabj
Add very basic MDL Scanning Laser Module (SLM) ROS driver to csiro-asl-ros-pkg

Show diffs side-by-side

added added

removed removed

Lines of Context:
 
1
#!/usr/bin/env python
 
2
 
 
3
#***********************************************************
 
4
#* Software License Agreement (BSD License)
 
5
#*
 
6
#*  Copyright (c) 2010, CSIRO Autonomous Systems Laboratory
 
7
#*  All rights reserved.
 
8
#*
 
9
#*  Redistribution and use in source and binary forms, with or without
 
10
#*  modification, are permitted provided that the following conditions
 
11
#*  are met:
 
12
#*
 
13
#*   * Redistributions of source code must retain the above copyright
 
14
#*     notice, this list of conditions and the following disclaimer.
 
15
#*   * Redistributions in binary form must reproduce the above
 
16
#*     copyright notice, this list of conditions and the following
 
17
#*     disclaimer in the documentation and/or other materials provided
 
18
#*     with the distribution.
 
19
#*   * Neither the name of the CSIRO nor the names of its
 
20
#*     contributors may be used to endorse or promote products derived
 
21
#*     from this software without specific prior written permission.
 
22
#*
 
23
#*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 
24
#*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 
25
#*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 
26
#*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 
27
#*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 
28
#*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 
29
#*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 
30
#*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 
31
#*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 
32
#*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 
33
#*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 
34
#*  POSSIBILITY OF SUCH DAMAGE.
 
35
#***********************************************************
 
36
 
 
37
# Author: David Haddon and Fred Pauling
 
38
#$Id: mdlslm.py 437 2011-07-14 04:17:07Z had067 $
 
39
 
 
40
 
 
41
from __future__ import division
 
42
import roslib; roslib.load_manifest("mdlslm")
 
43
import time
 
44
import numpy as np
 
45
from rospy.numpy_msg import numpy_msg # for numpy arrays in the messages
 
46
import struct
 
47
import rospy
 
48
import socket
 
49
import sys
 
50
from sensor_msgs.msg import LaserScan
 
51
 
 
52
 
 
53
class SLM:
 
54
  header_fmt=">cHHHB2s2s2s7s2s2s4sc"
 
55
  header_struct=struct.Struct(header_fmt)
 
56
  ls=numpy_msg(LaserScan)()
 
57
  pub=None
 
58
 
 
59
  def __init__(self,name="slm_laser", udp_addr="128.2.0.201", port=30, speed=10, frame="/slm", vis_spot=False, lo_res=False ):
 
60
    self.udp_addr=udp_addr
 
61
    self.udp_port=port
 
62
    #ros 
 
63
    rospy.init_node(name)
 
64
    self.pub=rospy.Publisher(name, numpy_msg(LaserScan))
 
65
    self.ls.range_max=655
 
66
    self.ls.range_min=0.01
 
67
    self.ls.header.frame_id=frame
 
68
    self.seq=0
 
69
    self.comms_fail=0
 
70
    
 
71
        
 
72
    #Open UDP port
 
73
    self.sock = socket.socket( socket.AF_INET, # Internet
 
74
                      socket.SOCK_DGRAM ) # UDP
 
75
    self.sock.settimeout(0.1)
 
76
    
 
77
    self.set_enable_reply(True)
 
78
    self.send_cmd("X1\n") # Set output to Binary
 
79
    self.set_speed(speed)
 
80
    self.set_visible(vis_spot)
 
81
    self.set_resolution(lo_res)
 
82
    self.send_cmd("S\n") # Start motor spinning
 
83
    self.set_laser_on(True)
 
84
  
 
85
  def shutdown(self):
 
86
    self.set_laser_on(False)
 
87
    self.set_visible(False)
 
88
    self.send_cmd("T\n") # Turn Motor off
 
89
    self.sock.close()
 
90
    print "\nLaser shutdown complete" 
 
91
 
 
92
  def send_cmd(self, cmd, recv_reply = True):
 
93
    tries_left=5
 
94
    self.sock.sendto( cmd, (self.udp_addr, self.udp_port) )    
 
95
    while (tries_left and recv_reply):
 
96
      try:
 
97
        data, addr = self.sock.recvfrom( 1500 ) # buffer size is 1500 bytes
 
98
        if len(data) > 0:
 
99
          self.comms_fail=0
 
100
          return data
 
101
      except socket.error as e:
 
102
        tries_left-=1
 
103
        self.comms_fail+=1
 
104
    if self.comms_fail > 20:
 
105
      raise Exception ('Comms Failed')
 
106
    return None
 
107
    
 
108
  def set_enable_reply(self, replies_on):
 
109
    self.send_cmd("H\n", recv_reply=False) # don't expect a response
 
110
    try:
 
111
      data, addr = self.sock.recvfrom( 1500 ) # buffer size is 1500 bytes
 
112
      if len(data) > 0:
 
113
        reply=True
 
114
    except socket.error as e:
 
115
      reply=False;  
 
116
    if reply==replies_on:
 
117
      return
 
118
    else:
 
119
      self.send_cmd("H\n")
 
120
      
 
121
    
 
122
  def set_laser_on(self, laser_on):
 
123
    if (laser_on):
 
124
      self.send_cmd("A\n")
 
125
      self.laser_on=laser_on
 
126
    else:
 
127
      self.send_cmd("B\n")
 
128
      self.laser_on=laser_on
 
129
  
 
130
  # Set Angular resolution to either 100th (fine) or 10th (course) degree
 
131
  def set_resolution(self, lo_res):
 
132
    if (lo_res):
 
133
      self.send_cmd("FT\n")
 
134
      self.fine_resolution=True
 
135
      print "Resolution set to course!"      
 
136
      
 
137
    else:
 
138
      self.send_cmd("FH\n")
 
139
      self.fine_resolution=False
 
140
 
 
141
      
 
142
 
 
143
  def set_visible(self, laser_on):
 
144
    if (laser_on):
 
145
      self.send_cmd("P\n")  # Turn Visible laser on
 
146
      self.visible_laser=True
 
147
    else:
 
148
      self.send_cmd("Q\n")  # Turn Visible laser off
 
149
      self.visible_laser=False
 
150
          
 
151
  # Set Head Speed to hz revolutions per second
 
152
  def set_speed (self, hz):
 
153
    cmd="I" + str(hz) + "\n" 
 
154
    self.speed=hz
 
155
    self.send_cmd(cmd)
 
156
    
 
157
  def recv_pkt(self):
 
158
    try:
 
159
      data, addr = self.sock.recvfrom( 1500 ) # buffer size is 1500 bytes
 
160
      if len(data) > 0:
 
161
        if ('$' in data):
 
162
          return data
 
163
    except socket.error as e:
 
164
      return "" 
 
165
 
 
166
  # Data reception stuff
 
167
  
 
168
  def parse_pkt(self, data):
 
169
       
 
170
    if len(data) > self.header_struct.size:
 
171
      
 
172
      hdr=self.header_struct.unpack_from(data)    
 
173
      if hdr[0] == '@':
 
174
        
 
175
        self.numpoints=hdr[1]
 
176
        self.angle_rate=hdr[2]/100
 
177
        self.angle_res=hdr[3]/1000
 
178
        self.num_points=hdr[4]
 
179
        self.hours=int(hdr[5])
 
180
        self.mins=int(hdr[6])
 
181
        self.secs=int(hdr[7])
 
182
        self.usecs=int(hdr[8])
 
183
        self.day=int(hdr[9])
 
184
        self.month=int(hdr[10])
 
185
        self.year=int(hdr[11])
 
186
        self.ls.time_increment=(1/((360*self.angle_rate)/self.angle_res))
 
187
        
 
188
        epoch_secs=int(time.mktime((self.year, self.month, self.day, self.hours,self.mins,self.secs,0,0,0)))
 
189
       
 
190
        if self.seq==0:
 
191
          # First time.. compute offset to allow for lack of real time on laser
 
192
          realtime=rospy.Time.now().to_sec()
 
193
          self.tm_offset=realtime-epoch_secs-self.usecs/1000000
 
194
          
 
195
        self.ls.header.stamp=rospy.Time(epoch_secs+self.tm_offset)    
 
196
          
 
197
      
 
198
        data_arr=np.frombuffer(data,offset=self.header_struct.size,dtype=np.uint8)
 
199
        
 
200
        if len(data_arr)==self.numpoints*8:
 
201
          
 
202
          data_arr=data_arr.reshape([-1,8])
 
203
          uint16be=np.dtype('>H')
 
204
          self.ranges=data_arr[:,1:3].copy().view(dtype=uint16be)
 
205
          self.signal=data_arr[:,3:5].copy().view(dtype=uint16be)
 
206
          self.angles=data_arr[:,5:7].copy().view(dtype=uint16be)
 
207
          return True
 
208
        else:
 
209
          print "Wrong size of data ", len(data_arr)
 
210
          return False
 
211
          
 
212
  def rosify_pkt(self):
 
213
    #everything need to be reversed as ROS expects counter clockwise scans
 
214
    
 
215
    self.ls.header.seq=self.seq
 
216
    self.ls.ranges=(self.ranges/100.0).astype(np.float32)
 
217
    self.ls.intensities=(self.signal).astype(np.float32)
 
218
    self.ls.angle_max=(360-self.angles[len(self.angles)-1]/100)/(180.0/np.pi)
 
219
    self.ls.angle_min=(360-self.angles[0]/100)/(180/np.pi)
 
220
    self.ls.angle_increment=-((self.angle_res)/(180.0/np.pi))
 
221
    
 
222
    self.pub.publish(self.ls)
 
223
    self.seq+=1
 
224
    
 
225
def is_arg_with_param(arg, prefix):
 
226
    if not arg.startswith(prefix):
 
227
        return False
 
228
    if not arg.startswith(prefix+"="):
 
229
        print "Expected '=' after "+prefix
 
230
        print
 
231
        usage(1)
 
232
    return True
 
233
 
 
234
def usage():
 
235
    print '''MDL SLM Driver
 
236
 
 
237
      CSIRO 2011
 
238
 
 
239
      David Haddon and Fred Pauling
 
240
 
 
241
      $Id: mdlslm.py 437 2011-07-14 04:17:07Z had067 $
 
242
 
 
243
      This is a quick hack driver.. do not use for production
 
244
 
 
245
      Usage:
 
246
 
 
247
      rosrun mdlslm mdlslm.py [-rate=<10>] [__name=<slm_laser>] [-port=<30>] [-ip=<128.2.0.201>] [-frame=</slm>] [-lo_res=0] [-visible=0]
 
248
 
 
249
      rate    :  Spin rate in Hz, should be between 1-20
 
250
      __name  :  Ros topic name
 
251
      port    :  Laser UDP port  (this will not change the laser settings!)
 
252
      ip      :  IP address of laser (once again.. will not change the laser settings)
 
253
      frame   :  Frame ID for LaserScan topic
 
254
      lo_res  :  set to 1 to use 10th degree increments, defaults to 100th degree
 
255
      visible :  Turn on the red visible laser'''
 
256
    
 
257
 
 
258
          
 
259
if __name__ == "__main__":
 
260
  udp_port=30
 
261
  udp_addr="128.2.0.201"
 
262
  slm=None
 
263
  name="slm_laser"
 
264
  speed=10
 
265
  frame="/slm"
 
266
  lo_res=0
 
267
  visible=0
 
268
 
 
269
  try:
 
270
    for arg in sys.argv[1:]: # Be very tolerant in case we are roslaunched.
 
271
      if arg == "--help":
 
272
        usage()
 
273
        exit(1)
 
274
      elif is_arg_with_param(arg, '-ip'):
 
275
        udp_addr = arg[4:]
 
276
      elif is_arg_with_param(arg, '-port'):
 
277
        udp_port = int(arg[6:])
 
278
      elif is_arg_with_param(arg, '-rate'):
 
279
        speed = int(arg[6:])
 
280
      elif is_arg_with_param(arg, '-frame'):
 
281
        frame= arg[7:]
 
282
      elif is_arg_with_param(arg, '-visible'):
 
283
        visible= int(arg[9:])        
 
284
      elif is_arg_with_param(arg, '-lo_res'):
 
285
        lo_res= int(arg[8:])        
 
286
      elif is_arg_with_param(arg, '__name:'):
 
287
        name = arg[8:]
 
288
        
 
289
    print 'Using topic name of %s' %(name)
 
290
    slm=SLM(name, udp_addr,udp_port, speed, frame, bool(visible), bool(lo_res))
 
291
 
 
292
    while not rospy.is_shutdown():
 
293
      data=slm.recv_pkt()
 
294
      #print "Len= " + str(len(data)) + "  Data=" +str(data)
 
295
      if slm.parse_pkt(data):
 
296
        slm.rosify_pkt()
 
297
        
 
298
  except rospy.ROSInterruptException: pass 
 
299
    
 
300
  except ValueError as parse_fail:
 
301
    print "Parse failed.. check your arguments\n : ", parse_fail
 
302
    usage()
 
303
  except Exception as ex:
 
304
    print "Caught an Error.. WTF? : ", ex
 
305
  if (slm != None):
 
306
    slm.shutdown()
 
307
  exit()
 
308