void ClientSocket::removeBodies(bool graspable){ std::vector <Body *> bd; readBodyIndList(bd); QTextStream os(this); for(std::vector<Body *>::iterator bp = bd.begin(); bp != bd.end(); ++bp){ if(!graspable || (*bp)->inherits("GraspableBody") ) graspItGUI->getIVmgr()->getWorld()->destroyElement(*bp, true); } os << 1 << '\n'; }
//Body transforms are sent backwards by the planner for some reason compared to the overloading //of << for transfs void ClientSocket::setBodyTransf(){ //FIXME add checking for badly formatted input here. std::vector <Body *> bd; readBodyIndList(bd); transf object_pose; for(std::vector<Body *>::iterator bp = bd.begin(); bp != bd.end(); ++bp){ readTransf(&object_pose); (*bp)->setTran(object_pose); } QTextStream os(this); os << "1 \n"; os.flush(); }
void ClientSocket::sendBodyTransf(){ std::vector <Body *> bd; readBodyIndList(bd); QTextStream os(this); for(std::vector<Body *>::iterator bp = bd.begin(); bp != bd.end(); ++bp){ //this is a hack around the overloading for standard strings and not qstrings. //this should be templated std::stringstream ss(std::stringstream::in | std::stringstream::out); ss << (*bp)->getTran().translation() << (*bp)->getTran().rotation() << "\n"; std::cout << ss.str() << "\n"; os << QString(ss.str().c_str()); } os << "\n"; os.flush(); std::cout << "Body list size:"<< bd.size() << "\n"; }
/*! This is the main routine for parsing input on the clientSocket. There should be one command for each line of input. This reads one line, and looks at the first word (up to the first space character) to determine the command. Then if there are body or robot indices to read, it calls a support routine to read those and return a vector of bodies or robots. These are then passed to the appropriate routine to carry out the action and write out any necessary results. */ void ClientSocket::readClient() { int i, numData, numBodies, numRobots; double time; std::vector<Body *> bodyVec; std::vector<Robot *> robVec; bool ok; while (canReadLine()) { line = readLine(); line.truncate(line.length() - 1); //strip newline character lineStrList = QStringList::split(' ', line); strPtr = lineStrList.begin(); #ifdef GRASPITDBG std::cout << "Command parser line: " << line.toStdString().c_str() << std::endl; #endif if (*strPtr == "getContacts") { strPtr++; if (strPtr == lineStrList.end()) { continue; } numData = (*strPtr).toInt(&ok); strPtr++; if (!ok) { continue; } #ifdef GRASPITDBG std::cout << "Num data: " << numData << std::endl; #endif if (readBodyIndList(bodyVec)) { continue; } numBodies = bodyVec.size(); for (i = 0; i < numBodies; i++) { sendContacts(bodyVec[i], numData); } } else if (*strPtr == "getAverageContacts") { strPtr++; if (readBodyIndList(bodyVec)) { continue; } numBodies = bodyVec.size(); for (i = 0; i < numBodies; i++) { sendAverageContacts(bodyVec[i]); } } else if (*strPtr == "getBodyName") { strPtr++; if (readBodyIndList(bodyVec)) { continue; } numBodies = bodyVec.size(); for (i = 0; i < numBodies; i++) { sendBodyName(bodyVec[i]); } } else if (*strPtr == "getRobotName") { strPtr++; if (readRobotIndList(robVec)) { continue; } numRobots = robVec.size(); for (i = 0; i < numRobots; i++) { sendRobotName(robVec[i]); } } else if (*strPtr == "getDOFVals") { strPtr++; if (readRobotIndList(robVec)) { continue; } numRobots = robVec.size(); for (i = 0; i < numRobots; i++) { sendDOFVals(robVec[i]); } } else if (*strPtr == "moveDOFs") { strPtr++; readDOFVals(); } else if (*strPtr == "render") { graspitCore->getIVmgr()->getViewer()->render(); } else if (*strPtr == "setDOFForces") { strPtr++; if (readRobotIndList(robVec)) { continue; } numRobots = robVec.size(); for (i = 0; i < numRobots; i++) if (readDOFForces(robVec[i]) == FAILURE) { continue; } } else if (*strPtr == "moveToContacts") { graspitCore->getWorld()->getCurrentHand()->approachToContact(30, true); } else if ((*strPtr) == "moveDynamicBodies") { strPtr++; if (strPtr == lineStrList.end()) { ok = FALSE; } else { time = (*strPtr).toDouble(&ok); strPtr++; } if (!ok) { moveDynamicBodies(-1); } else { moveDynamicBodies(time); } } else if (*strPtr == "computeNewVelocities") { #ifdef GRASPITDBG std::cout << "cnv" << std::endl; #endif strPtr++; if (strPtr == lineStrList.end()) { continue; } time = (*strPtr).toDouble(&ok); strPtr++; if (!ok) { continue; } #ifdef GRASPITDBG std::cout << time << std::endl; #endif computeNewVelocities(time); } } }
/*! This is the main routine for parsing input on the clientSocket. There should be one command for each line of input. This reads one line, and looks at the first word (up to the first space character) to determine the command. Then if there are body or robot indices to read, it calls a support routine to read those and return a vector of bodies or robots. These are then passed to the appropriate routine to carry out the action and write out any necessary results. */ void ClientSocket::readClient() { int i,numData,numBodies,numRobots; double time; std::vector<Body *> bodyVec; std::vector<Robot *> robVec; bool ok; while ( canReadLine() ) { line = readLine(); line.truncate(line.length()-1); //strip newline character lineStrList = QStringList::split(' ',line); strPtr = lineStrList.begin(); #ifdef GRASPITDBG std::cout <<"Command parser line: "<<line << std::endl; #endif if (*strPtr == "getContacts") { strPtr++; if (strPtr == lineStrList.end()) continue; numData = (*strPtr).toInt(&ok); strPtr++; if (!ok) continue; #ifdef GRASPITDBG std::cout << "Num data: "<<numData<<std::endl; #endif if (readBodyIndList(bodyVec)) continue; numBodies = bodyVec.size(); for (i=0;i<numBodies;i++) sendContacts(bodyVec[i],numData); } else if (*strPtr == "getAverageContacts") { strPtr++; if (readBodyIndList(bodyVec)) continue; numBodies = bodyVec.size(); for (i=0;i<numBodies;i++) sendAverageContacts(bodyVec[i]); } else if (*strPtr == "getBodyName") { strPtr++; if (readBodyIndList(bodyVec)) continue; numBodies = bodyVec.size(); for (i=0;i<numBodies;i++) sendBodyName(bodyVec[i]); } else if(*strPtr == "setBodyName") { strPtr++; int body_index; if(strPtr != lineStrList.end()){ body_index = strPtr->toInt(&ok); strPtr++; if(strPtr == lineStrList.end()) return; if (body_index == -1 || body_index >= graspItGUI->getIVmgr()->getWorld()->getNumBodies()) { body_index = graspItGUI->getIVmgr()->getWorld()->getNumBodies() - 1; } graspItGUI->getIVmgr()->getWorld()->getBody(body_index)->setName(*strPtr); } } else if (*strPtr == "getRobotName") { strPtr++; if (readRobotIndList(robVec)) continue; numRobots = robVec.size(); for (i=0;i<numRobots;i++) sendRobotName(robVec[i]); } else if (*strPtr == "getDOFVals") { strPtr++; if (readRobotIndList(robVec)) continue; numRobots = robVec.size(); for (i=0;i<numRobots;i++) sendDOFVals(robVec[i]); } else if (*strPtr == "moveDOFs") { strPtr++; readDOFVals(); } else if (*strPtr == "render") graspItGUI->getIVmgr()->getViewer()->render(); else if (*strPtr == "setDOFForces") { strPtr++; if (readRobotIndList(robVec)) continue; numRobots = robVec.size(); for (i=0;i<numRobots;i++) if (readDOFForces(robVec[i])==FAILURE) continue; } else if ((*strPtr) == "moveDynamicBodies") { strPtr++; if (strPtr == lineStrList.end()) ok = FALSE; else { time = (*strPtr).toDouble(&ok); strPtr++; } if (!ok) moveDynamicBodies(-1); else moveDynamicBodies(time); } else if (*strPtr == "computeNewVelocities") { #ifdef GRASPITDBG std::cout << "cnv" << std::endl; #endif strPtr++; if (strPtr == lineStrList.end()) continue; time = (*strPtr).toDouble(&ok); strPtr++; if (!ok) continue; #ifdef GRASPITDBG std::cout << time <<std::endl; #endif computeNewVelocities(time); } else if ((*strPtr) == "outputPlannerResults"){ strPtr++; outputPlannerResults(0); } else if ((*strPtr) == "outputCurrentGrasp"){ strPtr++; outputCurrentGrasp(); } else if ((*strPtr) == "sendBodyTransf"){ strPtr++; verifyInput(1); sendBodyTransf(); } else if ((*strPtr) == "setBodyTransf"){ strPtr++; verifyInput(7); setBodyTransf(); } else if ((*strPtr) == "addObstacle"){ strPtr++; verifyInput(1); addObstacle(*(strPtr+1)); strPtr+=2; } else if ((*strPtr) == "addObject"){ verifyInput(2); addGraspableBody(*(strPtr+1), *(strPtr+2)); strPtr+=3; verifyInput(7); transf object_pose; readTransf(&object_pose); World * w = graspItGUI->getIVmgr()->getWorld(); w->getGB(w->getNumGB() - 1)->setTran(object_pose); } else if ((*strPtr) == "getCurrentHandTran"){ strPtr++; getCurrentHandTran(); } else if ((*strPtr) == "signalGraspUnreachable"){ strPtr+=4; std::cout << line.toStdString() << std::endl; graspItGUI->getIVmgr()->blinkBackground(); } else if ((*strPtr) == "getPlannerTarget"){ strPtr+=1; QTextStream os (this) ; os << graspItGUI->getIVmgr()->getWorld()->getCurrentHand()->getGrasp()->getObject()->getName() << "\n"; } else if ((*strPtr) == "setPlannerTarget"){ QTextStream os(this); os << setPlannerTarget(*(strPtr+1)) << "\n"; strPtr+=2; } else if ((*strPtr) == "rotateHandLat"){ strPtr+=1; rotateHandLat(); } else if ((*strPtr) == "rotateHandLong"){ strPtr+=1; rotateHandLong(); } else if ((*strPtr) == "exec"){ strPtr+=1; exec(); } else if ((*strPtr) == "next"){ strPtr+=1; next(); } else if ((*strPtr) == "addPointCloud") { strPtr += 1; addPointCloud(); //QTextStream os(this); //os << addPointCloud() <<" \n"; } else if ((*strPtr) == "setCameraOrigin") { strPtr += 1; setCameraOrigin(); } else if ((*strPtr) == "removeBodies"){ strPtr += 1; removeBodies(); } else if ((*strPtr) == "clearGraspableBodies"){ strPtr += 1; removeBodies(true); } } }