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