//Return list of Objects collided with at Position "where"
//Collisions only with solid objects
//Does not consider if p_o is solid or not.
ObjectList WorldManager::isCollision(Object *p_o, Position where) {
Utility u;
//Make empty list
ObjectList collision_list;
ObjectListIterator li = ObjectListIterator(&updates);
li.first();
while (!li.isDone()) {
Object *p_temp_o = li.currentObject();
if (p_temp_o != p_o) {
//World position bounding box for object at where
Box b = u.getWorldBox(p_o, where);
//World position bounding box for other object
Box b_temp = u.getWorldBox(p_temp_o);
if (u.boxIntersectsBox(b, b_temp) && p_temp_o->isSolid()) {
collision_list.insert(p_temp_o); //Add object because of collision
} //No solid collision
} //Object cannot collide with itself
li.next();
}//End while
return collision_list;
}
/*
Return a list of objects collided at position where. Collsiosns only occur
between solide objects.
P-o's solidness is not considered
*/
ObjectList WorldManager::isCollision(Object *p_o, Position where) const{
Utility utility; //object for reference
//Make empty list
ObjectList collision_list;
//Create an iterator of the current objects
ObjectListIterator lu(&getAllobjects());
while (!lu.isDone()){
Object *p_temp_o = lu.currentObject();
if (p_temp_o != p_o){//Do not consider self
if (utility.positionsIntersect(p_temp_o->getPosition(), where) //check location
&& p_temp_o->isSolid() && p_o->isSolid()){ //check if solid
//if not self and solid and at correct position add to current collisions list
collision_list.insert(p_temp_o);
}//no solid collision
}//not self
lu.next();
}//end while
return collision_list;
}
void Verify::mapping_powv_stream() {
Permutation_powv_stream.clear();
string str;
Utility util;
for(int i= 0; i < District_number; i++) {
str = util.permutation2string(modeling_district_powv(Permutation_districts[i]));
Permutation_powv_stream.push_back(str);
}
return;
}
void RarArchiverStrategy::configure()
{
addExtension(".rar");
addExtension(".cbr");
setExecutables("rar", "unrar");
if (which("rar") != QString::null)
{
setExtractArguments("rar x @F");
setListArguments("rar lb @F");
setSupported();
}
else if (which("unrar") != QString::null)
{
FILE *f;
//
// now determine which unrar it is - free or non-free
if ((f = popen("unrar", "r")) != NULL)
{
QRegExp regexp("^UNRAR.+freeware");
for (QTextStream s(f); !s.atEnd(); )
{
if (regexp.indexIn(s.readLine()) >= 0)
{
nonfree_unrar = true;
break;
}
}
pclose(f);
if (nonfree_unrar)
{
setExtractArguments("unrar x @F");
setListArguments("unrar lb @F");
}
else
{
setExtractArguments("unrar -x @F");
setListArguments("unrar -t @F");
}
setSupported();
}
}
else if (which("unrar-free") != QString::null) //some distros rename free unrar like this
{
setExtractArguments("unrar-free -x @F");
setListArguments("unrar-free -t @F");
setSupported();
}
}
ObInfo generateObInfoGrid(const MotionState robot_state)
{
ObInfo result;
Range x(0.75, 2.);
Range y(0.75, 2.);
double ob_x = x.random();
// Round to 1 decimal place for grid of .1m x .1m
ob_x *= 10;
ob_x = round(ob_x);
ob_x /= 10;
double ob_y = y.random();
// Round to 1 decimal place for grid of .1m x .1m
ob_y *= 10;
ob_y = round(ob_y);
ob_y /= 10;
Range v(0, 0.5);
Range w(0, PI/2.f);
result.x = ob_x;
result.y = ob_y;
result.v = v.random();
result.w = w.random();
result.relative_direction = utility.displaceAngle(atan( ob_y / ob_x ), PI);
return result;
}
vector<int> Verify::retrieve_permutation(const string& str) {
string strng;
Calculation cal;
Utility util;
unsigned int index = str.length() - 2;
int size = table_list.tables[index].size();
for(int i = 0; i < size; i++) {
strng = table_list.tables[index][i];
if(cal.find_str(strng, str)) {
return util.string2permutation(cal.extract_powvs_string(strng, str.length()));
}
}
cout << "*** Cannot find the permutation: " << str << endl;
exit(-1);
}
void DivNode::print(){
Utility uti;
cout << "DivNode " << nodeID << endl;
for (int i = 0; i<edge_in.size(); i++){
cout << "in: NodeType ";
uti.print(edge_in[i].nodeFrom->getNodeType());
cout << ", ID ";
cout << edge_in[i].nodeFrom->getNodeID() << ": " << edge_in[i].d_out_current << " / "
<< edge_in[i].c_in_max << " IR: " << edge_in[i].interest_rate <<endl;
}
for (int i = 0; i<edge_out.size(); i++){
cout << "out: NodeType ";
uti.print(edge_out[i].nodeTo->getNodeType());
cout << ", ID ";
cout << edge_out[i].nodeTo->getNodeID() << ": " << edge_out[i].d_in_current << " / "
<< edge_out[i].c_out_max << " IR: " << edge_out[i].interest_rate <<endl;
}
cout << endl;
}
void Document::writeBibtex (
Library const &lib,
std::ostringstream& out,
bool const usebraces,
bool const utf8)
{
// BibTeX values cannot be larger than 1000 characters - should make sure of this
// We should strip illegal characters from key in a predictable way
out << "@" << bib_.getType() << "{" << key_ << ",\n";
BibData::ExtrasMap extras = bib_.getExtras ();
BibData::ExtrasMap::iterator it = extras.begin ();
BibData::ExtrasMap::iterator const end = extras.end ();
for (; it != end; ++it) {
// Exceptions to usebraces are editor and author because we
// don't want "Foo, B.B. and John Bar" to be literal
writeBibKey (
out,
(*it).first,
(*it).second,
((*it).first.lowercase () != "editor") && usebraces, utf8);
}
// Ideally should know what's a list of human names and what's an
// institution name be doing something different for non-human-name authors?
writeBibKey (out, "author", bib_.getAuthors(), false, utf8);
writeBibKey (out, "title", bib_.getTitle(), usebraces, utf8);
writeBibKey (out, "journal", bib_.getJournal(), usebraces, utf8);
writeBibKey (out, "volume", bib_.getVolume(), false, utf8);
writeBibKey (out, "number", bib_.getIssue(), false, utf8);
writeBibKey (out, "pages", bib_.getPages(), false, utf8);
writeBibKey (out, "year", bib_.getYear(), false, utf8);
writeBibKey (out, "doi", bib_.getDoi(), false, utf8);
if (tagUids_.size () > 0) {
out << "\ttags = \"";
std::vector<int>::iterator tagit = tagUids_.begin ();
std::vector<int>::iterator const tagend = tagUids_.end ();
for (; tagit != tagend; ++tagit) {
if (tagit != tagUids_.begin ())
out << ", ";
out << lib.getTagList()->getName(*tagit);
}
out << "\"\n";
}
out << "}\n\n";
}
void P7zipArchiverStrategy::configure()
{
addExtension(".7z");
addExtension(".cb7");
setExecutables("7z", "7zr");
if (which("7z") != QString::null)
{
setExtractArguments("7z x @F");
setListArguments("7z l @F");
setSupported();
}
else if (which("7zr") != QString::null)
{
setExtractArguments("7zr x @F");
setListArguments("7zr l @F");
setSupported();
}
}
void WorldManager::draw() {
ObjectList allObjects = getAllObjects();
ObjectListIterator li = ObjectListIterator(&allObjects);
Utility u = Utility();
for (int alt = 0; alt <= MAX_ALTITUDE; alt++) {
li.first();
while (!li.isDone()) {
if (li.currentObject()->getAltitude() == alt) {
//Bounding box coordinates are relative to Object
//so convert to world coordinates
Box temp_box = u.getWorldBox(li.currentObject());
if (u.boxIntersectsBox(temp_box, view)) {
li.currentObject()->draw();
}
}
li.next();
}
}
}
/**
* Temporarily duplicating functionality in printBibtex and
* writeBibtex -- the difference is that writeBibtex requires a
* Library reference in order to resolve tag uids to names.
* In order to be usable from PythonDocument printBibtex just
* doesn't bother printing tags at all.
*
* This will get fixed when the ill-conceived tag ID system is
* replaced with lists of strings
*/
Glib::ustring Document::printBibtex (
bool const useBraces,
bool const utf8)
{
std::ostringstream out;
// BibTeX values cannot be larger than 1000 characters - should make sure of this
// We should strip illegal characters from key in a predictable way
out << "@" << bib_.getType() << "{" << key_ << ",\n";
BibData::ExtrasMap extras = bib_.getExtras ();
BibData::ExtrasMap::iterator it = extras.begin ();
BibData::ExtrasMap::iterator const end = extras.end ();
for (; it != end; ++it) {
// Exceptions to useBraces are editor and author because we
// don't want "Foo, B.B. and John Bar" to be literal
writeBibKey (
out,
(*it).first,
(*it).second,
((*it).first.lowercase () != "editor") && useBraces, utf8);
}
// Ideally should know what's a list of human names and what's an
// institution name be doing something different for non-human-name authors?
writeBibKey (out, "author", bib_.getAuthors(), false, utf8);
writeBibKey (out, "title", bib_.getTitle(), useBraces, utf8);
writeBibKey (out, "journal", bib_.getJournal(), useBraces, utf8);
writeBibKey (out, "volume", bib_.getVolume(), false, utf8);
writeBibKey (out, "number", bib_.getIssue(), false, utf8);
writeBibKey (out, "pages", bib_.getPages(), false, utf8);
writeBibKey (out, "year", bib_.getYear(), false, utf8);
writeBibKey (out, "doi", bib_.getDoi(), false, utf8);
out << "}\n\n";
return out.str();
}
// Rendering entrance function
void MeshModelRender::DrawModel()
{
// set the default material here.
m_DefaultMaterial.SetMaterial();
Utility util;
// Whether show axis
if(util.IsSetFlag(m_State, RENDER_MODEL_AXIS))
DrawAxis();
// Whether show bounding box
if(util.IsSetFlag(m_State, RENDER_MODEL_BOUNDING_BOX))
DrawBoundingBox();
// Whether lighting
if(util.IsSetFlag(m_State, RENDER_MODEL_LIGHTING))
glEnable(GL_LIGHTING);
else
glDisable(GL_LIGHTING);
glEnable(GL_POINT_SMOOTH);
glEnable(GL_LINE_SMOOTH);
glEnable(GL_POLYGON_SMOOTH);
glHint(GL_POINT_SMOOTH_HINT, GL_NICEST);
glHint(GL_LINE_SMOOTH_HINT, GL_NICEST);
glHint(GL_POLYGON_SMOOTH_HINT, GL_NICEST);
// Whether per-element color
if(util.IsSetFlag(m_State, RENDER_MODEL_COLOR))
{
glEnable(GL_COLOR_MATERIAL);
glColorMaterial(GL_FRONT_AND_BACK, GL_DIFFUSE);
}
else
glDisable(GL_COLOR_MATERIAL);
switch(m_Mode) {
case RENDER_MODEL_VERTICES:
DrawModelDepth();
DrawModelVertices();
break;
case RENDER_MODEL_WIREFRAME:
DrawModelDepth();
DrawModelWireframe();
break;
case RENDER_MODEL_SOLID_FLAT:
DrawModelSolidFlat();
break;
case RENDER_MODEL_SOLID_SMOOTH:
DrawModelSolidSmooth();
break;
case RENDER_MODEL_SOLID_WITH_SHARPNESS:
DrawModelSolidWithSharpness();
break;
case RENDER_MODEL_SOLID_AND_WIREFRAME:
DrawModelSolidAndWireframe();
break;
case RENDER_MODEL_HIGHLIGHT_ONLY:
DrawModelHighlightOnly();
break;
case RENDER_MODEL_TEXTURE_MAPPING:
DrawModelTextureMapping();
break;
case RENDER_MODEL_VERTEX_COLOR:
DrawModelVertexColor();
break;
case RENDER_MODEL_FACE_COLOR:
DrawModelFaceColor();
break;
case RENDER_MODEL_TRANSPARENT:
DrawModelTransparent();
break;
case RENDER_MODEL_PARAM_TEXTURE:
DrawModelFaceTexture();
break;
}
glDisable(GL_LIGHTING);
if(util.IsSetFlag(m_State, RENDER_MODEL_POINT))
DrawPoint();
if(util.IsSetFlag(m_State, RENDER_MODEL_LINE))
DrawLine();
if(util.IsSetFlag(m_State, RENDER_MODEL_POLYGON))
DrawPolygon();
// draw kmax and kmin here.
if(util.IsSetFlag(m_State, RENDER_MODEL_KMAX_CURVATURE)
|| util.IsSetFlag(m_State, RENDER_MODEL_KMIN_CURVATURE))
{
DrawMeshCurvature(m_State);
}
glEnable(GL_LIGHTING);
}
/*
* Does NOT generate an ABTC
* Put that in later on once single test case is working well
*/
TestCaseTwo generateTestCase(const MotionState robot_state, int num_obs)
{
TestCaseTwo result;
ROS_INFO("In generateTestCase");
ROS_INFO("num_obs: %i", num_obs);
// Generate all obstacles and push them onto test case
for(int i=0;i<num_obs;i++)
{
ObInfo temp = generateObInfoGrid(robot_state);
if(i == 1)
{
// Get position distance from other obstacle
std::vector<double> one, two;
one.push_back(result.obs[0].x);
one.push_back(result.obs[0].y);
two.push_back(temp.x);
two.push_back(temp.y);
double dist = utility.positionDistance(one, two);
while(dist < 0.2) {
ROS_INFO("one: (%f, %f) temp: (%f, %f)", one[0], one[1], two[0], two[1]);
temp = generateObInfoGrid(robot_state);
two[0] = temp.x;
two[1] = temp.y;
dist = utility.positionDistance(one, two);
}
}
else if (i == 2)
{
// Get position distance from other two obstacles
std::vector<double> one, two, three;
one.push_back(result.obs[0].x);
one.push_back(result.obs[0].y);
two.push_back(result.obs[1].x);
two.push_back(result.obs[1].y);
three.push_back(temp.x);
three.push_back(temp.y);
double dist1 = utility.positionDistance(one, three);
double dist2 = utility.positionDistance(two, three);
while(dist1 < 0.2 || dist2 < 0.2)
{
ROS_INFO("one: (%f, %f) two:(%f, %f) temp: (%f, %f)", one[0], one[1], two[0], two[1], three[0], three[1]);
temp = generateObInfoGrid(robot_state);
three[0] = temp.x;
three[1] = temp.y;
dist1 = utility.positionDistance(one, three);
dist2 = utility.positionDistance(two, three);
}
}
temp.msg = buildObstacleMsg(temp.x, temp.y, temp.v, temp.relative_direction, temp.w);
result.obs.push_back(temp);
result.ob_list.obstacles.push_back(temp.msg);
ROS_INFO("result.obs.size(): %i", (int)result.obs.size());
ROS_INFO("result.ob_list.obstacles.size(): %i", (int)result.ob_list.obstacles.size());
}
return result;
}
void Identifiable::_setUID(const int inUID)
{
if (mUID != -1)
{
std::string _msg = "Attempting to re-assign an object's UID after it's been set! Object: " + mName + "#" + stringify(mUID);
throw integrity_violation(_msg);
}
mUID = inUID;
}
void Identifiable::_setName(std::string const& inName)
{
if (mName != "Unnamed")
{
std::string _msg = "Attempting to re-assign an object's UID after it's been set! Object: " + mName + "#" + stringify(mUID);
throw integrity_violation(_msg);
}
mName = inName;
}
void GetROC::reducedScores1(int colNum, int skip,
const char* inputFName,
const char* outputFName,
int pos, int index)
{
vector<float> TAR, FRR, TRR, FARx;
bool anyFAR = false;
Utility::readFourScores(inputFName, colNum, skip, FRR, FARx, TRR, TAR);
///cout <<"Loading for the file is done" << endl;
Utility *util = new Utility();
util->appendToFile(outputFName, "DOC# L/R DEX DESIGN Filename\n");
util->appendToFile(outputFName, "%d %d %d %d %s\n",
4, pos, index, 0,(const char*)util->getFilenamePart((char*)outputFName));
util->appendToFile(outputFName, " FRR FAR TRR TAR\n");
for(float v=0.0001f; v<1.0f; v=v*10.0f)
{
for(unsigned int i=0; i< FARx.size(); i++)
{
if(FARx[i] >= v && FARx[i] < (v*10.0f) && FARx[i] != -999)
//if(FARx[i] >= v && FARx[i] != -999)
{
if(FARx[i] > 0.99999f)
v=1.0f;
if(i != 0)
{
anyFAR = true;
float prev=v-FARx[i-1];
float next=FARx[i]-v;
if(prev==0.0f)
{
util->appendToFile(outputFName, "%.5f\t%.5f\t%.5f\t%.5f\n",
FRR[i-1], FARx[i-1], TRR[i-1], TAR[i-1]);
break;
}
else
{
float y1=0.0f, y2=0.0f;
y1=TAR[i-1]+((TAR[i]-TAR[i-1])/(prev+next)*prev);
y2=FRR[i-1]+((FRR[i]-FRR[i-1])/(prev+next)*prev);
util->appendToFile(outputFName, "%.5f\t%.5f\t%.5f\t%.5f\n",
y2, v, 1.0-v, y1);
break;
}
}
}
}
if(anyFAR == false)
{
util->appendToFile(outputFName, "%4.2f\t%4.2f\t%4.2f\t%4.2f\n",
-999.00, -999.00, -999.00, -999.00);
}
}
if(anyFAR == false && TAR.size() > 2)
{
int arr = TAR.size()-2;//ignore last (-999)
util->appendToFile(outputFName, "%.5f\t%.5f\t%.5f\t%.5f\n",
FRR[arr], FARx[arr], TRR[arr], TAR[arr]);
}
TAR.clear();
FRR.clear();
TRR.clear();
FARx.clear();
delete util;
cout <<"The reduced ROC is done..." << endl;
}
//no rescale
void GetROC::createROC(const char* outputFName,
vector<float> matchScores,
vector<float> nonMatchScores,
int pos, int index)//use va_list=> how to?
{
vector<float> vecMatch, vecNonMatch, vecThres;
vector<float> vecFRR, vecFAR, vecTRR, vecTAR;
//note that matchScores.size() and nonMatchScores.size() are the same.
long totalSize = matchScores.size();
float tar = 0.0f, frr = 1.0f, farx = 0.0f, trr = 1.0f;
float pfrr = 1.0f;
float pfar = 0.0f;
float eer = 0.0f;
bool foundEER = false;
//int a = 0, b = 0;
Utility *util = new Utility();
util->appendToFile(outputFName, "DOC# L/R DEX DESIGN Filename\n");
util->appendToFile(outputFName, "%d %d %d %d %s\n",
3, pos, index, 0,(const char*)util->getFilenamePart((char*)outputFName));
util->appendToFile(outputFName, "Matches NonMatches Thres FRR FAR TRR TAR\n");
//if the list is too big, this will reduce time.
int dist = 1;
if(totalSize > 50000)
dist = (int) (totalSize / 50000);
for(int j = 0; j < totalSize; j = j + dist)
{
for(int i = 0; i < totalSize; i = i + dist)
{
if(matchScores[i] >= 0.0f && matchScores[i] <= matchScores[j])//this should be "<="
{
tar = (float)(i+1)/(float)totalSize;
frr = 1.0f - tar;
}
if(nonMatchScores[i] >= 0.0f && nonMatchScores[i] <= matchScores[j])//this should be "<="
{
farx = (float)(i+1)/(float)totalSize;
trr = 1.0f - farx;
}
}
//printf("frr: %.5f, far: %.5f, trr: %.5f, tar: %.5f\n", frr, farx, trr, tar);
vecMatch.push_back(matchScores[j]);
vecNonMatch.push_back(nonMatchScores[j]);
vecThres.push_back(matchScores[j]);
vecFRR.push_back(frr);
vecFAR.push_back(farx);
vecTRR.push_back(trr);
vecTAR.push_back(tar);
if(!foundEER)
{
if(frr < farx)
{
float x = (pfar- pfrr)/ (frr-pfrr-farx+pfar);
eer = ((farx-pfar)*x+pfar);
foundEER = true;
//printf("eer: %.5f\n", eer);
}
}
pfrr = frr;
pfar = farx;
}
int nSize = vecFAR.size();
if(nSize < 200)
{
for(int i=0; i<nSize; i++)
{
util->appendToFile(outputFName, "%.5f\t%.5f\t%.5f\t%.5f\t%.5f\t%.5f\t%.5f\n",
vecMatch[i], vecNonMatch[i], vecThres[i], vecFRR[i], vecFAR[i], vecTRR[i], vecTAR[i]);
}
}
else
{
for(int i=0; i<nSize; i++)
{
if((unsigned int)(i+1) < vecFAR.size() && vecFAR[i] != vecFAR[i+1])
{
util->appendToFile(outputFName, "%.5f\t%.5f\t%.5f\t%.5f\t%.5f\t%.5f\t%.5f\n",
vecMatch[i], vecNonMatch[i], vecThres[i], vecFRR[i], vecFAR[i], vecTRR[i], vecTAR[i]);
}
}
util->appendToFile(outputFName, "%.5f\t%.5f\t%.5f\t%.5f\t%.5f\t%.5f\t%.5f\n",
vecMatch[nSize-1], vecNonMatch[nSize-1], vecThres[nSize-1],
vecFRR[nSize-1], vecFAR[nSize-1], vecTRR[nSize-1], vecTAR[nSize-1]);
}
if(eer == 0.0f)
util->appendToFile(outputFName, "%.5f\t-999.00\t-999.00\t-999.00\t-999.00\t-999.00\t-999.00\tEER\n", eer);
else
util->appendToFile(outputFName, "%.5f\t-999.00\t-999.00\t-999.00\t-999.00\t-999.00\t-999.00\tEER\n", eer);
delete util;
vecMatch.clear();
vecNonMatch.clear();
vecThres.clear();
vecFRR.clear();
vecFAR.clear();
vecTRR.clear();
vecTAR.clear();
cout <<"The score calculation is done..." << endl;
}
void Apartment ::add_utility()
{
Utility Temp;
Temp.Add();
U.push_back(Temp);
}
int main(int argc, char *argv[])
{
Utility util;
START_TIME=time (NULL);
string str;
// PARSE PARAMETERS
MIN_DENSITY=atof(argv[4]);
MIN_DIM=atoi(argv[6]);
MIN_SIZE=atoi(argv[8]);
DISTANCE_FUNCTION=atoi(argv[10]);
cout<<"Minimum density : "<<MIN_DENSITY<<endl;
cout<<"Minimum dim : "<<MIN_DIM<<endl;
cout<<"Minimum pattern size : "<<MIN_SIZE<<endl;
// REDIRECT IO
std::ostringstream converter;
converter<<argv[2]<<"experimental_results/alpha_"<<MIN_DENSITY<<"_minGraph_"<<MIN_DIM<<"_minSize_"<<MIN_SIZE<<"_distFunc_"<<DISTANCE_FUNCTION<<".txt";
cout<<"Redirecting output to:"<<converter.str()<<endl;
freopen (converter.str().c_str(),"w",stdout);
// LOAD
Loader loader;
loader.loadDataset(argv[2]);
// PREPROCESS INPUT
util.removeNonHomegenousEdges();
//util.partitionGraph(NUMBER_OF_PARTITIONS);
util.extractComponents(); // only in windows
vector<ConnectedComponent*>::const_iterator iter=connectedComponents.begin();
int numberOfComponents = (int)connectedComponents.size();
for (int i=0; i<numberOfComponents;i++)
{
ConnectedComponent* ccPtr = connectedComponents[i];
cout<<"\tComponent "<<i<< " size:"<< ccPtr->getSize()<<endl;
}
cout<<"\n++++++++++++++++++++++++++++++++\n\n\n"<<endl;
//cout<<"Number of processors:"<<omp_get_num_procs()<<endl;
cout<<"Number of processors used:"<<NUMBER_OF_CPUs<<endl;
//omp_set_num_threads(NUMBER_OF_CPUs);
cout<<"\n\nStarting algorithm!"<<endl;
//1- FIRST PHASE
//#pragma omp parallel for
for (int i=0; i<numberOfComponents;i++)
{
ConnectedComponent* ccPtr = connectedComponents[i];
cout<<"\n\n\n\n======================\n\n\tStarting Connected Component: "<<ccPtr->getID()<<"\tSize: "<<ccPtr->getSize()<<endl;
cout<<"Find seed patterns!"<<endl;
ccPtr->findSeedPatterns();
cout<<"Expand-by-one!"<<endl;
ccPtr->expand_by_one(true);
cout<<"\t\tFinished: "<<ccPtr->getID()<<"\tSize: "<<ccPtr->getSize()<<" # of Found Patterns:"<<ccPtr->getTempMaximalPatterns()->size()<<endl;
}
cout<<"\n--------------------------------\n\n\n"<<endl;
cout<<"\n\n\n\nRun time before merge (in minutes): "<<((float)time(NULL)-START_TIME)/60<<" "<<endl;
cout<<"Run time before merge (in second): "<<(time(NULL)-START_TIME)<<" "<<endl;
// all patterns will be merged to this guy
ConnectedComponent* masterComponent = new ConnectedComponent(-1);
// statistics
cout<<"\n\n"<<endl;
int patternCounter=0;
for (int i=0; i<numberOfComponents;i++)
{
ConnectedComponent* ccPtr = connectedComponents[i];
//cout<<"\tBefore Component "<<i<< " maximal pattern size:"<< ccPtr->getMaximalPatterns()->size()<<endl;
//ccPtr->removeRedundancyInMaximalPatterns();
patternCounter+=(int)ccPtr->getTempMaximalPatterns()->size();
//cout<<"\tAfter Component "<<i<< " maximal pattern size:"<< ccPtr->getMaximalPatterns()->size()<<endl;
}
cout<<"Before redundancy elim, total patterns in components:"<<patternCounter<<endl;
cout<<"\n\n\n\nRun time before redundancy (in minutes): "<<((float)time(NULL)-START_TIME)/60<<" "<<endl;
cout<<"Run time before redundancy (in second): "<<(time(NULL)-START_TIME)<<" "<<endl;
//3- REDUNDANCY ELIMINATION
util.unifyPatternsOfAllComponents2(masterComponent);
cout<<"After removing redundancy number of patterns:"<<masterComponent->getMaximalPatterns()->size()<<endl;
// statistics
cout<<"\n\n\n\n Final number of patterns:"<<masterComponent->getMaximalPatterns()->size()<<endl;
masterComponent->printMaximalPatterns();
//CLEAN MEMORY
util.deallocateConnectedComponents();
util.deallocateNodes();
//cout<<"Pattern counter:"<<Pattern::counter<<endl;
cout<<"Total run time(minutes): "<<((float)time(NULL)-START_TIME)/60<<" "<<endl;
cout<<"Total run time(seconds): "<<(time(NULL)-START_TIME)<<" "<<endl;
return 0;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "rovioTest");
ros::NodeHandle n;
imageClient = n.serviceClient<rovio_base::image>("rovioImage");
controlClient = n.serviceClient<rovio_base::manDrv>("rovioControl");
reportClient = n.serviceClient<rovio_base::report>("rovioReport");
Mat imgOrd = getImage();
cvtColor(imgOrd, imgOrd, CV_RGB2GRAY);
const int AREA_SIZE = 16;
int ordRows = imgOrd.rows;
int ordCols = imgOrd.cols;
int areaNumY = ordRows / AREA_SIZE;
int areaNumX = ordCols / AREA_SIZE;
int marginY = ordRows % AREA_SIZE;
int areaNum = areaNumX * areaNumY;
Rect ior(0, marginY, areaNumX * AREA_SIZE, areaNumY * AREA_SIZE);
Mat img = getImg(imgOrd, ior);
VideoWriter videoWriter("/home/viki/Rovio.avi", CV_FOURCC('M', 'J', 'P', 'G'), 3.0, Size(img.cols, img.rows));
int lastDirection = 1;
for (int i = 0; i < 1000; i++)
{
Mat img = getImg(getImage(), ior);
Mat_<int> regionMap(img.size());
regionMap.setTo(0);
regionMap(regionMap.rows - 18, regionMap.cols / 2) = 1;
regionMap(regionMap.rows - 10, regionMap.cols / 2) = 1;
RegionGrowthAlg alg;
alg.calcRegionMap(img, regionMap);
// Detect forward
int fAreaWidth = img.cols - 200;
int fAreaHeight = img.rows / 4;
int fTopX = (img.cols - fAreaWidth) / 2;
int fTopY = img.rows - fAreaHeight;
int ignorePixels = 1000;
Mat_<int> fArea = regionMap(Rect(fTopX, fTopY, fAreaWidth, fAreaHeight));
int fAreaSum = 0;
for (int i = 0; i < fArea.rows; i++)
{
int* pRow = fArea.ptr<int>(i);
for (int j = 0; j < fArea.cols; j++)
{
if (pRow[j] == 0)
fAreaSum++;
}
}
bool flagForward = fAreaSum < ignorePixels;
// Detect left and right
int lrAreaWidth = 100;
int marginX = 0;
int lrAreaHeight = fAreaHeight;
int lrTopY = img.rows - lrAreaHeight;
int lTopX = marginX;
int rTopX = img.cols - marginX - lrAreaWidth;
int lrIgnorePixels = 1000;
Mat_<int> lArea = regionMap(Rect(lTopX, lrTopY, lrAreaWidth, lrAreaHeight));
Mat_<int> rArea = regionMap(Rect(rTopX, lrTopY, lrAreaWidth, lrAreaHeight));
int lAreaSum = 0;
int rAreaSum = 0;
for (int i = 0; i < lArea.rows; i++)
{
int* plRow = lArea.ptr<int>(i);
int* prRow = rArea.ptr<int>(i);
for (int j = 0; j < lArea.cols; j++)
{
if (plRow[j] == 0)
lAreaSum++;
if (prRow[j] == 0)
rAreaSum++;
}
}
bool flagLeft = lAreaSum < lrIgnorePixels;
bool flagRight = rAreaSum < lrIgnorePixels;
//fArea.setTo(2);
lArea.setTo(3);
rArea.setTo(4);
Utility util;
util.drawSegmentBorder(img, regionMap);
// Mark info
//标记
int leftSum = 0;
int rightSum = 0;
int loopi = img.rows;
int loopj = img.cols;
for (int i = 0; i < loopi; i++)
{
int* pLeftRow = regionMap.ptr<int>(i);
int* pRIghtRow = pLeftRow + loopj / 2;
int loop = loopj / 2;
for (int j = 0; j < loop; j++)
{
if (pLeftRow[j] > 0)
{
leftSum++;
}
if (pRIghtRow[j] > 0)
{
rightSum++;
}
}
}
Point pos(loopj / 2 - 150, loopi / 2);
std::stringstream ss;
string tmp;
ss << leftSum;
ss >> tmp;
putText(img, tmp, pos, FONT_HERSHEY_SIMPLEX, 0.6, Scalar(0, 255, 0));
pos.x = loopj / 2 + 100;
ss.str("");
ss.clear();
ss << rightSum;
ss >> tmp;
putText(img, tmp, pos, FONT_HERSHEY_SIMPLEX, 0.6, Scalar(0, 255, 0));
int textLen = 40;
pos.x = fArea.cols / 2 - textLen + fTopX;
pos.y = fArea.rows / 2 + fTopY;
ss.str("");
ss.clear();
ss << fAreaSum;
ss >> tmp;
putText(img, tmp, pos, FONT_HERSHEY_SIMPLEX, 0.6, Scalar(0, 50, 255));
pos.x = lArea.cols / 2 - textLen + lTopX;
pos.y = lArea.rows / 2 + lrTopY;
ss.str("");
ss.clear();
ss << lAreaSum;
ss >> tmp;
putText(img, tmp, pos, FONT_HERSHEY_SIMPLEX, 0.6, Scalar(0, 50, 255));
pos.x = rArea.cols / 2 - textLen + rTopX;
pos.y = rArea.rows / 2 + lrTopY;
ss.str("");
ss.clear();
ss << rAreaSum;
ss >> tmp;
putText(img, tmp, pos, FONT_HERSHEY_SIMPLEX, 0.6, Scalar(0, 50, 255));
//检测直线区域
int lineLen = 200;
int lineStartX = img.cols / 2 - lineLen / 2;
int lineEndX = img.cols / 2 + lineLen / 2;
int lineY = img.rows - 140;
Point lineStart(lineStartX, lineY);
Point lineEnd(lineEndX, lineY);
line(img, lineStart, lineEnd, Scalar(255, 20, 20));
int blockNum = 0;
int* pLineRow = regionMap.ptr<int>(lineY);
for (int j = lineStartX; j < lineEndX; j++)
{
if (pLineRow[j] == 0)
{
blockNum++;
}
}
bool isBlocked = blockNum > lineLen / 2;
//视频
//cvtColor(img, img, CV_GRAY2RGB);
imshow("", img);
waitKey(10);
videoWriter << img;
//控制
isBlocked = (!flagLeft && !flagRight) || isBlocked;
int waitTime = 1;
rvMCUReport rvMcu = getReport();
if (rvMcu.isIrOn && rvMcu.isDetectedBarrier)
isBlocked = true;
if (true)
{
if (isBlocked)
{
int maxDif = 5000;
if (leftSum - rightSum > maxDif)
{
lastDirection = -1;
}
else if (rightSum - leftSum > maxDif)
{
lastDirection = 1;
}
if (lastDirection == -1)
{
control(5, 8);
waitKey(waitTime);
}
else
{
control(6, 8);
waitKey(waitTime);
}
}
else if (flagForward)
{
control(1, 6);
}
else if (flagLeft)
{
control(3, 8);
}
else if (flagRight)
{
control(4, 8);
}
else
{
printf("Error control");
}
}
}
return 0;
}
int main(void)
{
//Initialize allegro--------------------------------
MainUtility.InitAllegro();
if (MainUtility.Get_GameOver())
{
return -1;
}
srand(time(NULL));
//variables-----------------------------------------
const int FPS = 60;
//Allegro variables---------------------------------
ALLEGRO_EVENT_QUEUE *Event_Queue = NULL;
ALLEGRO_TIMER *Timer = NULL;
Event_Queue = al_create_event_queue();
Timer = al_create_timer(1.0 / FPS);
Display MainDisplay(Event_Queue);
if (!MainDisplay.TestDisplay())
{
return -1;
}
ALLEGRO_BITMAP *DungeonTiles = al_load_bitmap("Test.png");
ALLEGRO_BITMAP *AIImage = al_load_bitmap("AI_Sprite.png");
ALLEGRO_BITMAP *PlayerImage = al_load_bitmap("Player_Sprite.png");
ALLEGRO_BITMAP *BowImage = al_load_bitmap("Bow_Sprite.png");
ALLEGRO_BITMAP *SwordImage = al_load_bitmap("Sword_Sprite.png");
Player MainPlayer(PlayerImage, SwordImage, BowImage, MainDisplay.Get_ScreenWidth(), MainDisplay.Get_ScreenHeight(), Event_Queue);
Camera MainCamera(Event_Queue);
AI_Group TestAIGroup; // Instance of an AI_Group
TerrainGenerator Terrain(1);
Terrain.generateTerrain();
DungeonGenerator Dungeon(Event_Queue, &MainPlayer);
Dungeon.GenerateDungeon(MainDisplay);
MainPlayer.SetXPosition(Dungeon.GetStartPosition().x());
MainPlayer.SetYPosition(Dungeon.GetStartPosition().y());
TestAIGroup.RandomSetup(4, Dungeon, AIImage); // Generates 4 AI with random attributes in the group. Their spawn points will also be set randomly.
TestAIGroup.GetPathToPlayer(MainPlayer); // For testing, generate a path to the player's start position from each AI
ALLEGRO_EVENT_SOURCE ProjectileEvent;
al_init_user_event_source(&ProjectileEvent);
al_register_event_source(Event_Queue, al_get_timer_event_source(Timer));
al_register_event_source(Event_Queue, al_get_keyboard_event_source());
al_register_event_source(Event_Queue, al_get_mouse_event_source());
al_start_timer(Timer);
//Main game loop------------------------------------
while (!MainUtility.Get_GameOver())
{
ALLEGRO_EVENT ev;
al_wait_for_event(Event_Queue, &ev);
MainPlayer.EventHandler(ev, MainCamera.GetMouseXWorldCoordinate(), MainCamera.GetMouseYWorldCoordinate());
TestAIGroup.ProcessAll(ev, MainPlayer); // Process each AI in the group
MainCamera.EventHandler(ev, MainPlayer.GetXPosition(), MainPlayer.GetYPosition());
MainDisplay.Event_Handler(ev);
Dungeon.Event_Handler(ev);
// Collide with AI
if (TestAIGroup.CollideWithAI(MainPlayer.GetCollisionXBoundOne(), MainPlayer.GetCollisionYBoundOne()))
MainPlayer.MovementCollidingBoundOne();
if (TestAIGroup.CollideWithAI(MainPlayer.GetCollisionXBoundTwo(), MainPlayer.GetCollisionYBoundTwo()))
MainPlayer.MovementCollidingBoundTwo();
// Hit the AI
TestAIGroup.HitAI(MainPlayer.GetWeaponHitBoxXBoundOne(), MainPlayer.GetWeaponHitBoxYBoundOne(), MainPlayer.GetWeaponDamage());
TestAIGroup.HitAI(MainPlayer.GetWeaponHitBoxXBoundTwo(), MainPlayer.GetWeaponHitBoxYBoundTwo(), MainPlayer.GetWeaponDamage());
if (Dungeon.Get_Map()->CheckMapCollision(Vec2f(MainPlayer.GetCollisionXBoundOne(), MainPlayer.GetCollisionYBoundOne())))
MainPlayer.MovementCollidingBoundOne();
if (Dungeon.Get_Map()->CheckMapCollision(Vec2f(MainPlayer.GetCollisionXBoundTwo(), MainPlayer.GetCollisionYBoundTwo())))
MainPlayer.MovementCollidingBoundTwo();
//Code Dealing with drawing to the screen goes within this if statement
if (al_is_event_queue_empty(Event_Queue))
{
Dungeon.Draw();
MainPlayer.DrawPlayer();
//Terrain.draw();
TestAIGroup.DrawAll(); // Draw all AI (magenta squares). Their generated paths will also be drawn (small magenta circles).
//Draw display last
MainDisplay.Draw();
}
}
//Game Ending--------------------------------------
return 0;
}
int main(int argc, char** argv) {
srand( time(0));
ros::init(argc, argv, "ramp_planner");
ros::NodeHandle handle;
// Load ros parameters and obstacle transforms
//loadParameters(handle);
//loadObstacleTF();
num_obs = 3;
ros::Rate r(100);
/*
* Data to collect
*/
std::vector<bool> reached_goal;
std::vector<bool> bestTrajec_fe;
std::vector<double> time_left;
std::vector<bool> stuck_in_ic;
std::vector<bool> ic_occurred;
std::vector<TestCaseTwo> test_cases;
ros::Timer ob_trj_timer;
ob_trj_timer.stop();
int num_tests = 42;
ob_delay.push_back(2);
ob_delay.push_back(2);
ob_delay.push_back(4);
// Make an ObstacleList Publisher
pub_obs = handle.advertise<ramp_msgs::ObstacleList>("obstacles", 1);
ros::ServiceClient trj_gen = handle.serviceClient<ramp_msgs::TrajectorySrv>("trajectory_generator");
ros::Subscriber sub_bestT = handle.subscribe("bestTrajec", 1, bestTrajCb);
ros::Subscriber sub_imminent_collision_ = handle.subscribe("imminent_collision", 1, imminentCollisionCb);
ros::Subscriber sub_update = handle.subscribe("update", 1, updateCb);
std::ofstream f_reached;
f_reached.open("/home/sterlingm/ros_workspace/src/ramp/ramp_planner/system_level_data/8/reached_goal.txt",
std::ios::out | std::ios::app | std::ios::binary);
std::ofstream f_feasible;
f_feasible.open("/home/sterlingm/ros_workspace/src/ramp/ramp_planner/system_level_data/8/feasible.txt", std::ios::out
| std::ios::app | std::ios::binary);
std::ofstream f_time_left;
f_time_left.open("/home/sterlingm/ros_workspace/src/ramp/ramp_planner/system_level_data/8/time_left.txt",
std::ios::out | std::ios::app | std::ios::binary);
std::ofstream f_ic_stuck;
f_ic_stuck.open("/home/sterlingm/ros_workspace/src/ramp/ramp_planner/system_level_data/8/ic_stuck.txt", std::ios::out
| std::ios::app | std::ios::binary);
std::ofstream f_ic_occurred;
f_ic_occurred.open("/home/sterlingm/ros_workspace/src/ramp/ramp_planner/system_level_data/8/ic_occurred.txt",
std::ios::out | std::ios::app | std::ios::binary);
// Set flag signifying that the next test case is not ready
ros::param::set("/ramp/tc_generated", false);
ros::Duration d_history(1);
ros::Duration d_test_case_thresh(20);
for(int i=0;i<num_tests;i++)
{
MotionState initial_state;
//my_planner.randomMS(initial_state);
/*
*
* Generate a test case
*
*/
/*
* Generate the test case
*/
ABTC abtc;
/*
* Create test case where all obstacles stop, move, stop for 1 second each
*/
for(int i_ob=0;i_ob<3;i_ob++)
{
abtc.moving[i_ob] = 0;
abtc.moving[i_ob+3] = 1;
abtc.moving[i_ob+6] = 0;
abtc.times[i_ob] = 1;
abtc.times[i_ob+3] = 1;
abtc.times[i_ob+6] = 1;
}
generateObInfoGrid(initial_state);
/*
* Get test data for the abtc
*/
TestCaseTwo tc = generateTestCase(initial_state, num_obs);
tc.abtc = abtc;
/*
* Get trajectories for each obstacle
*/
ramp_msgs::TrajectorySrv tr_srv;
for(int i=0;i<tc.obs.size();i++)
{
ramp_msgs::TrajectoryRequest tr;
tf::Transform tf;
tf.setOrigin( tf::Vector3(0,0,0) );
tf.setRotation( tf::createQuaternionFromYaw(0) );
MotionType mt = my_planner.findMotionType(tc.obs[i].msg);
ramp_msgs::Path p = my_planner.getObstaclePath(tc.obs[i].msg, mt);
tr.path = p;
tr.type = PREDICTION;
tr_srv.request.reqs.push_back(tr);
}
// Call trajectory generator
if(trj_gen.call(tr_srv))
{
for(int i=0;i<tr_srv.response.resps.size();i++)
{
ROS_INFO("Traj: %s", utility.toString(tr_srv.response.resps[i].trajectory).c_str());
tc.ob_trjs.push_back(tr_srv.response.resps[i].trajectory);
}
}
else
{
ROS_ERROR("Error in getting obstacle trajectories");
}
/*
* Get static version of obstacles
*/
ramp_msgs::ObstacleList obs_stat;
for(int i=0;i<tc.obs.size();i++)
{
obs_stat.obstacles.push_back(getStaticOb(tc.obs[i].msg));
}
ROS_INFO("Generate: obs_stat.size(): %i", (int)obs_stat.obstacles.size());
IC_occur = false;
// Set flag signifying that the next test case is ready
ros::param::set("/ramp/tc_generated", true);
ROS_INFO("Generate: Waiting for planner to prepare");
/*
* Wait for planner to be ready to start test case
*/
bool start_tc = false;
while(!start_tc)
{
handle.getParam("/ramp/ready_tc", start_tc);
r.sleep();
ros::spinOnce();
}
ROS_INFO("Generate: Planner ready, publishing static obstacles");
// Publish static obstacles
pub_obs.publish(obs_stat);
// Wait for 1 second
d_history.sleep();
ROS_INFO("Generate: Done sleeping for 1 second");
// Publish dynamic obstacles
//pub_obs.publish(tc.ob_list);
tc.t_begin = ros::Time::now();
// Create timer to continuously publish obstacle information
ob_trj_timer = handle.createTimer(ros::Duration(1./20.), boost::bind(pubObTrj, _1, tc));
/*
* Wait for planner to run for time threshold
*/
while(start_tc)
{
handle.getParam("ramp/ready_tc", start_tc);
//ROS_INFO("generate_test_case: start_tc: %s", start_tc ? "True" : "False");
r.sleep();
ros::spinOnce();
}
ros::Duration elasped = ros::Time::now() - tc.t_begin;
ROS_INFO("generate_test_case: Test case completed");
// Set flag signifying that the next test case is not ready
ros::param::set("/ramp/tc_generated", false);
ob_trj_timer.stop();
/*
* Collect data
*/
MotionState goal;
goal.msg_.positions.push_back(2);
goal.msg_.positions.push_back(2);
goal.msg_.positions.push_back(PI/4.f);
double dist = utility.positionDistance( goal.msg_.positions, latestUpdate.msg_.positions );
//if(elasped.toSec()+0.01 < d_test_case_thresh.toSec())
//if(bestTrajec.trajectory.points.size() < 3)
if(dist < 0.2)
{
reached_goal.push_back(true);
f_reached<<true<<std::endl;
}
else
{
reached_goal.push_back(false);
f_reached<<false<<std::endl;
}
if(bestTrajec.feasible)
{
bestTrajec_fe.push_back(true);
time_left.push_back( bestTrajec.trajectory.points[ bestTrajec.trajectory.points.size()-1
].time_from_start.toSec());
f_feasible<<true<<std::endl;
f_time_left<<bestTrajec.trajectory.points[ bestTrajec.trajectory.points.size()-1
].time_from_start.toSec()<<std::endl;
}
else
{
bestTrajec_fe.push_back(false);
f_feasible<<false<<std::endl;
//time_left.push_back(9999);
}
if(IC_current)
{
stuck_in_ic.push_back(true);
f_ic_stuck<<true<<std::endl;
}
else
{
stuck_in_ic.push_back(false);
f_ic_stuck<<false<<std::endl;
}
if(IC_occur)
{
ic_occurred.push_back(true);
f_ic_occurred<<true<<std::endl;
}
else
{
ic_occurred.push_back(false);
f_ic_occurred<<false<<std::endl;
}
bestTrajec_at_end.push_back(bestTrajec);
test_cases.push_back(tc);
} // end for
f_reached.close();
f_feasible.close();
f_time_left.close();
f_ic_stuck.close();
f_ic_occurred.close();
std::cout<<"\n\nExiting Normally\n";
ros::shutdown();
return 0;
}
