$search
00001 #!/usr/bin/env python 00002 00003 #*********************************************************** 00004 #* Software License Agreement (BSD License) 00005 #* 00006 #* Copyright (c) 2010, CSIRO Autonomous Systems Laboratory 00007 #* All rights reserved. 00008 #* 00009 #* Redistribution and use in source and binary forms, with or without 00010 #* modification, are permitted provided that the following conditions 00011 #* are met: 00012 #* 00013 #* * Redistributions of source code must retain the above copyright 00014 #* notice, this list of conditions and the following disclaimer. 00015 #* * Redistributions in binary form must reproduce the above 00016 #* copyright notice, this list of conditions and the following 00017 #* disclaimer in the documentation and/or other materials provided 00018 #* with the distribution. 00019 #* * Neither the name of the CSIRO nor the names of its 00020 #* contributors may be used to endorse or promote products derived 00021 #* from this software without specific prior written permission. 00022 #* 00023 #* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00024 #* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00025 #* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00026 #* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00027 #* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00028 #* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00029 #* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 #* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 #* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 #* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00033 #* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00034 #* POSSIBILITY OF SUCH DAMAGE. 00035 #*********************************************************** 00036 00037 # Author: David Haddon and Fred Pauling 00038 #$Id: mdlslm.py 437 2011-07-14 04:17:07Z had067 $ 00039 00040 00041 from __future__ import division 00042 import roslib; roslib.load_manifest("mdlslm") 00043 import time 00044 import numpy as np 00045 from rospy.numpy_msg import numpy_msg # for numpy arrays in the messages 00046 import struct 00047 import rospy 00048 import socket 00049 import sys 00050 from sensor_msgs.msg import LaserScan 00051 00052 00053 class SLM: 00054 header_fmt=">cHHHB2s2s2s7s2s2s4sc" 00055 header_struct=struct.Struct(header_fmt) 00056 ls=numpy_msg(LaserScan)() 00057 pub=None 00058 00059 def __init__(self,name="slm_laser", udp_addr="128.2.0.201", port=30, speed=10, frame="/slm", vis_spot=False, lo_res=False ): 00060 self.udp_addr=udp_addr 00061 self.udp_port=port 00062 #ros 00063 rospy.init_node(name) 00064 self.pub=rospy.Publisher(name, numpy_msg(LaserScan)) 00065 self.ls.range_max=655 00066 self.ls.range_min=0.01 00067 self.ls.header.frame_id=frame 00068 self.seq=0 00069 self.comms_fail=0 00070 00071 00072 #Open UDP port 00073 self.sock = socket.socket( socket.AF_INET, # Internet 00074 socket.SOCK_DGRAM ) # UDP 00075 self.sock.settimeout(0.1) 00076 00077 self.set_enable_reply(True) 00078 self.send_cmd("X1\n") # Set output to Binary 00079 self.set_speed(speed) 00080 self.set_visible(vis_spot) 00081 self.set_resolution(lo_res) 00082 self.send_cmd("S\n") # Start motor spinning 00083 self.set_laser_on(True) 00084 00085 def shutdown(self): 00086 self.set_laser_on(False) 00087 self.set_visible(False) 00088 self.send_cmd("T\n") # Turn Motor off 00089 self.sock.close() 00090 print "\nLaser shutdown complete" 00091 00092 def send_cmd(self, cmd, recv_reply = True): 00093 tries_left=5 00094 self.sock.sendto( cmd, (self.udp_addr, self.udp_port) ) 00095 while (tries_left and recv_reply): 00096 try: 00097 data, addr = self.sock.recvfrom( 1500 ) # buffer size is 1500 bytes 00098 if len(data) > 0: 00099 self.comms_fail=0 00100 return data 00101 except socket.error as e: 00102 tries_left-=1 00103 self.comms_fail+=1 00104 if self.comms_fail > 20: 00105 raise Exception ('Comms Failed') 00106 return None 00107 00108 def set_enable_reply(self, replies_on): 00109 self.send_cmd("H\n", recv_reply=False) # don't expect a response 00110 try: 00111 data, addr = self.sock.recvfrom( 1500 ) # buffer size is 1500 bytes 00112 if len(data) > 0: 00113 reply=True 00114 except socket.error as e: 00115 reply=False; 00116 if reply==replies_on: 00117 return 00118 else: 00119 self.send_cmd("H\n") 00120 00121 00122 def set_laser_on(self, laser_on): 00123 if (laser_on): 00124 self.send_cmd("A\n") 00125 self.laser_on=laser_on 00126 else: 00127 self.send_cmd("B\n") 00128 self.laser_on=laser_on 00129 00130 # Set Angular resolution to either 100th (fine) or 10th (course) degree 00131 def set_resolution(self, lo_res): 00132 if (lo_res): 00133 self.send_cmd("FT\n") 00134 self.fine_resolution=True 00135 print "Resolution set to course!" 00136 00137 else: 00138 self.send_cmd("FH\n") 00139 self.fine_resolution=False 00140 00141 00142 00143 def set_visible(self, laser_on): 00144 if (laser_on): 00145 self.send_cmd("P\n") # Turn Visible laser on 00146 self.visible_laser=True 00147 else: 00148 self.send_cmd("Q\n") # Turn Visible laser off 00149 self.visible_laser=False 00150 00151 # Set Head Speed to hz revolutions per second 00152 def set_speed (self, hz): 00153 cmd="I" + str(hz) + "\n" 00154 self.speed=hz 00155 self.send_cmd(cmd) 00156 00157 def recv_pkt(self): 00158 try: 00159 data, addr = self.sock.recvfrom( 1500 ) # buffer size is 1500 bytes 00160 if len(data) > 0: 00161 if ('$' in data): 00162 return data 00163 except socket.error as e: 00164 return "" 00165 00166 # Data reception stuff 00167 00168 def parse_pkt(self, data): 00169 00170 if len(data) > self.header_struct.size: 00171 00172 hdr=self.header_struct.unpack_from(data) 00173 if hdr[0] == '@': 00174 00175 self.numpoints=hdr[1] 00176 self.angle_rate=hdr[2]/100 00177 self.angle_res=hdr[3]/1000 00178 self.num_points=hdr[4] 00179 self.hours=int(hdr[5]) 00180 self.mins=int(hdr[6]) 00181 self.secs=int(hdr[7]) 00182 self.usecs=int(hdr[8]) 00183 self.day=int(hdr[9]) 00184 self.month=int(hdr[10]) 00185 self.year=int(hdr[11]) 00186 self.ls.time_increment=(1/((360*self.angle_rate)/self.angle_res)) 00187 00188 epoch_secs=int(time.mktime((self.year, self.month, self.day, self.hours,self.mins,self.secs,0,0,0))) 00189 00190 if self.seq==0: 00191 # First time.. compute offset to allow for lack of real time on laser 00192 realtime=rospy.Time.now().to_sec() 00193 self.tm_offset=realtime-epoch_secs-self.usecs/1000000 00194 00195 self.ls.header.stamp=rospy.Time(epoch_secs+self.tm_offset) 00196 00197 00198 data_arr=np.frombuffer(data,offset=self.header_struct.size,dtype=np.uint8) 00199 00200 if len(data_arr)==self.numpoints*8: 00201 00202 data_arr=data_arr.reshape([-1,8]) 00203 uint16be=np.dtype('>H') 00204 self.ranges=data_arr[:,1:3].copy().view(dtype=uint16be) 00205 self.signal=data_arr[:,3:5].copy().view(dtype=uint16be) 00206 self.angles=data_arr[:,5:7].copy().view(dtype=uint16be) 00207 return True 00208 else: 00209 print "Wrong size of data ", len(data_arr) 00210 return False 00211 00212 def rosify_pkt(self): 00213 #everything need to be reversed as ROS expects counter clockwise scans 00214 00215 self.ls.header.seq=self.seq 00216 self.ls.ranges=(self.ranges/100.0).astype(np.float32) 00217 self.ls.intensities=(self.signal).astype(np.float32) 00218 self.ls.angle_max=(360-self.angles[len(self.angles)-1]/100)/(180.0/np.pi) 00219 self.ls.angle_min=(360-self.angles[0]/100)/(180/np.pi) 00220 self.ls.angle_increment=-((self.angle_res)/(180.0/np.pi)) 00221 00222 self.pub.publish(self.ls) 00223 self.seq+=1 00224 00225 def is_arg_with_param(arg, prefix): 00226 if not arg.startswith(prefix): 00227 return False 00228 if not arg.startswith(prefix+"="): 00229 print "Expected '=' after "+prefix 00230 print 00231 usage(1) 00232 return True 00233 00234 def usage(): 00235 print '''MDL SLM Driver 00236 00237 CSIRO 2011 00238 00239 David Haddon and Fred Pauling 00240 00241 $Id: mdlslm.py 437 2011-07-14 04:17:07Z had067 $ 00242 00243 This is a quick hack driver.. do not use for production 00244 00245 Usage: 00246 00247 rosrun mdlslm mdlslm.py [-rate=<10>] [__name=<slm_laser>] [-port=<30>] [-ip=<128.2.0.201>] [-frame=</slm>] [-lo_res=0] [-visible=0] 00248 00249 rate : Spin rate in Hz, should be between 1-20 00250 __name : Ros topic name 00251 port : Laser UDP port (this will not change the laser settings!) 00252 ip : IP address of laser (once again.. will not change the laser settings) 00253 frame : Frame ID for LaserScan topic 00254 lo_res : set to 1 to use 10th degree increments, defaults to 100th degree 00255 visible : Turn on the red visible laser''' 00256 00257 00258 00259 if __name__ == "__main__": 00260 udp_port=30 00261 udp_addr="128.2.0.201" 00262 slm=None 00263 name="slm_laser" 00264 speed=10 00265 frame="/slm" 00266 lo_res=0 00267 visible=0 00268 00269 try: 00270 for arg in sys.argv[1:]: # Be very tolerant in case we are roslaunched. 00271 if arg == "--help": 00272 usage() 00273 exit(1) 00274 elif is_arg_with_param(arg, '-ip'): 00275 udp_addr = arg[4:] 00276 elif is_arg_with_param(arg, '-port'): 00277 udp_port = int(arg[6:]) 00278 elif is_arg_with_param(arg, '-rate'): 00279 speed = int(arg[6:]) 00280 elif is_arg_with_param(arg, '-frame'): 00281 frame= arg[7:] 00282 elif is_arg_with_param(arg, '-visible'): 00283 visible= int(arg[9:]) 00284 elif is_arg_with_param(arg, '-lo_res'): 00285 lo_res= int(arg[8:]) 00286 elif is_arg_with_param(arg, '__name:'): 00287 name = arg[8:] 00288 00289 print 'Using topic name of %s' %(name) 00290 slm=SLM(name, udp_addr,udp_port, speed, frame, bool(visible), bool(lo_res)) 00291 00292 while not rospy.is_shutdown(): 00293 data=slm.recv_pkt() 00294 #print "Len= " + str(len(data)) + " Data=" +str(data) 00295 if slm.parse_pkt(data): 00296 slm.rosify_pkt() 00297 00298 except rospy.ROSInterruptException: pass 00299 00300 except ValueError as parse_fail: 00301 print "Parse failed.. check your arguments\n : ", parse_fail 00302 usage() 00303 except Exception as ex: 00304 print "Caught an Error.. WTF? : ", ex 00305 if (slm != None): 00306 slm.shutdown() 00307 exit() 00308