Una scheda hardware con due videocamerre per funzioni stereo

[Flavio58]

Starting with Surveyor's Stereo Vision System board

by Andrew Kirillov

The article gives introduction to Surveyor's Stereo Vision (SVS) board aimed for robotics applications. It provides also information on how to start writing C# applications to control the board remotely. Posted: November 23, 2009
Updated: December 15, 2009
Programming languages: C#
AForge.NET framework: 2.1.0


Sample application (sources) - 157K
Sample application (binaries) - 138K


Introduction

Being computer vision and robotics hobbyist I was always interested in building robots, which are not just equipped with some sensors, but also have cameras which may give them vision. And long before I started with robotics, my passion was always directed to robots which have two cameras, what makes them similar to most living creatures and what opens the opportunity for stereo vision applications.
If you are a scientists or an engineer working for a company doing stereo vision projects, it is not an issue to get some sort of stereo vision setup, which would allow you to experiment and work on your project. But if you are just a hobbyist it may be not so simple to get an affordable system for your hobby projects. In the past most hobbyists did not have much choice, but to build something on their own using two conventional USB web cameras (like I did in the past too). But nowadays we got more choice and instead of struggling with different issues of home-made stereo vision setup, we can get something dedicated for it. The simplest and cheapest 3D camera to try seems to be Minoru 3D web camera. This is actually something similar to what you can do on your own using two regular cameras, since Minoru 3D camera is just an assembly of 2 small USB cameras and a USB hub. Another available stereo camera is Bumblebee 2, which seems however to be directed mostly to professionals, not hobbyists. But these are just cameras, which has nothing to do with robotics (if you don't add additional parts of course). Luckily there is yet another available stereo system, which is aimed not just for computer vision, but also for robotics applications - Surveyor's Stereo Vision System.
Surveyor's Stereo Vision System

First I need to mention that Stereo Vision System (SVS) robotics board is not something close to Lego's NXT or Innovative First's VEX robotics kits (we can see it from SVS pictures obviously). It is not a kit full of building blocks and its box does not come with nice booklets providing building instructions. This is really a board with all the chips, pins, etc. put outside, so you need to be more careful with it and understand what you are going to build. It is aimed for a bit more experienced users. Having some experience with other robotics boards like Qwerk and Phidgets, I was not frightened by SVS too much. And a little note saying "Visit our website for more information", which was put into the box with the board, also was not too much surprising. Luckily the Surveyor's web site provides all the brief information required to start and the rest can be obtained from their technical support forum, which is really supported very nice - you will not stay waiting days to get a reply.
Note: Actually I would say that comparing robotics boards like Surveyor's SVS, Qwerk, Phidgets, etc. with RCX, NXT or VEX is not really fair. These are two different classes of robotics kits aimed to a bit different audience and to a bit different type of projects. Although the switch from one class of robotics kits to another may be a bit frightening for beginners, these robotics boards give more freedom and flexibility in building something really custom and interesting, which is not limited to blocks given with a kit.
So what is SVS? Basically it is a board with two cameras on it, WiFi module, motors' and servos' controllers and capability for connecting some additional sensors like ultrasonic sensors, compass, etc (the full spec may be obtained on the SVS's home page). It really looks like it has all we need to start building a robot equipped with stereo vision.
Although some robotics boards may allow connecting greater amount of different devices to them (like Qwerk, for example), there are two main things I really like in the SVS board - cameras and WiFi module are already on board. This makes a great benefit to it. You don't need to worry about which camera and external WiFi module to get and about different compatibility issues which may arise (like it was with the above mentioned Qwerk). Just power it on and you will get both communication and video working perfectly.
Another important thing to mention about SVS is that it has not just two cameras, but it is comprised of two SRV-1 Blackfin cameras. This is really important since each SRV-1 Blackfin camera is actually an independent device, which has its own processor, memory, etc. and each of them runs its own copy of firmware. The SVS board just puts them on the same board and makes a link between them making one SRV-1 camera operate as master device and the second as slave. The fact that SVS is based on SRV-1 means also that all the software (and firmware as well) written for SRV-1 is valid for SVS also. SVS just adds second SRV-1 module and a WiFi module to the same board.
SVS's software

The SVS board's package includes a CD with RoboRealm installation. "RoboRealm is a powerful robotic vision software application for use in computer vision, image processing, and robot vision tasks". So this is the first software to try and check if communication and video work fine without any issues. Personally I was a bit unlucky with this application and did not get good enough communication and video from SVS. Hopefully these were all the old bugs, which are fixed already. In addition there is number of other software applications available for the SVS board - most of them are available with sources written in different programming languages.
For those, who would like to write their own software, the SVS board provides three options:

• Write software on the firmware level. The board runs uCLinux and all the firmware's source code is open, so you may change them and make your custom build. If you go this road, then it may be worth of starting from here.
• Write application in C, which is executed using built-in C interpreter. If you go this road then you may start with SVS/SRV-1 C Interpreter documentation plus the next instructions for uploading the code to your board.
• Write applications, which manipulate SVS remotely. For doing this you will need to learn details of SRV-1 communication protocol.

The first two options are aimed for those applications, which require autonomous robots not depending on any other software running somewhere else. But this will require some knowledge and experience working on firmware level. The third option is aimed for robots, which are controlled remotely from PC, PDA, phone, etc. In this case you are welcome to choose any programming environment you like and you don't need changing anything at all on the board's side. However, going this road you may still need to know some basics about firmware updating with newer versions, which are released regularly.
As for me my aim was to control my robot remotely, so I've chosen the third approach in writing software. More of it, while building my robot (which I hope will be ready some day soon) and writing its software, I wanted to get set of reusable classes, which could be incorporated into AForge.NET framework and potentially be used by somebody else.
Communication protocol

So, if we are going to write our own software for remote manipulation of SVS board, then we need start with learning the communication protocol. The SRV-1 Control Protocol is very simple and is actually text based - all the commands are comprised mostly of ASCII characters (in some cases some binary values are also used, but only as parameters). To my mind it would be better if something different could be used as a protocol (something with strictly defined packet structure, which includes command code, status code, data size, data itself, etc), but this is the protocol which was chosen by Surveyor corporation initially because of simplicity to test it and then was kept for compatibility with future modifications.
And Surveyor corporation is really right in regards of testing the protocol - it is really simple to do it (at least one benefit of the protocol). You don't need any of the applications mentioned above, which already implement the protocol. All you need is a simple terminal client, which supports raw connection type (for example, PuTTY):
Note: By default SVS board is configured for an adhoc WiFi network with SSID "SRV1" (channel 6). Once you connect to the network, you can communicate with the board, which by default has IP address set to 169.254.0.10. Remember, that SVS board consists of two SRV-1 Blackfin cameras, which are independent devices. Each of these cameras may be accessed by connecting to different TCP port numbers. By default left camera (master) is accessible on port 10001 and the right camera (slave) is accessible on port 10002. So, just connect to the required port number and send commands defined by the protocol - SVS should reply to them.
The SVS board provides different type of replies for different commands. If reply has fixed length, then it always starts with '#' character and no CR+LF ("\r\n") characters are added to the end of it. If reply starts with "##", then it means that the reply may have variable length and it always contains CR+LF in the end. An important thing to note is that SVS does not reply at all to commands, which are not recognized by it - this should be kept in mind when you write your own software and handled properly.
Now, let's try to switch to some programming language (for example, C#) and get communication with SVS board from it. Usually the code will contain some sort of a loop, which gets new commands from a queue, sends them to the board, reads reply and provides it back to user (or discards it if user is not interested in the reply). The basic code may look something like this:
// number of bytes to read at once
int readSize = 1024;
// reply buffer size
int bufferSize = 1024 * 100;
// bytes actually read as a reply
int bytesRead = 0;

// buffer to read reply into
byte[] replyBuffer = new byte[bufferSize];

// create socket to use for communication
Socket socket = new Socket( AddressFamily.InterNetwork,
SocketType.Stream, ProtocolType.Tcp );
socket.ReceiveTimeout = 5000;
socket.SendTimeout = 1000;

// connect to SVS/SRV-1
socket.Connect( new IPEndPoint( IPAddress.Parse( ip ),
Convert.ToInt16( port ) ) );

// ...

while ( /* some condition to stop */ )
{
// get command/request from a queue, for example
// ..

// send request
socket.Send( request );

int bytesToRead = Math.Min( readSize, bufferSize );

// receive first portion
bytesRead = socket.Receive( replyBuffer, 0, bytesToRead, SocketFlags.None );

// check if response contains image
if ( ( bytesRead > 10 ) &&
( replyBuffer[0] == (byte) '#' ) &&
( replyBuffer[1] == (byte) '#' ) &&
( replyBuffer[2] == (byte) 'I' ) &&
( replyBuffer[3] == (byte) 'M' ) &&
( replyBuffer[4] == (byte) 'J' ) )
{
// extract image size
int imageSize = System.BitConverter.ToInt32( replyBuffer, 6 );

bytesToRead = imageSize + 10 - bytesRead;

// read the rest
while ( bytesToRead != 0 )
{
int read = socket.Receive( replyBuffer, bytesRead,
Math.Min( readSize, bytesToRead ), SocketFlags.None );

bytesRead += read;
bytesToRead -= read;
}
}
else if ( ( bytesRead >= 2 ) &&
( replyBuffer[0] == (byte) '#' ) &&
( replyBuffer[1] == (byte) '#' ) )
{
// make sure reply contains CR+LF
// if not, continue reading from socket like with image reply
// ...
}

// by this time the command reply should be in the buffer
// and size of reply should be equal to bytesRead

// notify user about reply from SVS
}
Of course the code above is not complete and does not handle different type of exceptions as well as it does not cover the mechanism of communication with user (getting commands and providing replies), but it gives the basic idea of writing communication loop with SVS board.
As I already mentioned before, my aim was to write some reusable classes to communicate with SRV-1 and SVS robots and to incorporate them into AForge.NET framework. The work was done and the classes together with documentation and sample applications are available in 2.1.0 version of the framework (there we can find complete communication code with SRV-1). Using classes from AForge.Robotics.Surveyor namespace it is not required to handle all the communication issues on your own, but just use provided methods to perform most of the robot's control. For many of the commands defined in SRV-1 Control Protocol, the classes provide dedicated methods, which do all the work for you. But in the case if some commands are not represented by corresponding methods, the classes provide methods to directly send a command to SRV-1 device and get a reply. Below is small sample of using SVS class for communication with SVS board:
SVS svs = new SVS( );
// connect to SVS board
svs.Connect( "169.254.0.10" );
// get version string
string version = svs.GetVersion( );
// set video resolution and quality
svs.SetQuality( 7 );
svs.SetResolution( SRV1.VideoResolution.Small );
// ...
The rest of the article is mostly dedicated to two things: 1) how to use AForge.NET framework to access different parts of SVS board (cameras, motors, servos, etc); 2) sharing some experience working with SVS board, which may include some found underwater stones.
Accessing video stream

