File content as of revision 5:586f5122b731:

#include "mbed.h"
#include "motordriver.h"
#include "ros.h"
#include "geometry_msgs/Vector3.h"
#include "QEI.h"
#include "geometry_msgs/Quaternion.h"
#include "ros/time.h"
//#include "std_msgs/String.h"

ros:: NodeHandle nh; 

//pwm, fwd, rev, can brake
Motor motorLeft(D9,D5,D4, 1);
Motor motorRight(D10, D7, D8, 1); 

//PINA / PINB / INDEXPIN / PPR / ENCODING (X2 BY DEFAULT)
QEI leftEncoder(D3, D12, NC, 378);
QEI rightEncoder(D2, D13, NC, 378);

void messageCb( const geometry_msgs::Vector3& msg)
{
       motorLeft.speed(msg.x);
       motorRight.speed(msg.y);
}


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

//std_msgs::String str_msg; 
//ros::Publisher chatter("chatter", &str_msg);

ros::Subscriber<geometry_msgs::Vector3> sub("/motor/command", &messageCb );

int leftPulseA=0; 
int rightPulseA=0;
int lastLeftPulse=0;
int lastRightPulse=0;
unsigned long rightTime;
unsigned long leftTime;
unsigned long timeStamp;


int main() {
    
    nh.initNode();
    nh.subscribe(sub);
    //nh.advertise(chatter);
    nh.advertise(wheelPub);
    
    while (true) {
       
    leftPulseA=leftEncoder.getPulses();
    rightPulseA = rightEncoder.getPulses();
    leftTime = leftEncoder.getTime();
    rightTime = rightEncoder.getTime();
    
    if(leftTime>rightTime)
        timeStamp=leftTime;
    else
        timeStamp=rightTime;
    
    wheelMsg.x= leftPulseA;
    wheelMsg.y = rightPulseA;
    wheelMsg.z = timeStamp;
    wheelMsg.w = 0.0;     
    
    
    if((lastLeftPulse!=leftPulseA)||(lastRightPulse!=rightPulseA)){
        wheelPub.publish(&wheelMsg);
        lastLeftPulse=leftPulseA;
        lastRightPulse=rightPulseA;
    }
    
    nh.spinOnce();
    wait_ms(250);
    } 
}