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