File content as of revision 10:52f59f5a9b7d:

#include "mbed.h"
#include "QEI.h"
#include "ros.h"
#include "ros/time.h"
#include <tf/tf.h>
#include <math.h>

#include "geometry_msgs/Quaternion.h"
#include "geometry_msgs/Vector3.h"
#include "iostream"
#include "std_msgs/Float64.h"


ros::NodeHandle nh;

//PINA / PINB / INDEXPIN / PPR / ENCODING (X2 BY DEFAULT)
QEI leftX(D4, D5, NC, 378,QEI::X2_ENCODING);
QEI leftY(D10, D11, NC, 378,QEI::X2_ENCODING);
QEI rightX(D0, D1, NC, 378,QEI::X2_ENCODING);
QEI rightY(D2, D3, NC, 378,QEI::X2_ENCODING);


geometry_msgs::Quaternion wheelMsg;
ros::Publisher wheelPub("/odom_msg", &wheelMsg); 

std_msgs::Float64 lwheelMsg;
ros::Publisher lwheelPub("/Lodom_msg", &lwheelMsg); 

std_msgs::Float64 rwheelMsg;
ros::Publisher rwheelPub("/Rodom_msg", &rwheelMsg);



//rosserial does not like volatile ints
int leftPosX=0; 
int leftPosY=0;
int rightPosX=0; 
int rightPosY=0;


int main(){

    nh.initNode();
    nh.advertise(wheelPub);
    nh.advertise(lwheelPub);
    nh.advertise(rwheelPub);
    while(true){
    leftPosX = leftX.getPulses();
    leftPosY = leftY.getPulses();
    rightPosX = rightX.getPulses();
    rightPosY = rightY.getPulses();
    
    wheelMsg.x= leftPosX/10;
    wheelMsg.y = leftPosY/10;
    wheelMsg.z = rightPosX/10;
    wheelMsg.w = rightPosY/10; 
    lwheelMsg.data=wheelMsg.y;
    rwheelMsg.data= wheelMsg.w;
    //lwheelMsg.data= (sqrt(pow((wheelMsg.x),2)+pow((wheelMsg.y),2)));
    //rwheelMsg.data= sqrt(pow(wheelMsg.z,2)+pow(wheelMsg.w,2));


    wheelPub.publish(&wheelMsg);
    lwheelPub.publish(&lwheelMsg);
    rwheelPub.publish(&rwheelMsg);
    
    nh.spinOnce();
    wait(.35);
    }
}