|
Example 2: Tier-Scalable Planetary Reconnaissance
Mobile Robot Agent
<?xml version="1.0"?>
<!DOCTYPE myMessage SYSTEM "mobilec.dtd">
<MOBILEC_MESSAGE>
<MESSAGE message="MOBILE_AGENT">
<MOBILE_AGENT>
<AGENT_DATA>
<NAME>mobagent1</NAME>
<OWNER>IEL</OWNER>
<HOME>monkey.engr.ucdavis.edu:5050</HOME>
<TASKS task="3" num="0">
<TASK num="0" complete="0" server="mouse2.engr.ucdavis.edu:5050" code_id="1" />
<TASK num="1" complete="0" server="fish.engr.ucdavis.edu:5050" code_id="2" />
<TASK num="2" complete="0" server="monkey.engr.ucdavis.edu:5050" code_id="3" />
<AGENT_CODE id="1">
<![CDATA[
#include <cv.h>
#include <highgui.h>
#define IMAGEWIDTH 640
#define IMAGEHEIGHT 480
int nSideContours = 0;
int nTargetContours = 0;
CvRect *targetObjectsROI;
CvRect *sideObjectsROI;
int targetObjectsROI_X[10] = {0};
int targetObjectsROI_Y[10] = {0};
int targetObjectsROI_Width[10] = {0};
int sideObjectsROI_X[10] = {0};
int sideObjectsROI_Y[10] = {0};
int sideObjectsROI_Width[10] = {0};
int
main (void)
{
CvCapture *capture = NULL;
IplImage
*sideImage = NULL,
*sideImageGray = NULL,
*sideImageRed = NULL,
*sideImageBlue = NULL,
*sideImageGreen = NULL,
*sideImageRBDiff = NULL,
*sideImageRGDiff = NULL,
*sideImageSum = NULL;
int *sideObjectsIndex;
CvRect *sideObjectsRotatedROI;
CvMemStorage* contoursStorage = NULL;
CvSeq* contoursSequence = NULL;
int i;
// ------ Load image --------
// Uncomment the following to caputer from the camera
/*
capture = cvCaptureFromCAM(0);
sideImage = cvQueryFrame(capture);
*/
sideImage = cvLoadImage("/tmp/sideview.png", 1);
// ------ Find Field Objects -------
// Convert to Gray Scale
sideImageGray = cvCreateImage (cvGetSize(sideImage), 8, 1);
cvConvertImage (sideImage, sideImageGray, CV_BGR2GRAY);
// Smooth and threshold
cvSmooth(sideImageGray, sideImageGray, CV_GAUSSIAN, 9, 9, 0, 0);
cvThreshold(sideImageGray, sideImageGray, 220, 255, CV_THRESH_BINARY);
cvSmooth(sideImageGray, sideImageGray, CV_GAUSSIAN, 5, 5, 0, 0);
// Find contours and save locations
contoursStorage = cvCreateMemStorage(0);
nSideContours = cvFindContours(sideImageGray, contoursStorage,
&contoursSequence, sizeof(CvContour),
CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE, cvPoint(0,0));
sideObjectsROI = (CvRect*)malloc(sizeof(CvRect)*nSideContours);
for(i=0;i<nSideContours;i++)
{
sideObjectsROI[i] = cvBoundingRect(contoursSequence, 0);
contoursSequence = contoursSequence->h_next;
}
cvClearMemStorage(contoursStorage);
// ------ End Find Field Objects -------
// ------ Find Target Objects -------
sideImageRed = cvCreateImage(cvGetSize(sideImage), 8, 1);
sideImageBlue = cvCreateImage(cvGetSize(sideImage), 8, 1);
sideImageGreen = cvCreateImage(cvGetSize(sideImage), 8, 1);
sideImageRGDiff = cvCreateImage(cvGetSize(sideImage), 8, 1);
sideImageRBDiff = cvCreateImage(cvGetSize(sideImage), 8, 1);
sideImageSum = cvCreateImage(cvGetSize(sideImage), 8, 1);
// Split color componenets
cvSplit(sideImage, sideImageBlue, sideImageGreen, sideImageRed, NULL);
cvSub(sideImageRed, sideImageGreen, sideImageRGDiff, NULL);
cvSub(sideImageRed, sideImageBlue, sideImageRBDiff, NULL);
cvAdd(sideImageRGDiff, sideImageRBDiff, sideImageSum, NULL);
// Threshold and smooth
cvThreshold(sideImageSum, sideImageSum, 250, 255, CV_THRESH_BINARY);
cvSmooth(sideImageSum, sideImageSum, CV_GAUSSIAN, 5, 5, 0, 0);
contoursStorage = cvCreateMemStorage(0);
nTargetContours = cvFindContours(sideImageSum, contoursStorage,
&contoursSequence, sizeof(CvContour),
CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE, cvPoint(0,0));
targetObjectsROI = (CvRect*)malloc(sizeof(CvRect)*nTargetContours);
for(i=0;i<nTargetContours;i++)
{
targetObjectsROI[i] = cvBoundingRect(contoursSequence, 0);
contoursSequence = contoursSequence->h_next;
}
cvClearMemStorage(contoursStorage);
// ------ End Find Target Objects -------
for(i=0;i<nSideContours;i++)
{
sideObjectsROI_X[i] = (int) sideObjectsROI[i].x;
sideObjectsROI_Y[i] = (int) sideObjectsROI[i].y;
sideObjectsROI_Width[i] = (int) sideObjectsROI[i].width;
}
for(i=0;i<nTargetContours;i++)
{
targetObjectsROI_X[i] = (int) targetObjectsROI[i].x;
targetObjectsROI_Y[i] = (int) targetObjectsROI[i].y;
targetObjectsROI_Width[i] = (int) targetObjectsROI[i].width;
}
mc_AgentVariableSave(mc_current_agent, "nSideContours");
mc_AgentVariableSave(mc_current_agent, "nTargetContours");
mc_AgentVariableSave(mc_current_agent, "sideObjectsROI_X");
mc_AgentVariableSave(mc_current_agent, "sideObjectsROI_Y");
mc_AgentVariableSave(mc_current_agent, "sideObjectsROI_Width");
mc_AgentVariableSave(mc_current_agent, "targetObjectsROI_X");
mc_AgentVariableSave(mc_current_agent, "targetObjectsROI_Y");
mc_AgentVariableSave(mc_current_agent, "targetObjectsROI_Width");
cvReleaseImage (&sideImage);
cvReleaseImage (&sideImageGray);
cvReleaseImage (&sideImageRed);
cvReleaseImage (&sideImageBlue);
cvReleaseImage (&sideImageGreen);
cvReleaseImage (&sideImageRBDiff);
cvReleaseImage (&sideImageRGDiff);
cvReleaseImage (&sideImageSum);
return 0;
}
]]>
</AGENT_CODE>
<AGENT_CODE id="2">
<![CDATA[
#include <cv.h>
#include <highgui.h>
#include <gaul.h>
#define ROTATIONANGLE (25.0*3.1415/180.0)
#define IMAGEWIDTH 640
#define IMAGEHEIGHT 480
#define K3RADIUS 25 // In pixels as seen from the aerial camera
#define K3XPOSITION 264 // In pixels as seen from the aerial camera
#define K3YPOSITION 109 // In pixels as seen from the aerial camera
int topMap[64][48] = {0};
char** robotPath = NULL;
int GAPathPlanning();
int wayPointsX[50];
int wayPointsY[50];
int wayPathSize = 0;
int
main (void)
{
CvCapture *capture = NULL;
IplImage
*topImage = NULL,
*topImageGray = NULL;
int *nSideContours;
int *nTargetContours;
int *sideObjectsROI_X;
int *sideObjectsROI_Y;
int *sideObjectsROI_Width;
int *targetObjectsROI_X;
int *targetObjectsROI_Y;
int *targetObjectsROI_Width;
int *sideObjectsIndex;
int *targetSideObjectsIndex;
CvRect *sideObjectsRotatedROI;
int nTopContours = 0;
CvMemStorage* contoursStorage = NULL;
CvSeq* contoursSequence = NULL;
CvRect *topObjectsROI;
int *targetTopObjectsIndex;
int i;
// ------ Load image --------
// Uncomment the following to caputer from the camera
/*
capture = cvCaptureFromCAM(0);
topImage = cvQueryFrame(capture);
*/
topImage = cvLoadImage("/tmp/topview.png", 1);
// ------ Find Field Objects -------
// Grab grayscale
topImageGray = cvCreateImage (cvGetSize(topImage), 8, 1);
cvConvertImage (topImage, topImageGray, CV_BGR2GRAY);
// Smooth and threshold
cvSmooth(topImageGray, topImageGray, CV_GAUSSIAN, 5, 5, 0, 0);
cvThreshold(topImageGray, topImageGray, 254, 255, CV_THRESH_BINARY);
cvSmooth(topImageGray, topImageGray, CV_GAUSSIAN, 7, 7, 0, 0);
// Find contours and save locations
contoursStorage = cvCreateMemStorage(0);
nTopContours = cvFindContours(topImageGray, contoursStorage,
&contoursSequence, sizeof(CvContour),
CV_RETR_TREE, 5, cvPoint(0,0));
topObjectsROI = (CvRect*)malloc(sizeof(CvRect)*nTopContours);
for(i=0;i<nTopContours;i++)
{
topObjectsROI[i] = cvBoundingRect(contoursSequence, 0);
contoursSequence = contoursSequence->h_next;
}
cvClearMemStorage(contoursStorage);
nSideContours
= (int*)mc_AgentVariableRetrieve(mc_current_agent, "nSideContours", 0);
nTargetContours
= (int*)mc_AgentVariableRetrieve(mc_current_agent, "nTargetContours", 0);
sideObjectsROI_X
= (int*)mc_AgentVariableRetrieve(mc_current_agent, "sideObjectsROI_X", 0);
sideObjectsROI_Y
= (int*)mc_AgentVariableRetrieve(mc_current_agent, "sideObjectsROI_Y", 0);
sideObjectsROI_Width
= (int*)mc_AgentVariableRetrieve(mc_current_agent, "sideObjectsROI_Width", 0);
targetObjectsROI_X
= (int*)mc_AgentVariableRetrieve(mc_current_agent, "targetObjectsROI_X", 0);
targetObjectsROI_Y
= (int*)mc_AgentVariableRetrieve(mc_current_agent, "targetObjectsROI_Y", 0);
targetObjectsROI_Width
= (int*)mc_AgentVariableRetrieve(mc_current_agent, "targetObjectsROI_Width", 0);
// ------ Correlate Targets with Field Objects -------
targetSideObjectsIndex = (int*)malloc(sizeof(int)*(*nTargetContours));
for(i=0; i<(*nTargetContours); i++)
{
int j = 0;
int leftSide = 0;
int rightSide = 0;
int objectXCenter;
int minimumDistance = 9999;
leftSide = targetObjectsROI_X[i];
rightSide = targetObjectsROI_X[i]+targetObjectsROI_Width[i];
for(j=0; j<*nSideContours; j++)
{
objectXCenter = sideObjectsROI_X[j] + sideObjectsROI_Width[j]/2;
if(objectXCenter > leftSide && objectXCenter < rightSide &&
sideObjectsROI_Y[j] < targetObjectsROI_Y[i])
{
if((targetObjectsROI_Y[i] - sideObjectsROI_Y[j]) < minimumDistance)
{
minimumDistance = (targetObjectsROI_Y[i] - sideObjectsROI_Y[j]);
targetSideObjectsIndex[i] = j;
}
}
}
}
// ---- End Correlate Targets with Field Objects -----
// ------ Sort Objects -------
sideObjectsRotatedROI = (CvRect*)malloc(sizeof(CvRect)*(*nSideContours));
sideObjectsIndex = (int*)malloc(sizeof(int)*(*nSideContours));
for(i=0; i<*nSideContours; i++)
{
int X, Y;
X = -(IMAGEWIDTH/2 - sideObjectsROI_X[i]);
Y = (IMAGEHEIGHT/2 - sideObjectsROI_Y[i]);
sideObjectsRotatedROI[i] =
cvRect(sideObjectsROI_X[i], sideObjectsROI_Y[i],
sideObjectsROI_Width[i], 0);
sideObjectsIndex[i] = i;
sideObjectsRotatedROI[i].x = X * cos(ROTATIONANGLE) - Y * sin(ROTATIONANGLE);
sideObjectsRotatedROI[i].y = X * sin(ROTATIONANGLE) + Y * cos(ROTATIONANGLE);
sideObjectsRotatedROI[i].x = (IMAGEWIDTH/2 + sideObjectsRotatedROI[i].x);
sideObjectsRotatedROI[i].y = (IMAGEHEIGHT/2 - sideObjectsRotatedROI[i].y);
}
// we assume nTopContours == nSideContours
// sort objects
for(i=0;i<nTopContours-1;i++)
{
CvRect tempRect;
int tempIndex;
int j;
// Sort by y
for(j=i+1;j<nTopContours;j++)
{
if(sideObjectsRotatedROI[j].y < sideObjectsRotatedROI[i].y)
{
tempRect = sideObjectsRotatedROI[i];
sideObjectsRotatedROI[i] = sideObjectsRotatedROI[j];
sideObjectsRotatedROI[j] = tempRect;
tempIndex = sideObjectsIndex[i];
sideObjectsIndex[i] = sideObjectsIndex[j];
sideObjectsIndex[j] = tempIndex;
}
if(topObjectsROI[j].y < topObjectsROI[i].y)
{
tempRect = topObjectsROI[i];
topObjectsROI[i] = topObjectsROI[j];
topObjectsROI[j] = tempRect;
}
}
}
// Sort by x if needed
for(i=0;i<(nTopContours-1);i++)
{
CvRect tempRect;
int tempIndex;
int j;
if((sideObjectsRotatedROI[i+1].y - sideObjectsRotatedROI[i].y) < 20)
{
if(sideObjectsRotatedROI[i+1].x < sideObjectsRotatedROI[i].x)
{
tempRect = sideObjectsRotatedROI[i];
sideObjectsRotatedROI[i] = sideObjectsRotatedROI[i+1];
sideObjectsRotatedROI[i+1] = tempRect;
tempIndex = sideObjectsIndex[i];
sideObjectsIndex[i] = sideObjectsIndex[i+1];
sideObjectsIndex[i+1] = tempIndex;
}
}
if((topObjectsROI[i+1].y - topObjectsROI[i].y) < 20)
{
if(topObjectsROI[i+1].x < topObjectsROI[i].x)
{
tempRect = topObjectsROI[i];
topObjectsROI[i] = topObjectsROI[i+1];
topObjectsROI[i+1] = tempRect;
}
}
}
// ------ End Sort Objects -------
// ------ Correlate Target Objects -------
targetTopObjectsIndex = (int*)malloc(sizeof(int)*nTopContours);
memset(targetTopObjectsIndex, 0, nTopContours);
for(i=0; i<(*nTargetContours); i++)
{
int j;
for(j=0; j<*nSideContours; j++)
{
if(sideObjectsIndex[j] == targetSideObjectsIndex[i])
{
targetTopObjectsIndex[i] = j;
}
}
}
// ------ Correlate Target Objects -------
// ---- Begin Create Map --------
// Fill In Objects and Target
for(i=0; i<nTopContours; i++)
{
int xIni = 0, xFin = 0;
int yIni = 0, yFin = 0;
int j;
xIni = (topObjectsROI[i].x - K3RADIUS) / 10;
yIni = (topObjectsROI[i].y - K3RADIUS) / 10;
xFin = (topObjectsROI[i].x + topObjectsROI[i].width + K3RADIUS) / 10;
yFin = (topObjectsROI[i].y + topObjectsROI[i].height + K3RADIUS) / 10;
int x, y;
for(x = xIni; x <= xFin; x++)
{
for(y = yIni; y <= yFin; y++)
{
// Object
topMap[x][y] = 1;
// Target
for(j=0;j<(*nTargetContours); j++)
if(targetTopObjectsIndex[j] == i)
topMap[x][y] = 2;
}
}
}
// ---- End Create Map --------
wayPathSize = GAPathPlanning();
for(i=0;i<wayPathSize;i++)
{
wayPointsX[i] = robotPath[i][0];
wayPointsY[i] = robotPath[i][1];
}
mc_AgentVariableSave(mc_current_agent, "wayPathSize");
mc_AgentVariableSave(mc_current_agent, "wayPointsX");
mc_AgentVariableSave(mc_current_agent, "wayPointsY");
// Memory cleanup
cvReleaseImage (&topImage);
cvReleaseImage (&topImageGray);
free(sideObjectsROI_X);
free(sideObjectsROI_Y);
free(sideObjectsROI_Width);
free(targetObjectsROI_X);
free(targetObjectsROI_Y);
free(targetObjectsROI_Width);
free(topObjectsROI);
free(sideObjectsRotatedROI);
free(sideObjectsIndex);
free(targetSideObjectsIndex);
free(targetTopObjectsIndex);
return 0;
}
// ------------------- GA PATH PLANNING ----------------------
char
dir[2][9] =
{{1, 1, 0, -1, -1, -1, 0, 1},
{0, 1, 1, 1, 0, -1, -1, -1}},
beg_pt[2] = {K3XPOSITION/10,K3YPOSITION/10};
boolean path_fitness(population *pop, entity *entity)
{
int i;
char cur_pt[2] = {beg_pt[0],beg_pt[1]}; // Start at beg_pt
char value = 0;
double fitness = 0;
double d = 0;
double w = 0;
char target = 0;
for(i=0;i<pop->len_chromosomes;i++)
{
if(target)
{
((char *)entity->chromosome[0])[i] = -99;
continue;
}
// reset w to 0
w = 0;
value = ((char *)entity->chromosome[0])[i];
if(value < -8) continue;
if(value < 0) {
value += 8;
((char *)entity->chromosome[0])[i] = value;
}
d = ( (value & 1) ? 1.4142 : 1);
cur_pt[0] += dir[0][value];
cur_pt[1] += dir[1][value];
if(cur_pt[0] < 1 || cur_pt[0] > 63)
w = 300.0;
else if(cur_pt[1] < 1 || cur_pt[1] > 47)
w = 300.0;
else if(topMap[cur_pt[0]][cur_pt[1]] == 1)
w = 300.0;
else if(topMap[cur_pt[0]][cur_pt[1]] == 2)
{
w = -100;
target = 1;
}
// take into account total length?
fitness += d*(1+w);
}
entity->fitness = -fitness;
return TRUE;
}
boolean path_seed(population *pop, entity *adam)
{
int i;
static int count = 0;
/* Checks. */
if (!pop) die("Null pointer to population structure passed.");
if (!adam) die("Null pointer to entity structure passed.");
for(i=0;i<pop->len_chromosomes;i++)
((char*)adam->chromosome[0])[i] = (char)random_int_range(0, 7);
return TRUE;
}
int GAPathPlanning()
{
int i;
int j;
int size;
int value;
char cur_pt[2] = {beg_pt[0], beg_pt[1]};
population *pop; /* Population of solutions. */
//random_seed(0);
random_init();
pop = ga_genesis_char(
2000, /* const int population_size */
1, /* const int num_chromo */
224, /* const int len_chromo */
NULL, /* GAgeneration_hook generation_hook */
NULL, /* GAiteration_hook iteration_hook */
NULL, /* GAdata_destructor data_destructor */
NULL, /* GAdata_ref_incrementor data_ref_incrementor */
path_fitness, /* GAevaluate evaluate */
path_seed, /* GAseed seed */
NULL, /* GAadapt adapt */
ga_select_one_bestof2, /* GAselect_one select_one */
ga_select_two_bestof2, /* GAselect_two select_two */
ga_mutate_char_multipoint, /* GAmutate mutate */
ga_crossover_char_allele_mixing, /* GAcrossover crossover */
NULL, /* GAreplace replace */
NULL /* vpointer User data */
);
ga_population_set_parameters(
pop, /* population *pop */
GA_SCHEME_DARWIN, /* const ga_scheme_type scheme */
GA_ELITISM_PARENTS_SURVIVE, /* const ga_elitism_type elitism */
0.9, /* double crossover */
0.02, /* double mutation */
0.0 /* double migration */
);
ga_evolution(
pop, /* population *pop */
100 /* const int max_generations */
);
if(ga_get_entity_from_rank(pop,0))
{
for(i=0; i<pop->len_chromosomes; i++)
{
value = ((char *)ga_get_entity_from_rank(pop,0)->chromosome[0])[i];
if(value == -99) break;
}
size = i;
robotPath = (char **)malloc(sizeof(char*)*size);
for(i=0;i<size;i++)
robotPath[i] = (char *)malloc(sizeof(char)*2);
robotPath[0][0] = cur_pt[0];
robotPath[0][1] = cur_pt[1];
for(i=1; i<size; i++)
{
value = ((char *)ga_get_entity_from_rank(pop,0)->chromosome[0])[i-1];
cur_pt[0] += dir[0][value];
cur_pt[1] += dir[1][value];
robotPath[i][0] = cur_pt[0];
robotPath[i][1] = cur_pt[1];
}
}
ga_extinction(pop);
return size;
}
]]>
</AGENT_CODE>
<AGENT_CODE id="3">
<![CDATA[
#include <stdio.h>
int
main (void)
{
int* wayPathSize;
int* wayPointsX;
int* wayPointsY;
int i;
wayPathSize = (int*)mc_AgentVariableRetrieve(mc_current_agent, "wayPathSize", 1);
wayPointsX = (int*)mc_AgentVariableRetrieve(mc_current_agent, "wayPointsX", 1);
wayPointsY = (int*)mc_AgentVariableRetrieve(mc_current_agent, "wayPointsY", 1);
printf("Robot Way Points\n");
printf("----------------\n");
for(i=0;i<*wayPathSize;i++)
printf("%3d (%3d,%3d)\n", i, wayPointsX[i], wayPointsY[i]);
return 0;
}
]]>
</AGENT_CODE>
</TASKS>
</AGENT_DATA>
</MOBILE_AGENT>
</MESSAGE>
</MOBILEC_MESSAGE>
|
| Integration Engineering Laboratory | UCD MTU Sandia |