/*----------------------------------------------------------------------*
| finalize construction of this interface |
*----------------------------------------------------------------------*/
bool MOERTEL::Interface::Complete()
{
if (IsComplete())
{
if (OutLevel()>0)
std::cout << "MOERTEL: ***WRN*** MOERTEL::Interface::InterfaceComplete:\n"
<< "MOERTEL: ***WRN*** InterfaceComplete() was called before, do nothing\n"
<< "MOERTEL: ***WRN*** file/line: " << __FILE__ << "/" << __LINE__ << "\n";
return true;
}
//-------------------------------------------------------------------
// check for NULL entries in maps
bool ok = true;
for (int i=0; i<2; ++i)
{
std::map<int,Teuchos::RCP<MOERTEL::Node> >::const_iterator curr;
for (curr=node_[i].begin(); curr!=node_[i].end(); ++curr)
{
if (curr->second == Teuchos::null)
{
std::cout << "***ERR*** MOERTEL::Interface::Complete:\n"
<< "***ERR*** Interface # " << Id_ << ":\n"
<< "***ERR*** found NULL entry in map of nodes\n"
<< "***ERR*** file/line: " << __FILE__ << "/" << __LINE__ << "\n";
ok = false;
}
}
}
for (int i=0; i<2; ++i)
{
std::map<int,Teuchos::RCP<MOERTEL::Segment> >::const_iterator curr;
for (curr=seg_[i].begin(); curr!=seg_[i].end(); ++curr)
{
if (curr->second == Teuchos::null)
{
std::cout << "***ERR*** MOERTEL::Interface::Complete:\n"
<< "***ERR*** Interface # " << Id_ << ":\n"
<< "***ERR*** found NULL entry in map of segments\n"
<< "***ERR*** file/line: " << __FILE__ << "/" << __LINE__ << "\n";
ok = false;
}
}
}
int lok = ok;
int gok = 1;
gcomm_.MinAll(&lok,&gok,1);
if (!gok) return false;
//-------------------------------------------------------------------
// check whether all nodes for segments are present
// (take in account that node might be on different processor)
// this test is expensive and does not scale. It is therefore only performed
// when user requests a high output level
#if 1
if (OutLevel()>9)
{
for (int proc=0; proc<gcomm_.NumProc(); ++proc)
{
for (int side=0; side<2; ++side)
{
// create length of list of all nodes adjacent to segments on proc
int sendsize = 0;
if (proc==gcomm_.MyPID())
{
std::map<int,Teuchos::RCP<MOERTEL::Segment> >::const_iterator curr;
for (curr=seg_[side].begin(); curr!=seg_[side].end(); ++curr)
sendsize += curr->second->Nnode();
}
gcomm_.Broadcast(&sendsize,1,proc);
// create list of all nodes adjacent to segments on proc
std::vector<int> ids(sendsize);
if (proc==gcomm_.MyPID())
{
std::map<int,Teuchos::RCP<MOERTEL::Segment> >::const_iterator curr;
int counter=0;
for (curr=seg_[side].begin(); curr!=seg_[side].end(); ++curr)
{
const int* segids = curr->second->NodeIds();
for (int i=0; i<curr->second->Nnode(); ++i)
ids[counter++] = segids[i];
}
}
gcomm_.Broadcast(&ids[0],sendsize,proc);
// check on all processors for nodes in ids
std::vector<int> foundit(sendsize);
std::vector<int> gfoundit(sendsize);
for (int i=0; i<sendsize; ++i)
{
foundit[i] = 0;
if (node_[side].find(ids[i]) != node_[side].end())
foundit[i] = 1;
}
gcomm_.MaxAll(&foundit[0],&gfoundit[0],sendsize);
for (int i=0; i<sendsize; ++i)
{
if (gfoundit[i]!=1)
{
if (gcomm_.MyPID()==proc)
std::cout << "***ERR*** MOERTEL::Interface::Complete:\n"
<< "***ERR*** cannot find segment's node # " << ids[i] << "\n"
<< "***ERR*** in map of all nodes on all procs\n"
<< "***ERR*** file/line: " << __FILE__ << "/" << __LINE__ << "\n";
ids.clear();
foundit.clear();
gfoundit.clear();
gcomm_.Barrier();
return false;
}
}
// tidy up
ids.clear();
foundit.clear();
gfoundit.clear();
} // for (int size=0; side<2; ++side)
} // for (int proc=0; proc<gcomm_.NumProc(); ++proc)
}
#endif
//-------------------------------------------------------------------
// find all procs that have business on this interface (own nodes/segments)
// build a Epetra_comm that contains only those procs
// this intra-communicator will be used to handle most stuff on this
// interface so the interface will not block all other procs
{
#ifdef EPETRA_MPI
std::vector<int> lin(gcomm_.NumProc());
std::vector<int> gin(gcomm_.NumProc());
for (int i=0; i<gcomm_.NumProc(); ++i) lin[i] = 0;
// check ownership of any segments
for (int i=0; i<2; ++i)
if (seg_[i].size() != 0)
{
lin[gcomm_.MyPID()] = 1;
break;
}
// check ownership of any nodes
for (int i=0; i<2; ++i)
if (node_[i].size() != 0)
{
lin[gcomm_.MyPID()] = 1;
break;
}
gcomm_.MaxAll(&lin[0],&gin[0],gcomm_.NumProc());
lin.clear();
// typecast the Epetra_Comm to Epetra_MpiComm
Epetra_MpiComm* epetrampicomm = dynamic_cast<Epetra_MpiComm*>(&gcomm_);
if (!epetrampicomm)
{
std::stringstream oss;
oss << "***ERR*** MOERTEL::Interface::Complete:\n"
<< "***ERR*** Interface " << Id() << ": Epetra_Comm is not an Epetra_MpiComm\n"
<< "***ERR*** file/line: " << __FILE__ << "/" << __LINE__ << "\n";
throw ReportError(oss);
}
// split the communicator into participating and none-participating procs
int color;
int key = gcomm_.MyPID();
// I am taking part in the new comm if I have any ownership
if (gin[gcomm_.MyPID()])
color = 0;
// I am not taking part in the new comm
else
color = MPI_UNDEFINED;
// tidy up
gin.clear();
// create the local communicator
MPI_Comm mpi_global_comm = epetrampicomm->GetMpiComm();
MPI_Comm* mpi_local_comm = new MPI_Comm();
MPI_Comm_split(mpi_global_comm,color,key,mpi_local_comm);
// create the new Epetra_MpiComm
if (*mpi_local_comm == MPI_COMM_NULL)
lcomm_ = Teuchos::null;
else
lcomm_ = Teuchos::rcp(new Epetra_MpiComm(*mpi_local_comm)); // FIXME: who destroys the MPI_Comm inside?
#if 0
// test this stuff on the mpi level
int grank,lrank;
MPI_Comm_rank(mpi_global_comm,&grank);
if (*mpi_local_comm != MPI_COMM_NULL)
MPI_Comm_rank(*mpi_local_comm,&lrank);
else
lrank = -1;
for (int proc=0; proc<gcomm_.NumProc(); ++proc)
{
if (proc==gcomm_.MyPID())
std::cout << "using mpi comms: I am global rank " << grank << " and local rank " << lrank << std::endl;
gcomm_.Barrier();
}
// test this stuff on the epetra level
if (lComm())
for (int proc=0; proc<lcomm_->NumProc(); ++proc)
{
if (proc==lcomm_->MyPID())
std::cout << "using epetra comms: I am global rank " << gcomm_.MyPID() << " and local rank " << lcomm_->MyPID() << std::endl;
lcomm_->Barrier();
}
gcomm_.Barrier();
#endif
#else // the easy serial case
Epetra_SerialComm* serialcomm = dynamic_cast<Epetra_SerialComm*>(&gcomm_);
if (!serialcomm)
{
std::stringstream oss;
oss << "***ERR*** MOERTEL::Interface::Complete:\n"
<< "***ERR*** Interface " << Id() << ": Epetra_Comm is not an Epetra_SerialComm\n"
<< "***ERR*** file/line: " << __FILE__ << "/" << __LINE__ << "\n";
throw ReportError(oss);
}
lcomm_ = Teuchos::rcp(new Epetra_SerialComm(*serialcomm));
#endif // end of #ifdef PARALLEL
}
//-------------------------------------------------------------------
// create a map of all nodes to there PID (process id)
if (lComm())
for (int proc=0; proc<lcomm_->NumProc(); ++proc)
{
int lnnodes = 0;
if (proc==lcomm_->MyPID())
lnnodes = node_[0].size() + node_[1].size();
lcomm_->Broadcast(&lnnodes,1,proc);
std::vector<int> ids(lnnodes);
if (proc==lcomm_->MyPID())
{
std::map<int,Teuchos::RCP<MOERTEL::Node> >::const_iterator curr;
int counter=0;
for (int side=0; side<2; ++side)
for (curr=node_[side].begin(); curr!=node_[side].end(); ++curr)
ids[counter++] = curr->first;
}
lcomm_->Broadcast(&ids[0],lnnodes,proc);
for (int i=0; i<lnnodes; ++i)
nodePID_.insert(std::pair<int,int>(ids[i],proc));
ids.clear();
}
//-------------------------------------------------------------------
// create a map of all segments to there PID (process id)
if (lComm())
for (int proc=0; proc<lcomm_->NumProc(); ++proc)
{
int lnsegs = 0;
if (proc==lcomm_->MyPID())
lnsegs = seg_[0].size() + seg_[1].size();
lcomm_->Broadcast(&lnsegs,1,proc);
std::vector<int> ids(lnsegs);
if (proc==lcomm_->MyPID())
{
std::map<int,Teuchos::RCP<MOERTEL::Segment> >::const_iterator curr;
int counter=0;
for (int side=0; side<2; ++side)
for (curr=seg_[side].begin(); curr!=seg_[side].end(); ++curr)
ids[counter++] = curr->first;
}
lcomm_->Broadcast(&ids[0],lnsegs,proc);
for (int i=0; i<lnsegs; ++i)
segPID_.insert(std::pair<int,int>(ids[i],proc));
ids.clear();
}
//-------------------------------------------------------------------
// set isComplete_ flag
// we set it here already as we will be using some methods that require it
// from now on
isComplete_ = true;
//-------------------------------------------------------------------
// make the nodes know there adjacent segments
// find max number of nodes to a segment
if (lComm())
{
int lmaxnnode = 0;
int gmaxnnode = 0;
for (int side=0; side<2; ++side)
{
std::map<int,Teuchos::RCP<MOERTEL::Segment> >::const_iterator scurr;
for (scurr=seg_[side].begin(); scurr!=seg_[side].end(); ++scurr)
if (lmaxnnode < scurr->second->Nnode())
lmaxnnode = scurr->second->Nnode();
}
lcomm_->MaxAll(&lmaxnnode,&gmaxnnode,1);
// loop all procs and broadcast their adjacency
for (int proc=0; proc<lcomm_->NumProc(); ++proc)
{
// local number of segments
int lnseg = 0;
if (proc==lcomm_->MyPID())
lnseg = seg_[0].size() + seg_[1].size();
lcomm_->Broadcast(&lnseg,1,proc);
// allocate vector to hold adjacency
int offset = gmaxnnode+2;
int size = lnseg*offset;
std::vector<int> adj(size);
// proc fills adjacency vector adj and broadcasts
if (proc==lcomm_->MyPID())
{
int count = 0;
for (int side=0; side<2; ++side)
{
std::map<int,Teuchos::RCP<MOERTEL::Segment> >::const_iterator scurr;
for (scurr=seg_[side].begin(); scurr!=seg_[side].end(); ++scurr)
{
Teuchos::RCP<MOERTEL::Segment> seg = scurr->second;
adj[count] = seg->Id();
adj[count+1] = seg->Nnode();
const int* ids = seg->NodeIds();
for (int i=0; i<seg->Nnode(); ++i)
adj[count+2+i] = ids[i];
count += offset;
}
}
}
lcomm_->Broadcast(&adj[0],size,proc);
// all procs read adj and add segment to the nodes they own
int count = 0;
for (int i=0; i<lnseg; ++i)
{
int segid = adj[count];
int nnode = adj[count+1];
for (int j=0; j<nnode; ++j)
{
int nid = adj[count+2+j];
if (lcomm_->MyPID() == NodePID(nid))
{
// I own this node, so set the segment segid in it
Teuchos::RCP<MOERTEL::Node> node = GetNodeViewLocal(nid);
if (node == Teuchos::null)
{
std::stringstream oss;
oss << "***ERR*** MOERTEL::Interface::Complete:\n"
<< "***ERR*** cannot find node " << nid << "\n"
<< "***ERR*** in map of all nodes on this proc\n"
<< "***ERR*** file/line: " << __FILE__ << "/" << __LINE__ << "\n";
throw ReportError(oss);
}
node->AddSegment(segid);
}
else
continue;
}
count += offset;
}
adj.clear();
} // for (int proc=0; proc<lcomm_->NumProc(); ++proc)
} // if (lComm())
//-------------------------------------------------------------------
// build redundant segments and nodes
if (lComm())
{
int ok = 0;
ok += RedundantSegments(0);
ok += RedundantSegments(1);
ok += RedundantNodes(0);
ok += RedundantNodes(1);
if (ok != 4)
{
std::stringstream oss;
oss << "***ERR*** MOERTEL::Interface::Complete:\n"
<< "***ERR*** building of redundant information failed\n"
<< "***ERR*** file/line: " << __FILE__ << "/" << __LINE__ << "\n";
throw ReportError(oss);
}
}
//-------------------------------------------------------------------
// make topology segments <-> nodes for each side
if (lComm())
BuildNodeSegmentTopology();
//-------------------------------------------------------------------
// delete distributed nodes and segments
for (int i=0; i<2; ++i)
{
seg_[i].clear();
node_[i].clear();
}
//-------------------------------------------------------------------
// we are done
// note that there might not be any functions on the interface yet
// they still have to be set
return ok;
}
void takepom(){
printf("takepom \n");
set_servo_position(CLAW1, CLAW1_FRONT);
set_servo_position(CLAW2, CLAW2_FRONT);
set_servo_position(CAM_SERVO,CAM_SERVO_POSITION_FRONT);
stop();
msleep(1000);
turn_right();
msleep(500);
int findit=1;
int left_or_right=1; //ob nach linksoder rechts drehen l=0, r=1
int getit=0; //wenn er das pom hat
int foundNothingToLong=0;
double sec=seconds();
while(!getit){
if((sec+10)<seconds()){ //wenn er 10 sec nichts findet
if(foundNothingToLong=2){
forward();
while((analog10(ET)<310&&( analog10(LEFT_SENSOR)<left_blk-150||analog10(RIGHT_SENSOR)<right_blk-150))){
msleep(5);
}
if(analog10(ET)<310){
turn_left();
msleep(600);
forward();
while( analog10(LEFT_SENSOR)<left_blk-150||analog10(RIGHT_SENSOR)<right_blk-150){}
}
}
else{
foundNothingToLong++;
back();
msleep(400); //etwas rückwerts
sec=seconds();
if(!get_object_count(green)>0){ //wenn ers nicht hat
forward();
msleep(400);
findit=0; //nicht gefunden
turn_left();
while(analog10(LEFT_SENSOR)<left_blk-150){}
turn_left();
while(analog10(MIDDLE_SENSOR)<mid_blk-150){}
break;
}
}
}
if(left_or_right){
turn_right_speed(slow_speed);
while(analog10(RIGHT_SENSOR)<right_blk-150&&!get_object_count(green)>0){ //solange Kammera und Sensor nichts sehen rechts drehen
camera_update();
}
stop(); //wenn etwas gefunden stehen bleiben
msleep(200);
if(analog10(RIGHT_SENSOR)>right_blk-150){ //schauen ob Kamera oder Sensor
left_or_right=0; //bei sensor drehrichtung ändern
}
else{
camera_update();
if(get_object_bbox(0,0).width>5&&get_object_bbox(0,0).height>5&&analog10(RIGHT_SENSOR)<right_blk-150){ //wenn Kammera : Wa blob ein zu kleiner Blob(was falsches gesehen), wenn blob groß stehen bleiben und feinjustireung(foundit) machen
getit=foundit();
}
}
}
else{
turn_left_speed(slow_speed);
while(analog10(LEFT_SENSOR)<left_blk-150&&!get_object_count(green) >0){
camera_update();
}
stop();
msleep(200);
if(analog10(LEFT_SENSOR)>left_blk-150){
left_or_right=1;
}
else{
camera_update();
if(get_object_bbox(0,0).width>5&&get_object_bbox(0,0).height>5){
getit=foundit();
}
}
}
}
if(findit){
put_poms_out(); // poms nach drausen schieben
}
}
static void
process_file(char *fn)
{
int i;
long n;
int c;
workf = fopen(fn, "r+");
if (workf == NULL) {
fprintf(stderr, "%s: cannot read %s\n", __progname, fn);
return;
}
printf("(file: %s)\n", fn);
current_file = fn;
for (i = 0; i <= MAX_C_A; i++) {
nls[i] = -1;
}
nls[MAX_C_A] = 0;
tbeg = -1;
if (wordmode) {
bufp0 = &buf[1];
rahead = s1l + 1;
buf[0] = '\0';
} else {
bufp0 = &buf[0];
rahead = s1l;
}
if (debugging) {
printf("[rahead = %d, bufp0-buf = %ld]\n",
rahead, (long) (bufp0 - &buf[0]));
}
n = 0;
bufp = bufp0;
bufpmax = &buf[sizeof(buf) - s1l - 2];
flying = allfly;
flystate = 1;
while (1) {
c = getc(workf);
if (c == EOF) {
if (tbeg >= 0) {
if (bufp > bufp0)
fwrite(bufp0, 1, bufp - bufp0, tempf);
fseek(workf, tbeg, 0);
n = ftell(tempf);
fseek(tempf, 0L, 0);
for (; n; n--) {
putc(getc(tempf), workf);
}
fflush(workf);
ftruncate(fileno(workf), ftell(workf));
}
fclose(workf);
return;
}
*bufp++ = c;
n++;
if (debugging) {
printf("[got %c, n now %ld, bufp-buf %ld]\n",
c, n, (long) (bufp - bufp0));
}
if ((n >= rahead) && foundit() && doit()) {
int wbehind;
if (debugging) {
printf("[doing change]\n");
}
wbehind = 1;
if (tbeg < 0) {
tbeg = ftell(workf) - rahead;
fseek(tempf, 0L, 0);
if (debugging) {
printf("[tbeg set to %d]\n",
(int)tbeg);
}
wbehind = 0;
}
if (bufp[-1] == '\n')
add_shift(nls, ftell(workf), MAX_C_A + 1);
if ((n > rahead) && wbehind) {
fwrite(bufp0, 1, n - rahead, tempf);
if (debugging) {
printf("[writing %ld from bufp0]\n",
n - rahead);
}
}
fwrite(str2, 1, s2l, tempf);
n = rahead - s1l;
if (debugging) {
printf("[n now %ld]\n", n);
}
if (n > 0) {
bcopy(bufp - n, bufp0, n);
if (debugging) {
printf("[copying %ld back]\n", n);
}
}
bufp = bufp0 + n;
} else {
if (bufp[-1] == '\n')
add_shift(nls, ftell(workf), MAX_C_A + 1);
if (bufp >= bufpmax) {
if (tbeg >= 0) {
fwrite(bufp0, 1, n - rahead, tempf);
if (debugging) {
printf("[flushing %ld]\n",
n - rahead);
}
}
n = rahead;
bcopy(bufp - n, bufp0, n);
if (debugging) {
printf("[n now %ld]\n[copying %ld back]\n", n, n);
}
bufp = bufp0 + n;
}
}
}
}