#include "ros/ros.h"
#include "nav_msgs/Odometry.h"
#include "LinearMath/btQuaternion.h"
#include "LinearMath/btMatrix3x3.h"
void chatterCallback(const nav_msgs::Odometry odom)
{
// POSICION (Respecto al sistema de referencia del plano (fijo))
float pos_x=odom.pose.pose.position.x;
float pos_y=odom.pose.pose.position.y;
float pos_z=odom.pose.pose.position.z;
// ORIENTACION (Respecto al sistema de referencia del plano (fijo))
//quaternion
float qx=odom.pose.pose.orientation.x;
float qy=odom.pose.pose.orientation.y;
float qz=odom.pose.pose.orientation.z;
float qw=odom.pose.pose.orientation.w;
// Conversion quaternion to Euler (RPY).
btQuaternion q(qx,qy,qz,qw);
double roll, pitch, yaw; // ojo no valen float para getRPY()
btMatrix3x3(q).getRPY(roll, pitch, yaw);
ROS_INFO("\nPosicion (x,y,z):\t (%f,%f,%f)\nOrientation (r,p,y):\t (%f,%f,%f)\nOrientation (qx,qy,qz,qw):\t (%f,%f,%f,%f)", pos_x,pos_y,pos_z,roll, pitch, yaw,qx,qy,qz,qw);
}
int main(int argc, char **argv)
{
// ROS_INFO("Entra programa");
ros::init(argc, argv, "sensor_controller");
ros::NodeHandle n1;
ROS_INFO("Intentamos subscribirnos a /Odometria");
ros::Subscriber state = n1.subscribe("/Odometria", 1000, chatterCallback);
ROS_INFO("Hecho");
// Tipo de mensaje: Odometry.msg :
// Header header
// string child_frame_id
// geometry_msgs/PoseWithCovariance pose
// geometry_msgs/TwistWithCovariance twist
ROS_INFO("Leemos posicion y orientacion");
ros::spin();
// Funcion que mantiene a main en un bucle en el que se llama a chatterCallback cada vez que llegue un mensaje
// Terminara una vez que ros::ok() sea falso, es decir, [ctrl+C] o finalize el master.
return 0;
}
//// EJEMPLO DEL TIPO DE MENSAJE RECIBIDO:
//riqui@riqui:~$ rostopic echo -n 1 /Odometria
//header:
//seq: 2167
//stamp: <-- Tiempo de simulacion!!!!!!!!!
//secs: 1536
//nsecs: 60000000
//frame_id: plane
//child_frame_id: ''
//pose:
//pose:
//position:
//x: 12.1967452337
//y: -1.49001181605e-10
//z: 4.01113182678
//orientation:
//x: -2.16810373184e-08
//y: 5.17319014277e-09
//z: 1.31126387298e-08
//w: 1.0
//covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
//twist:
//twist:
//linear:
//x: 8.56895197208e-15
//y: 3.28701100063e-16
//z: 3.20911530406e-17
//angular:
//x: 4.62984550518e-13
//y: -6.8233190863e-13
//z: -1.34464162202e-13
//covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
//---
//riqui@riqui:~$
e-REdING. Biblioteca de la Escuela Superior de Ingenieros de Sevilla.
