void EntityAdapter::setOrientation( Config::Real xAngle, Config::Real yAngle, Config::Real zAngle ) { Ogre::Quaternion qx(Ogre::Radian(xAngle), Ogre::Vector3(1.0,0.0,0.0)) ; Ogre::Quaternion qy(Ogre::Radian(yAngle), Ogre::Vector3(0.0,1.0,0.0)) ; Ogre::Quaternion qz(Ogre::Radian(zAngle), Ogre::Vector3(0.0,0.0,1.0)) ; m_entity->setOrientation(qx*qy*qz) ; }
void EditorIkSolver::_checkPivotXY(ArRef<Joint> joint, Vector3d /* requestedTranslation */, Quaterniond requestedRotation) { double prx, pry, prz; joint->getDeltaRotation().getEulerAngles(prx, pry, prz); requestedRotation = joint->getDeltaRotation() * requestedRotation; double rx, ry, rz; requestedRotation.getEulerAngles(rx, ry, rz); double* max[2]; double* min[2]; ar_down_cast<JointConstraint2DOF>(joint->getConstraint())->getLimitValues(min, max); double xmin = *min[0], xmax = *max[0], ymin = *min[1], ymax = *max[1]; rx = _clampWithPreviousValue(xmin, xmax, prx, rx); ry = _clampWithPreviousValue(ymin, ymax, pry, ry); Quaterniond qx(Vector3d(1.0, 0.0, 0.0), rx); Quaterniond qy(Vector3d(0.0, 1.0, 0.0), ry); joint->setDeltaRotation(qx * qy); }
void CameraControllerThirdPerson::onMouseMove( u16 x,u16 y ) { if(m_bLButtonDown) { float angleX = (float)(x - m_ptOrigin.x) * 0.1f; float angleY = (float)(y - m_ptOrigin.y) * 0.1f; Quaternion qy(angleX, Vec3::UNIT_Y); Mat4 mtxWorld(qy); m_mtxWorld = m_mtxWorld * mtxWorld; Mat4 mi = m_mtxWorld.inverse(); Vec3 v = m_pCamera->getRight(); v = mi * v; Quaternion q; q.FromAngleAxis(angleY, v); m_mtxWorld = m_mtxWorld * q; } else if(m_bMButtonDown) { m_pCamera->moveRelative(Vec3((-x + m_ptOrigin.x) / 4.0f,0,0)); m_pCamera->moveRelative(Vec3(0,(y - m_ptOrigin.y) / 4.0f,0)); } m_ptOrigin.x = x; m_ptOrigin.y = y; }
inline PxQuat EulerAngleToQuat(const PxVec3 &rot) { PxQuat qx(degToRad(rot.x), PxVec3(1.0f, 0.0f, 0.0f)); PxQuat qy(degToRad(rot.y), PxVec3(0.0f, 1.0f, 0.0f)); PxQuat qz(degToRad(rot.z), PxVec3(0.0f, 0.0f, 1.0f)); return qz * qy * qx; }
void MotionCallback(int x, int y) { int dx = mx - x; int dy = my - y; if (gMouseSphere) // Move the mouse sphere { NxVec3 pos; ViewUnProject(x,y, gMouseDepth, pos); gMouseSphere->setGlobalPosition(pos); gHitActor->wakeUp(); } else if (gHitCloth) // Attach the cloth vertex { NxVec3 pos; ViewUnProject(x,y, gMouseDepth, pos); gHitCloth->attachVertexToGlobalPosition(gHitClothVertex, pos); } else if (bLeftMouseButtonPressed) // Set camera { gCameraForward.normalize(); gCameraRight.cross(gCameraForward,NxVec3(0,1,0)); NxQuat qx(NxPiF32 * dx * 20 / 180.0f, NxVec3(0,1,0)); qx.rotate(gCameraForward); NxQuat qy(NxPiF32 * dy * 20 / 180.0f, gCameraRight); qy.rotate(gCameraForward); } mx = x; my = y; }
static void MotionCallback(int x, int y) { int dx = gMouseX - x; int dy = gMouseY - y; gDir.normalize(); //カメラの視線ベクトルを正規化 gViewY.cross(gDir, NxVec3(0,1,0)); // if( gMouseButton[0] && gMouseButton[1] ) { //Zoom: Left + Center Buttons Drag gEye -= gDir * 0.5f * dy; } else { if( gMouseButton[0] ) { //Rotate: Left Button Drag NxQuat qx(NxPiF32 * dx * 10/ 180.0f, NxVec3(0,1,0)); qx.rotate(gDir); NxQuat qy(NxPiF32 * dy * 10/ 180.0f, gViewY); qy.rotate(gDir); } else if( gMouseButton[1] ) { //Move: Center Button Drag gEye += 0.1f * (gViewY * dx - NxVec3(0, 1, 0) * dy); } } gMouseX = x; gMouseY = y; glutPostRedisplay(); }
invariant_error_type get_invariant_error(const pp::vector_topology< vect_n<double> >&, const point_type& x, const input_type& u, const output_type& y, const time_type& t) const { vect<double,2> qy(y[2],y[3]); qy = unit(qy); vect<double,2> qx(x[2],x[3]); qx = unit(qx); return invariant_error_type(y[0] - x[0], y[1] - x[1], //qy[0] * qx[0] + qy[1] * qx[1], // c_y * c_x + s_y * s_x qy[1] * qx[0] - qy[0] * qx[1]); // s_y * c_x - c_y * s_x };
void SimObjBase::setQ(const dReal *q) { int i = 0; qw(q[i]); i++; qx(q[i]); i++; qy(q[i]); i++; qz(q[i]); i++; }
void euler2quaternion(Eigen::Quaternion<scalar> &res, const scalar& phi, const scalar& theta, const scalar& psi) { Eigen::Quaternion<scalar> qx(std::cos(phi/2), std::sin(phi/2), 0, 0), qy(std::cos(theta/2), 0, std::sin(theta/2), 0), qz(std::cos(psi/2), 0, 0, std::sin(psi/2)); res = qz * qy * qx; }
void SimObjBase::setAxisAndAngle(double ax, double ay, double az, double degAngle) { Sgv::Quaternion q(degAngle*180/M_PI, ax, ay, az); qw(q.qw()); qx(q.qx()); qy(q.qy()); qz(q.qz()); }
void RotateCamera(int dx, int dy) { gDir = gDir.Normalize(); gN = gDir ^ Point(0,1,0); NxQuat qx(NxPiF32 * dx * 20/ 180.0f, Point(0,1,0)); qx.rotate(gDir); NxQuat qy(NxPiF32 * dy * 20/ 180.0f, gN); qy.rotate(gDir); }
void Quat :: setRotXYZ( const float a, const float b, const float c ) { Quat qx(ROT_X, a); Quat qy(ROT_Y, b); Quat qz(ROT_Z, c); *this = qx * qy * qz; } /* RotXIdent */
/*********************************************************** constructor from 3 angles ***********************************************************/ LbaQuaternion::LbaQuaternion(float anglex, float angley, float anglez) { LbaQuaternion qx(anglex, LbaVec3(1, 0, 0)); LbaQuaternion qy(angley, LbaVec3(0, 1, 0)); LbaQuaternion qz(anglez, LbaVec3(0, 0, 1)); LbaQuaternion res = (qy * qx * qz); X = res.X; Y = res.Y; Z = res.Z; W = res.W; }
void AHRS::draw(){ if(!serial.isInitialized() || !isAvailable){ ostringstream buff; if (!serial.isInitialized()) { buff << "Can't connect to serial port " << currPort; ofDrawBitmapString(buff.str(), (ofGetWidth()-400)*0.5, ofGetHeight()*0.5); } else if(!isAvailable) { buff << "Not receiving data from AHRS"; ofDrawBitmapString(buff.str(), (ofGetWidth()-250)*0.5, ofGetHeight()*0.5); } return; } camera.update(); camera.begin(); ofEnableDepthTest(); box.setOrientation(ofVec3f::zero()); ofQuaternion qr(roll, ofVec3f(0,0,1)); ofQuaternion qp(-pitch, ofVec3f(1,0,0)); ofQuaternion qy(-(yaw-yawOffset), ofVec3f(0,1,0)); ofQuaternion qt = qr * qp * qy; box.setOrientation(qt); ofPushStyle(); ofSetColor(0, 125, 255); box.drawWireframe(); ofPopStyle(); drawGrid(); drawAcc(); ofDisableDepthTest(); camera.end(); // ostringstream buff; // buff << "yaw : " << yaw << endl // << "pitch : " << pitch << endl // << "roll : " << roll << endl; // ofDrawBitmapString(buff.str(), 10, 15); ofPushStyle(); int w = ofGetWidth() * 0.25; int h = ofGetHeight()*0.25; plot->draw(ofGetWidth()-10-w, 10, w, h); // int w = ofGetWidth() * 0.5; // plot->draw((ofGetWidth()-w)*0.5, 10, w, 250); ofPopStyle(); }
void RotateCamera(int dx, int dy) { const Point Up(0.0f, 1.0f, 0.0f); gDir.Normalize(); gViewY = gDir^Up; // #### TODO: replicate this using Ice quats MyQuat qx(NxPiF32 * dx * 20.0f/ 180.0f, Up); qx.rotate(gDir); MyQuat qy(NxPiF32 * dy * 20.0f/ 180.0f, gViewY); qy.rotate(gDir); }
/*! * @brief It rotates for the specification of the relative angle. * @param[in] x-axis rotation weather(i of quaternion complex part) * @param[in] y-axis rotation weather(j of quaternion complex part) * @param[in] z-axis rotation weather(k of quaternion complex part) * @param[in] flag for ansolute / relational (1.0=absolute, else=relational) */ void SimObjBase::setAxisAndAngle(double ax, double ay, double az, double angle, double direct) { // The angle is used now at the relative angle specification. if (dynamics()) { return; } Rotation r; if (direct != 1.0) r.setQuaternion(qw(), qx(), qy(), qz()); // alculate relational angle r.setAxisAndAngle(ax, ay, az, angle, direct); const dReal *q = r.q(); setQ(q); }
//-------------------------------------------------------------- void testApp::mouseDragged(int x, int y, int button){ if (button==0){ ofQuaternion qy((x-ofGetPreviousMouseX()), ofVec3f(0,1,0)); ofQuaternion qx((y-ofGetPreviousMouseY()), ofVec3f(1,0,0)); qrot *= qx*qy; } if (button==2){ ofQuaternion qz((x-ofGetPreviousMouseX()), ofVec3f(0,0,1)); qrot *= qz; scale += (float)(ofGetPreviousMouseY()-y)/(ofGetWindowHeight()/2); if (scale<0) scale = 0.0001; } }
void MotionCallback(int x, int y) { int dx = mx - x; int dy = my - y; gCameraForward.normalize(); gCameraRight.cross(gCameraForward,NxVec3(0,1,0)); NxQuat qx(NxPiF32 * dx * 20 / 180.0f, NxVec3(0,1,0)); qx.rotate(gCameraForward); NxQuat qy(NxPiF32 * dy * 20 / 180.0f, gCameraRight); qy.rotate(gCameraForward); mx = x; my = y; }
static void MotionCallback(int x, int y) { int dx = mx - x; int dy = my - y; Dir = Dir.normalize(); N = Dir.cross(btVector3(0,1,0)); NxQuat qx(NxPiF32 * dx * 20/ 180.0f, btVector3(0,1,0)); qx.rotate(Dir); NxQuat qy(NxPiF32 * dy * 20/ 180.0f, N); qy.rotate(Dir); mx = x; my = y; }
void Camera::HandleMotion(int x, int y) { int dx = m_mouse_x - x; int dy = m_mouse_y - y; PxVec3 viewY = m_dir.cross(PxVec3(0, 1, 0)).getNormalized(); PxQuat qx(PxPi * dx / 180.0f, PxVec3(0, 1, 0)); m_dir = qx.rotate(m_dir); PxQuat qy(PxPi * dy / 180.0f, viewY); m_dir = qy.rotate(m_dir); m_dir.normalize(); m_mouse_x = x; m_mouse_y = y; }
void MouseMotion(int x, int y ) { if(g_isButtonDown[0] || g_isButtonDown[2]) { int dx = g_last_x - x; int dy = g_last_y - y; g_CameraForward.normalize(); g_CameraRight.cross(g_CameraForward, NxVec3(0,1,0)); NxQuat qx(NxPiF32 * dx * 20/ 180.0f, NxVec3(0,1,0)); qx.rotate(g_CameraForward); NxQuat qy(NxPiF32 * dy * 20/ 180.0f, g_CameraRight); qy.rotate(g_CameraForward); g_last_x = x; g_last_y = y; } if (g_isButtonDown[1]) { float dt = 0.1f; if ((float) (x-g_last_x)>0) { g_CameraPos -= g_CameraRight*g_Speed*dt; } else if ((float) (x-g_last_x)<0) { g_CameraPos += g_CameraRight*g_Speed*dt; } if ((float) (y-g_last_y)>0) { g_CameraPos +=NxVec3(0.0f,1.0f,0.0f)*g_Speed*dt; } else if ((float) (y-g_last_y)<0) { g_CameraPos -=NxVec3(0.0f,1.0f,0.0f)*g_Speed*dt; } g_last_x = x; g_last_y = y; } glutPostRedisplay(); }
casa::ImageRegion* ImageRegionGenerator::_makeRegionPolygon( casa::ImageInterface<casa::Float> * image, Carta::Lib::Regions::Polygon* polygon ){ casa::ImageRegion* imageRegion = nullptr; if ( image && polygon ){ QPolygonF poly = polygon->qpolyf(); int cornerPointCount = poly.count(); casa::Vector<casa::Double> x( cornerPointCount ); casa::Vector<casa::Double> y( cornerPointCount ); const casa::CoordinateSystem &cs = image->coordinates( ); bool successful = true; for ( int i = 0; i < cornerPointCount; ++i ) { QPointF polyPt = poly.value( i ); casa::Vector<casa::Double> worldPt = _toWorld( cs, polyPt.x(), polyPt.y(), &successful ); if ( successful ){ x[i] = worldPt[0]; y[i] = worldPt[1]; } else { qDebug() << "i="<<i<<" could not convert x="<<polyPt.x()<<" y="<<polyPt.y()<<" to world."; break; } } if ( successful ){ casa::Vector<casa::Int> dispAxes(2); const casa::CoordinateSystem &cs = image->coordinates( ); int directionIndex = cs.findCoordinate( casa::Coordinate::DIRECTION ); if ( directionIndex >= 0 ){ casa::Vector<casa::Int> dirPixelAxis = cs.pixelAxes(directionIndex); dispAxes(0) = dirPixelAxis[0]; dispAxes(1) = dirPixelAxis[1]; const casa::String units( RAD_UNITS.toStdString().c_str() ); try { casa::Quantum<casa::Vector<casa::Double> > qx( x, units ); casa::Quantum<casa::Vector<casa::Double> > qy( y, units ); casa::WCPolygon poly(qx, qy, casa::IPosition(dispAxes), cs); imageRegion = new casa::ImageRegion(poly); } catch( casa::AipsError& error ) { qDebug() << "Error making image region: "<<error.getMesg().c_str(); } } } } return imageRegion; }
void IHPDriver::handleEvent(core::objectmodel::Event *event) { //std::cout<<"IHPDriver::handleEvent() called:"<<std::endl;////////////////////////////////////////////////////// static double time_prev; if (firstDevice && dynamic_cast<sofa::simulation::AnimateEndEvent *> (event)) { // force the simulation to be "real-time" CTime *timer; timer = new CTime(); double time = 0.001*timer->getRefTime()* PaceMaker::time_scale; // in sec // if the computation time is shorter than the Dt set in the simulation... it waits ! if ((time- time_prev) < getContext()->getDt() ) { double wait_time = getContext()->getDt() - time + time_prev; timer->sleep(wait_time); } time_prev=time; } if (dynamic_cast<sofa::simulation::AnimateBeginEvent *>(event)) { //turnOn xitactVisu if(!visuActif && xitactVisu.getValue() && initVisu) { simulation::Node *parent = dynamic_cast<simulation::Node *>(this->getContext()); parent->addChild(nodeXitactVisual); nodeXitactVisual->updateContext(); visuActif = true; } //turnOff xitactVisu else if(initVisu && visuActif && !xitactVisu.getValue()) { simulation::Node *parent = dynamic_cast<simulation::Node *>(this->getContext()); parent->removeChild(nodeXitactVisual); nodeXitactVisual->updateContext(); visuActif=false; } // calcul des angles à partir de la direction proposée par l'interface... // cos(ThetaX) = cx sin(ThetaX) = sx cos(ThetaZ) = cz sin(ThetaZ) = sz . // au repos (si cx=1 et cz=1) on a Axe y // on commence par tourner autour de x puis autour de z // [cz -sz 0] [1 0 0 ] [0] [ -sz*cx] // [sz cz 0]*[0 cx -sx]*[1] = [ cx*cz ] // [0 0 1] [0 sx cx] [0] [ sx ] xiTrocarAcquire(); XiToolState state; xiTrocarQueryStates(); xiTrocarGetState(indexTool.getValue(), &state); // saving informations in class structure. data.simuState = state; Vector3 dir; dir[0] = -(double)state.trocarDir[0]; dir[1] = (double)state.trocarDir[2]; dir[2] = -(double)state.trocarDir[1]; double pi = 3.1415926535; double thetaY; double thetaX; thetaY = (atan2(dir[0],-sqrt(1-dir[0]*dir[0]))); thetaX = (pi-acos(dir[2]*sqrt(1-dir[0]*dir[0])/(dir[0]*dir[0]-1))); //look if thetaX and thetaY are NaN if(!(thetaX == thetaX)) { cout<<"ratrapage X"<<endl; thetaX=pi; } if(!(thetaY == thetaY)) { cout<<"ratrapage Y"<<endl; thetaY=pi; } if(dir[1]>=0) thetaX*=-1; while(thetaY<=0) thetaY+=2*pi; while(thetaX<=0) thetaX+=2*pi; while(thetaY>2*pi) thetaY-=2*pi; while(thetaX>2*pi) thetaX-=2*pi; if (showToolStates.getValue()) // print tool state this->displayState(); if (testFF.getValue()) // try FF when closing handle this->updateForce(); // Button and grasp handling event XiStateFlags stateFlag; stateFlag = state.flags - data.restState.flags; if (stateFlag == XI_ToolButtonLeft) this->leftButtonPushed(); else if (stateFlag == XI_ToolButtonRight) this->rightButtonPushed(); if (state.opening < graspThreshold.getValue()) { this->graspClosed(); } //XitactVisu VecCoord& posD =(*visualXitactDOF->x.beginEdit()); posD.resize(4); VecCoord& posA =(*visualAxesDOF->x.beginEdit()); posA.resize(3); VecCoord& posB =(*positionBase.beginEdit()); //data.positionBaseGlobal[0]=posB[0]; data.posBase=posB[0].getCenter(); data.quatBase=posB[0].getOrientation(); SolidTypes<double>::Transform tampon(posB[0].getCenter(),posB[0].getOrientation()); positionBase.endEdit(); posD[0].getCenter() = tampon.getOrigin(); posD[0].getOrientation() = tampon.getOrientation(); sofa::helper::Quater<double> qRotX(Vec3d(1,0,0),pi/2); sofa::helper::Quater<double> qRotY(Vec3d(0,0,-1),pi/2); SolidTypes<double>::Transform transformRotX(Vec3d(0.0,0.0,0.0),qRotX); SolidTypes<double>::Transform transformRotY(Vec3d(0.0,0.0,0.0),qRotY); SolidTypes<double>::Transform tamponAxes=tampon; posA[0].getCenter() = tamponAxes.getOrigin(); posA[0].getOrientation() = tamponAxes.getOrientation(); tamponAxes*=transformRotX; posA[1].getCenter() = tamponAxes.getOrigin(); posA[1].getOrientation() = tamponAxes.getOrientation(); tamponAxes*=transformRotY; posA[2].getCenter() = tamponAxes.getOrigin(); posA[2].getOrientation() = tamponAxes.getOrientation(); sofa::helper::Quater<double> qy(Vec3d(0,1,0),thetaY); sofa::helper::Quater<double> qx(Vec3d(1,0,0),thetaX); SolidTypes<double>::Transform transform2(Vec3d(0.0,0.0,0.0),qx*qy); tampon*=transform2; posD[1].getCenter() = tampon.getOrigin(); posD[1].getOrientation() = tampon.getOrientation(); sofa::helper::Quater<float> quarter3(Vec3d(0.0,0.0,1.0),-state.toolRoll); SolidTypes<double>::Transform transform3(Vec3d(0.0,0.0,-state.toolDepth*Scale.getValue()),quarter3); tampon*=transform3; posD[2].getCenter() = tampon.getOrigin(); posD[2].getOrientation() = tampon.getOrientation(); if(posTool) { VecCoord& posT = *(posTool->x0.beginEdit()); //cout<<"xitact "<<deviceIndex.getValue()<<" "<<posD[2]<<endl; posT[deviceIndex.getValue()]=posD[2]; posTool->x0.endEdit(); } sofa::helper::Quater<float> quarter4(Vec3d(0.0,1.0,0.0),-data.simuState.opening/(float)2.0); SolidTypes<double>::Transform transform4(Vec3d(0.0,0.0,0.44*Scale.getValue()),quarter4); tampon*=transform4; posD[3].getCenter() = tampon.getOrigin(); posD[3].getOrientation() = tampon.getOrientation(); visualXitactDOF->x.endEdit(); Vec1d& openT = (*openTool.beginEdit()); openT[0]=(data.simuState.opening)*(maxTool.getValue()-minTool.getValue())+minTool.getValue(); openTool.endEdit(); if(changeScale) { float rapport=((float)Scale.getValue())/oldScale; for(int j = 0; j<4 ; j++) { sofa::defaulttype::ResizableExtVector<sofa::defaulttype::Vec<3,float>> &scaleMapping = *(visualNode[j].mapping->points.beginEdit()); for(unsigned int i=0; i<scaleMapping.size(); i++) { for(int p=0; p<3; p++) scaleMapping[i].at(p)*=rapport; } visualNode[j].mapping->points.endEdit(); } oldScale=(float)Scale.getValue(); changeScale=false; } } if (dynamic_cast<core::objectmodel::KeypressedEvent *>(event)) { core::objectmodel::KeypressedEvent *kpe = dynamic_cast<core::objectmodel::KeypressedEvent *>(event); onKeyPressedEvent(kpe); } else if (dynamic_cast<core::objectmodel::KeyreleasedEvent *>(event)) { core::objectmodel::KeyreleasedEvent *kre = dynamic_cast<core::objectmodel::KeyreleasedEvent *>(event); onKeyReleasedEvent(kre); } //std::cout<<"IHPDriver::handleEvent() ended:"<<std::endl; }
int main(int argc, char** argv) { char port[] = "27015"; char ip[] = "10.0.0.67"; int numbytes; char buf[MAXDATASIZE]; // Get input parameters for(int a=0; a < argc; a++) { if( strcmp( argv[a], "--ip" ) == 0 ) { strcpy(ip, argv[a+1]); } if( strcmp( argv[a], "--port" ) == 0 ) { strcpy(port, argv[a+1]); } } TCP myClient = TCP(port, ip); ros::init(argc, argv, "joints"); ros::NodeHandle n; std::string receivedString; std::string attribute; std::stringstream ss; std::istringstream iss; tf::TransformBroadcaster br; tf::Transform transform; ros::Rate loop_rate(30); if( myClient.Connect() == 0 ) { // Receive data numbytes = recv(myClient.s,buf,MAXDATASIZE-1,0); while (numbytes != 0 && ros::ok()) { numbytes = recv(myClient.s,buf,MAXDATASIZE-1,0); buf[numbytes]='\0'; receivedString.assign(buf); // debug // std::cout << "Ahoy! Received " << numbytes << " bytes. " << receivedString << std::endl; std::istringstream iss(receivedString); std::vector<double> x (25, 0.0); std::vector<double> y (25, 0.0); std::vector<double> z (25, 0.0); std::vector<std::vector<double> > X (6, x); std::vector<std::vector<double> > Y (6, y); std::vector<std::vector<double> > Z (6, z); std::vector<double> qx (25, 0.0); std::vector<double> qy (25, 0.0); std::vector<double> qz (25, 0.0); std::vector<double> qw (25, 1.0); std::vector<std::vector<double> > QX (6, qx); std::vector<std::vector<double> > QY (6, qy); std::vector<std::vector<double> > QZ (6, qz); std::vector<std::vector<double> > QW (6, qw); int j; int count = 6; int id; std::vector<int> foundIdx; std::string frame; std::string joint; /* code */ while(!iss.eof()) { // joint = "joint_"; iss >> attribute; frame = "person_"; if( strcmp(attribute.c_str(), "count:") == 0 ) { iss >> count; // ROS_INFO("count %d",count); } else if( strcmp(attribute.c_str(), "id:") == 0 ) { iss >> id; foundIdx.push_back(id); frame.append(boost::lexical_cast<std::string>(id)); // ROS_INFO("id %s",frame); // std::cout << id << " " << frame << " "; }