Freesteel Blog » Hokuyo laser scanning for a cave eventually

Hokuyo laser scanning for a cave eventually

Monday, September 23rd, 2013 at 2:27 pm Written by:

I just spent a grey sky weekend down in Bristol investigating the cave surveying technology potential of this Hokuyo UTM-30LX-EW, that I bought with my much diminished pocket money.

I bought this one, as opposed to the slightly cheaper UTM-30LX that most robot research teams go for, because it is IP67 rated, as opposed to IP64. The IP Code says the first 6 means “Dust tight”, and second digit 7 means protected against “Immersion up to 1m” rather than 4, protected against “splashing of water”.

I know what caves are like.

The disadvantage is that the EW runs off an ethernet lead, rather than a USB, which makes things difficult because it’s a protocol not designed for plugging wires directly into your computer not via a router (hence all that nonsense with cross-over cables).

So our first demos involved plugging the device into a router, connecting to that router from another computer and running their UrgBeni application to produce this output of a scan:

The laser scanner is hard-coded with the IP number 192.168.0.10 port 10940. If you plug the device directly into the ethernet socket of your laptop it won’t connect unless it’s got an ethernet IP number matching 192.168.0.10 up to the subnet mask of 255.255.255.0. In Windows you check your numbers with ipconfig and you reset the value with the command:

netsh interface ip set address "Local Area Connection" static 192.168.0.5

Honestly, you could waste hundreds of hours without this piece of vital information.

And so, we got it working locally and began piecing together the operation of the BSD licensed urg_library written in C++ that we were — fortunately — able to run in a Microsoft Visual Studio debugger. This code filled out some of the gaps in their 30 page protocol document.

We ported some of its operation into Python. Basically, all you need to connect to the device are these three lines:

s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.settimeout(2.0)
s.device.connect(("192.168.0.10", 10940))

The commands that you send in to the device are usually in the form of a string of letters and a linefeed.

For example, if you send in the three characters VVn using the Python code:

s.send('PPn')

The response looks like

PPn
00Pn
MODL:UTM-30LX-EW;In
DMIN:23;7n
DMAX:60000;Jn
ARES:1440;^n
AMIN:0;?n
AMAX:1080;Zn
AFRT:540;0n
SCAN:2400;Un
n

A response record is a set of lines that ends with two line-feeds. A quick and dirty piece of code to get this done — without worrying about messages getting concatenated by Nagle’s algorithm — is:

response = ''
while 'nn' not in response:
    response += s.recv(100)

The format of every response record is as follows. Line 1: echo the original command. Line 2: a 2 byte status code ’00’ followed by a checksum byte. Remaining lines: the response data.

The parameters in the device are, apparently, hard-coded. Which is good as it saves us setting them wrong. The character after those semi-colons above is a checksum again.

The scan rate is 2400rpm, which equates to 40 scans per second. Minimum measuring distance is 23mm (the radius of the device), maximum is 60m (the product spec says it’s 30m).

The resolution of each 360 degree rotational sweep of the laser is 1440 samples, or a quarter of a degree. However, the rear 90 degrees of the device isn’t scanned, so the samples only go from 0 up to 1080 inclusive, or three quarters of 1440. The zero measurement is not facing you, but is in fact 45 degrees rotated from you to your left. Imagine holding the device in your hand in front of you with its back facing your chest. There is a little gremlin on the device starting at your chest. The gremlin rotates 45 degrees clockwise to his right so he is now looking at your left shoulder. That’s position 0. He needs to spin another 135 degrees before he is facing directly ahead. This equates to 540 samples (a quarter of a degree per sample). And this explains the AFRT number above.

When it’s on the device is spinning all the time and getting quite hot. It’s going to be a challenge to carry enough battery power down the cave to make it run.

The special command string for extracting a single sample scan from the device is:

MD0000108001001n

The 0000 and 1080 are AMIN and AMAX. But following this is ’01’ for the cluster count (number of distance measurement to merge in each scan), ‘0’ the scan interval (number of scans to skip when asking for multiple scans), and ’01’ the number of multiple scans.

Are you with me so far?

The response comes back in 2 records. The first is the echo, with status code ’00’:

MD0000108001001n
00Pn
n

The second record is a measurement record (we’ve requested exactly one):