There are multiple ways of getting access to video frames produced by SVS board. The simplest one is just to take single snap-shot image from one of the cameras using SVS.GetImage() method:
// get image from left camera
Bitmap leftImage = svs.GetImage( SVS.Camera.Left );
The method works fine, but it returns just a single image. This means that if you need to continuously receive video frames from SVS board, then you may need to organize your own loop preferably in a background worker thread, which does constant polling of SVS asking for new frames. To simplify things there is SRV1Camera class, which does all the work for you and provides new video frames through event handler:
// get left camera
SRV1Camera camera = svs.GetCamera( SVS.Camera.Left );

// set NewFrame event handler
camera.NewFrame += new NewFrameEventHandler( video_NewFrame );
// start the video source
camera.Start( );
// ...

private void video_NewFrame( object sender, NewFrameEventArgs eventArgs )
{
// get new frame
Bitmap bitmap = eventArgs.Frame;
// process the frame
}
The SRV1Camera class creates its own background thread and does all the dirty work. Using its FrameInterval property it is even possible to control video frame rate. Of course it will not allow you to receive more frames per second than camera can provide, but it may be used to set lower frame rates leaving camera a chance to do something else (like handle other commands). The class becomes even more useful because of its integration with the rest of AForge.NET framework's classes. Using VideoSourcePlayer control it is possibly not only to receive video frames continuously, but also display video in your application. Just put two VideoSourcePlayer controls on your form and assign left and right cameras to it:
// start left camera
SRV1Camera leftCamera = svs.GetCamera( SVS.Camera.Left );
leftCameraPlayer.VideoSource = leftCamera;
leftCameraPlayer.Start( );

