void D3GridRouter::EachToManyMulticast(comID id, int size, void *msg, int numpes, int *destpes, int more) { int i; //Buffer the message if (size) { PeGrid->InsertMsgs(numpes, destpes, size, msg); } if (more) return; routerStage = 0; ComlibPrintf("All messages received %d %d\n", CkMyPe(), COLLEN); //Send the messages int firstproc = MyPe - (MyPe % (ROWLEN * COLLEN)); for (i=0;i<COLLEN;i++) { ComlibPrintf("ROWLEN = %d, COLLEN =%d first proc = %d\n", ROWLEN, COLLEN, firstproc); //int MYROW = (MyPe % (ROWLEN * COLLEN))/ROWLEN; int nextrowrep = firstproc + i*ROWLEN; int nextpe = (MyPe % (ROWLEN * COLLEN)) % ROWLEN + nextrowrep; int nummappedpes=NumPes-nextrowrep; if (nummappedpes <= 0) { // looks for nextpe in the previous plane nextpe -= ROWLEN * COLLEN; if(nextpe < 0) continue; } if (nextpe >= NumPes) { int mm=(nextpe-NumPes) % nummappedpes; nextpe=nextrowrep+mm; } if (nextpe == MyPe) { ComlibPrintf("%d calling recv directly\n", MyPe); recvCount[0]++; RecvManyMsg(id, NULL); continue; } //ComlibPrintf("nummappedpes = %d, NumPes = %d, nextrowrep = %d, nextpe = %d, mype = %d\n", nummappedpes, NumPes, nextrowrep, nextpe, MyPe); gmap(nextpe); ComlibPrintf("sending to column %d and dest %d in %d\n", i, nextpe, CkMyPe()); GRIDSENDFN(id, 0, 0, psize[i], oneplane[i],CkpvAccess(RouterRecvHandle), nextpe); } }
void CirclePacker::convertOGtoMat(nav_msgs::OccupancyGridConstPtr g) { //ROS_INFO("In CirclePacker::convertOGtoMat"); // Use the GridMap2D library to convert from nav_msgs::OccupancyGrid to cv::Mat gridmap_2d::GridMap2D gmap(g, false); // Create a window //cv::namedWindow("testing", CV_WINDOW_AUTOSIZE); src = gmap.binaryMap(); // Show the image //cv::imshow("testing", src); // PRESS ESC TO BEFORE CLOSING WINDOW, OTHERWISE THE PROGRAM WILL HANG //cv::waitKey(0); }
void D3GridRouter::RecvManyMsg(comID id, char *msg) { ComlibPrintf("%d recvcount=%d,%d recvexpected = %d,%d\n", MyPe, recvCount[0], recvCount[1], recvExpected[0],recvExpected[1]); int stage = 0; if (msg) { stage = PeGrid->UnpackAndInsert(msg); recvCount[stage]++; } if ((recvCount[0] == recvExpected[0]) && (routerStage == 0)){ routerStage = 1; int myrow = (MyPe % (ROWLEN * COLLEN)) / COLLEN; int myrep = myrow*ROWLEN + MyPe - (MyPe % (ROWLEN * COLLEN)); for (int i=0;i<ROWLEN;i++) { int nextpe = myrep + i; //if (nextpe >= NumPes || nextpe==MyPe) continue; if(nextpe == MyPe) { recvCount[1]++; RecvManyMsg(id, NULL); continue; } if(nextpe >= NumPes) { nextpe -= ROWLEN * COLLEN; if(nextpe < 0) continue; } int *pelist = zline; int k = 0; //ComlibPrintf("recv:myrow = %d, nplanes = %d\n", myrow, nplanes); //ComlibPrintf("recv:%d->%d:", MyPe, nextpe); for(k = 0; k < nplanes; k++) { int dest = myrow * ROWLEN + i + ROWLEN * COLLEN * k; //ComlibPrintf("%d,", dest); if(dest >= NumPes) break; zline[k] = dest; } //ComlibPrintf(")\n"); ComlibPrintf("Before gmap %d\n", nextpe); gmap(nextpe); //ComlibPrintf("After gmap %d\n", nextpe); ComlibPrintf("%d:sending recv message %d %d\n", MyPe, nextpe, myrep); GRIDSENDFN(id, 1, 1, k, pelist, CkpvAccess(RouterRecvHandle), nextpe); } } if((recvCount[1] == recvExpected[1]) && (routerStage == 1)){ routerStage = 2; for (int k=0; k < nplanes; k++) { int nextpe = (MyPe % (ROWLEN * COLLEN)) + k * ROWLEN * COLLEN; if (nextpe >= NumPes || nextpe==MyPe) continue; int gnextpe = nextpe; int *pelist = &gnextpe; ComlibPrintf("Before gmap %d\n", nextpe); gmap(nextpe); //ComlibPrintf("After gmap %d\n", nextpe); ComlibPrintf("%d:sending proc message %d %d\n", MyPe, nextpe, nplanes); GRIDSENDFN(id, 2, 2, 1, pelist, CkpvAccess(RouterProcHandle), nextpe); } LocalProcMsg(id); } }