bool getCurrentPoseAndSpeeds( mrpt::poses::CPose2D &curPose, float &curV, float &curW)
{
robotSim.getRealPose( curPose );
double phi = M_PI/2 - curPose.phi();
if(phi<0)
{
cout << "changing" <<phi << endl;
phi+= 2*M_PI;
cout << "to" << phi << endl;
}
//curPose.phi(phi);
/*float temp = curPose.x();
curPose.x(curPose.y());
curPose.y(temp);*/
curV = robotSim.getV();
curW = robotSim.getW();
return true;
}
