Introduction
The purpose of this project was to design and build a group of three robots that could coordinate an effort to explore and map a room. There was a team of students each focusing on a different portion of the project. My portion of the project dealt with creating the map in real time to be shown on the graphical user interface. The idea was to be able to show the operator what the map would look like as it was being created.
Background
The Human-Robot Site Survey was a branch from a previous project that I also worked on. The project was an urban search and rescue where we simulated a robot and its surroundings. The object was to program the robot to maneuver the terrain and locate hazards, like fire, and victims in the area. We were then able to create a map with the obstacles in the room and where the victims were located. The design of the site survey project allowed us to start with the search and rescue program architecture and make modifications as needed.
Project Design
The robots used were manufactured by iRobot and had the iCreate developer platform on them. This allowed for loading an external program onto the robot and run it remotely. The mapping algorithm made use of the position data and the distance data from the laser sensor. For the urban search and rescue project each robot had its own scanning laser sensor that would return a distance. Since those sensors were well out of our budget, we had to come up with a different method to measure distance. Each robot has a camera and a laser with a line generating lens attached. The camera takes two pictures, one without the laser and one with the laser. It then subtracts the two images to give a better idea of where the laser line was in the photo. We then processed the image to determine the height of the laser line and then employed and algorithm to turn this information into a distance, giving use the ability to know how far away the object was from the robot. This information would then be sent through the mapping algorithm and be plotted on the screen.
The goal of the mapping algorithm was to find patterns in the laser data. Another team member in the search and rescue group wrote the code and I modified it to work for this project. It would look at two consecutive data sets and try to match them. By manipulating the second data set it would get it as close to the first data set as possible. This was done iteratively and since the fit would almost never be exact there was a set value that would cease calculations if the two data sets were within that value of each other. When the manipulated set was within tolerance those points were registered and stored. It was those points that were sent to the user interface to be plotted on the map.
A few modifications had to be made to the original code in order to make this happen. In the original version, the user interface got the data it needed directly from the robot. This data was not in global coordinates nor was it manipulated to minimize error. The architecture of this project did not lend itself well to this set up due to the fact the onboard sensors for the robots were inaccurate. The mapping algorithm helped reduce this error. I had to change where the data came from which involved modifying a few functions, writing new ones and adding variables. This can be seen in more detail in the attached code where the bold print indicates code that I added or modified.
Problems Encountered
There were several problems encountered while modifying the original code. The first problem came when the data set to be drawn was empty. The mapping algorithm takes a while to complete the manipulation so there is a gap between when the program begins the run and when points can be drawn. Since the user interface tries to draw points from the very beginning there were errors from it trying to access an empty matrix. This problem was solved by creating a variable called Registered that is false while the matrix is empty. By making the drawing dependent on this variable, the interface would not try to draw points while registered is false. Once there were registered points, Registered would be set to true and the interface would be able to draw the points on the screen.
The second problem involved the rest of the original program and specifically the communications. In order to run the simulation the program connects to the UT server and the image server. The UT server is the gaming platform used to create the maps and robots and the image server is how we get images from the robot. The connections between the code and those servers are not reliable so there were issues with keeping a connection long enough to be able to test the modified code. In fact, the connections never held for much more than 60 seconds which is not enough time to get a set of points registered to plot. We haven’t tried to fix the problem due to the fact that the communications code is extremely complex and makes diagnosing the problem difficult.
The final problem and probably the most devastating is one that only showed up after all the errors caused by my code had been eliminated. After about 30 seconds of running the simulation a Java heap space error occurs saying that the computer is out of memory. This causes the program to stop running and there is not much that can be done about it. This same thing happened in the search and rescue project and we believe that it was caused by the database having to write directly to the hard drive. The problem was never solved in the previous project and that is why it appeared again here. The database is how we stored all the data coming from the robot and it consumed a good portion of the processing power of the computer. The only way to fix this would be to revamp the way the database stores things which was not feasible in the time allotted.
Results/Conclusions
Although I was never able to test my modifications to the original program, I do know that the errors causing the program to crash were not caused by the code I added. I double checked my logic and there are no errors so in theory the changes should work. Since the real robots communicate differently than the virtual ones there is code written by other team members that would facilitate a test of this code. However, the code for parts of the total program is still separate from each other and has not been combined into one working piece. Once the parts have been compiled into the final product perhaps the code can be tested, but until then we only have what works in theory.
Note: This code is taken out of context in order to illustrate what I did to modify it.
Public Class DroneRobotController;
private static Matrix plotPoints = new Matrix(181,3); //variable that the registered points go into
/*
*Method that takes two consecutive data sets and manipulates one to get it close to the other.
* Originally did not return anything, was modified to return a Matrix object.
* Author: Zaid Towfic
*/
private Matrix register(RobotUtilities connection)
{
//Pull laser data from the database
LaserMessage lm = new LaserMessage();
LaserSet ls;
connection.search(”SELECT * FROM LASERDATA ORDER BY TIME DESC LIMIT 1;”, lm);
if(lm.size() > 0)
ls = (LaserSet) lm.getCollection().get(0);
else
return null;
float[] newData = ls.getData();
//Pull position data from the database
INSMessage ins = new INSMessage();
INSSet is = new INSSet();
do
{
connection.search(”SELECT * FROM INSSENSOR WHERE TIME >= “+ls.getTime()+” ORDER BY TIME ASC LIMIT 1;”, ins);
//System.out.println(ls.getTime());
}while(ins.size() <= 0);
is = (INSSet) ins.getCollection().get(0);
System.out.println(Math.abs(is.getTime() – ls.getTime()));
System.out.flush();
//Perform manipulations
Matrix data = new Matrix(3,newData.length);
double angle=0;
int i;
for(i=0;i<newData.length;i++)
{
if(newData[i] < 19.0)
{
data.set(0, i, newData[i]*Math.cos(angle));
data.set(1, i, newData[i]*Math.sin(angle));
data.set(2, i, 1);
}
else
{
data.set(0, 0, 0);
data.set(0, i, 0);
data.set(2, i, 1);
}
angle += 0.0174533;
}
Matrix rotation = new Matrix(3,3);
rotation.set(0, 0, Math.cos(is.getYaw()));
rotation.set(1, 1, Math.cos(is.getYaw()));
rotation.set(0, 1, Math.sin(is.getYaw()));
rotation.set(1, 0, -1*Math.sin(is.getYaw()));
rotation.set(2, 2, 1);
rotation.print(10, 10);
Matrix TT = newPoint(is.getLocationY(), is.getLocationX(), 0);
Matrix newPoints = (rotation.times(data));
newPoints = newPoints.plus(repmat(TT, data.getColumnDimension()));
//Store registered points in the database
String SQL;
for(i=0;i<newPoints.getColumnDimension();i++)
{
SQL = “INSERT INTO MAPXYZ (Robot_Name,X,Y,Z) VALUES (’”+name+”‘,”+newPoints.get(0, i)+”,”+newPoints.get(1, i)+”,”+newPoints.get(2, i)+”);”;
connection.getConnection().execute( SQL );
try{
get_wcsServer().send(RobotNames.base, SQL);
}catch(Exception e)
{
System.err.print(”FAILED TO SEND MESSAGE: “+SQL);
}
}
Float.parseFloat(po[1]), 0), data.getColumnDimension()));
//Tests to see if newPoints has data in it. If yes – registered=true, If no – registered=false
if(newPoints != null){
ShadowRobot.setRegistered(true);
}
else{
ShadowRobot.setRegistered(false);
}
return newPoints; //returns newPoints to be used in the OpenGL window
}
/**
* New method that returns newPoints matrix to use in the OpenGL map
*/
public static Matrix getNewPoints(){
return plotPoints;
}
Public Class ShadowRobot;
private double[][] dataPoints = new double[181][3]; //double array to hold matrix values
private static boolean registered = false; //variable to know when we have registered points
//sets the boolean variable registered
public static void setRegistered(boolean status){
registered = status;
}
/*
*Method to draw the laser points on the OpenGL window
*Originally took laser points from the robot to plot
*/
public void drawpoints(GL gl){
if (this.name == 1){
//Draw the robot icon
gl.glBegin(GL.GL_POINTS);
gl.glColor3f(0.99f,0.0f,0.0f);
gl.glVertex3f(0.0f,0.0f, 0.0f);
gl.glColor3f(0.86f,0.98f,0.79f);
//If there is data in the matrix go in here
if(registered){
//Copy matrix into 2D array for efficiency in drawing
dataPoints = DroneRobotController.getNewPoints().getArrayCopy();
int i;
for (i=0;i<=180;i+=1){
//Draw each point
gl.glVertex2d(dataPoints[i][0],dataPoints[i][1]);
}
}
gl.glEnd();
}
}
Good style. Adviced to friends
Posted by Pablo, on August 25th, 2008, at 3:50 am. #.