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);
}
}