// start right camera
SRV1Camera rightCamera = svs.GetCamera( SVS.Camera.Right );
rightCameraPlayer.VideoSource = rightCamera;
rightCameraPlayer.Start( );
The SVS class provides some additional video related methods, like SetResolution(), SetQuality() and FlipVideo(), which names are quite self describing. From the SRV-1 Control Protocol definition we can see that SRV-1 Blackfin cameras provide some more video related functions, but so far they are not exposed through dedicated methods. To my mind all the extra stuff may be done faster on the PC which manipulates the SVS board, so loading the board with more stuff to do will make it only less responsive to other commands. But if any of the extra functions is still required, then it may enabled by sending direct command to the SVS board:
// get direct access to the left SRV-1 Blackfin camera
SRV1 srv = svs.GetDirectAccessToSRV1( SVS.Camera.Left );
// send direct command "g1" - enable color segmentation
srv.Send( new byte[] { (byte) 'g', (byte) '1' } );
Manipulating motors

The interface set by SRV-1 Control Protocol defines two ways of controlling motors connected to SVS or SRV-1 robot - using set of predefined commands or using direct control of motors' power. The first method allows to send such predefined commands like "Drive forward", "Drive Left", "Drive Backward", "Increase Speed", "Stop", etc. These commands may be sent using ControlMotors() method of the AForge.NET framework:
svs.ControlMotors( SRV1.MotorCommand.DriveForward );
svs.ControlMotors( SRV1.MotorCommand.IncreaseSpeed );
svs.ControlMotors( SRV1.MotorCommand.DriveLeft );
svs.ControlMotors( SRV1.MotorCommand.Stop );
The second approach of controlling motors is direct control of both motors' speed. The approach is represented by RunMotors() method, which allows to set speed of each motor in the [0, 127] range (if it is required to run motors in opposite direction, then speed value should be negative) and also amount of time to run motors. To simplify things the framework also provides StopMotors() method, which is equivalent to calling RunMotors() with motors' speed set to 0. Below is the sample of usage these methods.
// drive forward for 1 second (100 * 10 = 1 sec)
svs.RunMotors( 70, 70, 100 );
// drive backward forever
svs.RunMotors( -70, -70, 0 );
// turn right (left wheel's speed is greater)
svs.RunMotors( 80, 60, 0 );
// finally stop
svs.StopMotors( );
Which of the two methods to use to control SRV-1/SVS robot's motors? As for me I personally always prefer direct access to motors, since it gives more flexibility and control over them. Experimenting with both approaches on my robot I've found few issues with predefined commands, which make them a bit not so nice in use.