MD0000108001000
99b
9HiE_
0@Z0@P0@N0@N0@H0@H0A20A<0AB0AK0AN0AT0AY0Ac0Ad0Ak0B20B40B:0BA0BG0J
BQ0BT0BV0BY0Bl0Bm0C00C40C:0C;0CF0D80F40FB0FB0F60F60F60Ef0Ef0Ec0ED
_0E^0EV0EQ0EK0EK0EE0EE0ED0ED0E=0E50Dn0Dn0Dk0Dd0D_0D_0D_0D_0DY0DXU
...
=F0=C0=C0=C0<X0<90<80<50<80;n0;g0;f0;b0;b0;Y0;X0;Y0;X0;R0;K0;F0;H
F0;Q0;Q0;R0;R0;W0;W0;[0;_0;d0;h0;l0;n0;o0lt;1E

The first line is the command echo again, except that the final two characters, the ’00’, states the number of pending scans left in the request. We’ve requested one scan, so there are zero pending scans. In Python we decode this with int(line0[13:15]).

Line 1 (second line) is the status response (’99’ is normal) with check character.

Line 2 (third line) is the timestamp in 4 characters ‘9HiE’, with checksum byte ‘_’. These four characters encode 24 bits, or 6 bits each character. Obviously this device has been designed to be debugged with a text editor because they have gone to lengths to shift all the binary encodings away from the control codes in ascii. The 6 bits encodes a number between 0 and 63, which is shifted up 48 places to ascii ‘0’. So in Python we decode this line like so:

timems = ((ord(line2[0])-48)<<18) + ((ord(line2[1])-48)<<12) + 
          ((ord(line2[2])-48)<<6) + (ord(line2[3])-48)

All subsequent lines encode the block of distance measurements (with check codes and linefeeds inserted) in 3 byte (18 bit) codes. To strip off and concatenate these lines, do:

distancedata = ''.join(line[:-1]  for line in lines[3:])

Decode the distance data into a list as follows:

distances = [ ]
for i in range(2, len(distancedata), 3):
    v = ((ord(distancedata[i-2])-48)<<12) + ((ord(distancedata[i-1])-48)<<6) + 
         (ord(distancedata[i])-48)
    distances.append(v)

The resulting size of the distances array should be 1081, because both the 0 and 1080 indices are included. In millimetres, 24 bits is enough to encode 16km. The cheaper Hokuyo URG-04L-UG01 scanner gives 12 bits in 2 butes per measurements and has a range limit of 4m. I decided not to buy that one.

To convert these values into a sequence of XY points that we can plot as a curve, do:

curve = [ ]
for i in range(len(distances)):
    v = distances[i]
    th = math.radians(360*(i - 540.0)/1440.0)
    curve.append((-v * math.sin(th), v * math.cos(th)))

We built this into a loop and, with the power of my shambolic twistcodewiki system, plotted a sequence of contours above one another as we lifted up the device from the desk slowly.

You can see the monitor on the right as well as the corner of the wall above the desk.

Then we mounted the device sideways on a camera tripod using lots of rubber bands.

This is an image we were able to create when we set it going on continuous acquisition with the device standing in the corner of the bedroom.

Without a further sensor (eg an electronic compass or an inertial device) we can't tell the direction it is facing. Can I recover this direction by accurately locating the corner between the ceiling and the wall and triangulating?

Another issue is the problem of missing scan records. When I save the timestamp values (in milliseconds) into an array timesms, I get the following result:

>>> print [t-timesms[0]  for t in timesms]
[0, 25, 50, 75, 100, 125, 150, 200, 225, 250, 275, 300, 325, 350, 400, 
 425, 450, 475, 500, 525, 550, 600,...] 

Look at that! We're missing 175, 375, 475, etc. It seems as though the data is received through the ethernet not quite quickly enough to keep up with the 25 millisecond rate at which the approx 3.4kb records are generated by the device, so it just skips them.

We wasted a lot of time getting to the bottom of this (but didn't). It's not because of the speed of Python. It seems to be a problem in the ethernet. By logging the times on each s.recv(100) call, it appeared that the distance records were sent as three chunks of about 1kb each with up to a 10millisecond dwell between them. This means it takes 30milliseconds to receive a data record that was acquired in 25milliseconds. The 5milliseconds loss per record must eventually be accounted for by a slippage every 5 to 8 records, or otherwise the device would develop an unsustainable backlog of data in short order.

Does the software distributed with the machine perform any better?

Well, UrgBeni can log a stream of scan data to a file with the name '2013_09_22_12_11_36_857.ubh' which looks like:

[appName]
URG Benri
[appVersion]
1.5.3
[model]
UTM-30LX-EW
[frontStep]
540
  ...
[timestamp]
76031
[logtime]
2013-09-22 12:11:41.860
[scan]
1061;1056;1056;1051;1045;1037;1032;1030;1029;1019;1016;1016;1018;1027;1076;1083;1088;1111;...
[timestamp]
76056
[logtime]
2013-09-22 12:11:41.891
  ...

With the power of Python, we can extract and print these timestamps, like so:

x = open("2013_09_22_12_11_36_857.ubh").read()
ts = map(int, re.findall("[timestamp]s+(.*)", x))
print [t-ts[0]  for t in ts]

giving:

[0, 25, 50, 75, 125, 150, 175, 200, 250, 275, 300, 325, 375, 400, 425, 
450, 500, 525, 550, 575, 625, ...

Same problem.

I'll have to check with the manufacturers about this. I don't think it's experienced with the USB interface.

Finally, how to make this thing potentially portable.

We're not going to take a full laptop computer underground. Not only does it consume too much power, but it'll get smashed immediately.

So we need to run this from something smaller. We did manage to control it from an Arduino with an ethernet shield, but it looks like the 140kb of data this thing spews out per second is a bit too fast to handle -- even if you simply log the byte stream directly to the SD card and don't do any parsing. We're eyeing up a Beaglebone or RaspberryPi for this job now.

Still, it was a good bit of progress for one weekend, all things considered.

7 Comments

  • 1. DJ replies at 25th January 2014, 6:28 pm :

    Hey!
    That post was really helpful… I have been screwing my head up badly with some of the programs available on the URG site, but finally your post came in as my savior!:)
    Though I am not an avid programmer, switching to python makes sense to me, but like you mentioned towards the end of your post, you plan to use beaglebone, etc. for data processing, etc. So is it possible if you could guide me a l’il abt how to use the output data from this sensor for real time navigation control of any vehicle?
    & how practically viable do u think the system would be ? 😛

  • 2. Julian replies at 27th January 2014, 12:19 pm :

    Hi DJ,

    What model of Hok do you have?

    For us the timing is really important, and there is an extra wire from the Hok that gives a 1ms pulse on every rotation. The BeagleBone has inputs to read this signal, whereas a PC does not. The BeagleBone is also a lot more robust and portable.

    The advantage of Python is that it’s quick and easy to implement threads — which you need if you are to manage multiple data channels and responses.

    The BeagleBone has enough power to run Python, so we’re very happy.

    Here’s some simple code for reading and archiving out the data using threads.

    https://bitbucket.org/goatchurch/twistcodewiki/src/fde13b8536a1b081f80cf52825916815b2e1e6b8/woody/datalogimulaser.py?at=default

    It was a bit of hassle getting the correct pyUSB library installed. But you can log on to it and use it exactly like a normal computer.

  • 3. DJ replies at 17th February 2014, 9:49 am :

    Thanx a ton for your response Julian. 🙂
    I am using the same model as yours – UTM 30 LX EW. Actually I plan to control an AGV using this. I have too planned to use beaglebone, but I am not sure which model to go for.
    Currently I have placed my bet on Beagle Board ‘Black’ cuz it seems easy having all necessary peripherals, still which one would u suggest ?
    Also I am l’il sceptic of BBB sustaining a heavy processing/operation of about 10 Hrs/Day, you think it’ll last ? 😛
    Another issue I am facing right now is that when I use your codes i.e. :

    import socket
    s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    s.settimeout(2.0)
    s.device.connect((“192.168.0.10″, 10940))
    s.send(‘PPn’)
    response = ”
    while ‘nn’ not in response:
    response += s.recv(100)

    There is a always a ‘TIMEOUT ERROR’. I am unable to get why, cuz the sensor works well with hyper terminal as well as Urg_Benri app. Any ideas ?
    Also why did u use “PPn” command when the protocol says only “PP” ?? O.o
    I know that’s like too much stuff to answer in one go, but I would PLEASE request you to help me out. I have like skimmed thru almost entire internet & unable to understand what’s missing with these codes. 🙁

  • 4. Andrew replies at 5th March 2014, 3:47 pm :

    We are currently using BBB, not sure if it is the best choice, only time will tell.
    Sorry, not seen the timeout error.

    Andrew

  • 5. Nick replies at 16th August 2014, 4:02 pm :

    Very helpful. Thank you! I did indeed waste hours looking for the
    >netsh interface ip set address "Local Area Connection" static 192.168.0.5
    command for windows in order to use open a socket with python.

    For anyone thinking about running this on a Raspberry Pi, I have the connection up and running on my Pi. The Raspbian equivalent of windows netsh interface can be found in /etc/network/interfaces/> and edited with sudo nano. I found instructions for that here

  • 6. Engseba replies at 2nd December 2014, 11:20 pm :

    Hi,
    I Should use the laser scanner UTM 30 LX EW on board of UAV.
    But I do not know that hardware used to operate the laser on board.
    That is, I should acquire the point cloud and save on board.
    could you help me?

  • 7. Ibrahim replies at 11th March 2016, 7:17 pm :

    Hi,
    I am struggling to understand how to interpret the distance data and also change them into x,y coordinates. i need to learn how to do it by hand not using code, so that i can go ahead and implement it using other programming language.
    i figured if you explain to me what the code you written is and how it works and why it works that way and an example as well please, it will be very helpful. Especially the code below

    distances = [ ]
    for i in range(2, len(distancedata), 3):
    v = ((ord(distancedata[i-2])-48)<<12) + ((ord(distancedata[i-1])-48)<<6) +
    (ord(distancedata[i])-48)
    distances.append(v)

    curve = [ ]
    for i in range(len(distances)):
    v = distances[i]
    th = math.radians(360*(i – 540.0)/1440.0)
    curve.append((-v * math.sin(th), v * math.cos(th)))

    Your help would be really appreciated.

Leave a comment

XHTML: You can use these tags: <a href="" title=""> <abbr title=""> <acronym title=""> <blockquote cite=""> <code> <em> <strong>