geometry_msgs::Quaternion transformOrientation(geometry_msgs::Quaternion q, Eigen::Matrix4f tf){
// Obtener componente de rotacion de la matriz
tf::Matrix3x3 rotation (tf(0,0), tf(0,1), tf(0,2), tf(1,0), tf(1,1), tf(1,2), tf(2,0), tf(2,1), tf(2,2));
// Convertir a quaternion tf
tf::Quaternion rot_q;
rotation.getRotation(rot_q);
// Convertir quaternionmsg a tf
tf::Quaternion in_q (q.x, q.y, q.z, q.w);
// Añadir rotaciones
rot_q *= in_q; // OJO: SERÁ EL ORDEN CORRECTO?
// Crear nuevo quaternion
geometry_msgs::Quaternion new_q;
new_q.x = rot_q.x();
new_q.y = rot_q.y();
new_q.z = rot_q.z();
new_q.w = rot_q.w();
return new_q;
}
void handle(char a)
{
char next;
if(a == '\'' || a == '"')
in_q(a);
else if(a == '/')
{
if((next = getchar()) == '*')
in_com();
else if(next == '/')
{
putchar(a);
handle(next);
}
else
{
putchar(a);
putchar(next);
}
}
else
putchar(a);
}