• None of the predefined motors' commands will work unless at least one direct command is used. This means that at least StopMotors() method should be called once before calling ControlMotors() method.
• Predefined commands like "Increase Speed" and "Decrease Speed" don't have any effect until the next driving command. My expectation was that if I use "Drive Forward" command and then "Increase Speed", then the robot should continue driving with higher speed. But it will continue driving with the same speed as before. To get higher speed it is required to send "Drive Forward" command again (or any other driving command).
• It seems that speed settings, which are used for predefined motors' commands, are mostly suited for robots like the original SRV-1 robot, which is not very big. For bigger robots, like I had, the "Drive Forward" command may not have any effect at all unless few "Increase Speed" commands are sent. This fact brings all sorts of other issues. For example, the commands "Rotate left/right by 20 degrees" don't work accurately at all on bigger robots even with high speed settings, since motors' power and time for rotation were not calculated for such robots.

It is really hard to create generic predefined commands, which suite every robot needs, and the above issues show it clearly. So I always prefer direct control of motors if robot's API/protocol allows it.
There is one more motors' related method available in the framework (as well as command in SRV-1 Control Protocol). The EnableFailsafeMode() method allows to set which power values to apply to both motors in the case if robot does not receive any commands during last 2 seconds. The method is very useful for instructing robot to stop if communication was lost, for example.