File content as of revision 7:f71761cac14a:

#include "mbed.h"
#include "QEI.h"
#include "ros.h"
#include "ros/time.h"
#include <tf/tf.h>
#include "geometry_msgs/Quaternion.h"
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include "geometry_msgs/Vector3.h"

#define ODOM_TICK_TOPIC "/odom_msg"
#define ODOM_TOPIC "/odom"

#define ODOM_FRAME "odom"
#define BASE_FRAME "base_link"

#define RADTODEG(angleInRadians) ((angleInRadians) * 180.0 / M_PI)

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

//for odom_pub
//geometry_msgs::Quaternion odom_quat;
geometry_msgs::TransformStamped odom_trans;
struct OdomValues {
    double x;
    double y;
    double theta;
    
    OdomValues(double x, double y, double theta) {
        this->x = x;
        this->y = y;
        this->theta = theta;    
    }
};
nav_msgs::Odometry odomMsg;
ros::Publisher odomPub("odom", &odomMsg);
geometry_msgs::TransformStamped odomTransform;
tf::TransformBroadcaster odomBroadcaster;

OdomValues currentOdom(0.0, 0.0, 0.0);

void publishOdometry();



//rosserial does not like volatile ints
int leftPosX=0; 
int leftPosY=0;
int rightPosX=0; 
int rightPosY=0;
//double theta=0;
//ros::Time current_time;
int main(){

    nh.initNode();
    nh.advertise(wheelPub);
    
    nh.advertise(odomPub);
    odomBroadcaster.init(nh);
    // pc.baud(57600);

    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;    
    wheelPub.publish(&wheelMsg);


/*    current_time = nh.now();
    
    odom_quat.x=0;
    odom_quat.y=0;
    odom_quat.z=0;
    odom_quat.w=0;    

    
    odom_trans.header.stamp= current_time;
    odom_trans.header.frame_id="mouse1_odom";
    odom_trans.header.stamp = current_time;
    odom_trans.header.frame_id = "odom";
    odom_trans.child_frame_id = "base_link";
    odom_trans.transform.translation.x = wheelMsg.x;
    odom_trans.transform.translation.y = wheelMsg.y;
    odom_trans.transform.translation.z = 0.0;
    odom_trans.transform.rotation = odom_quat;    

    //send the transform
    odom_broadcaster.sendTransform(odom_trans);
    */
    publishOdometry();
    nh.spinOnce();
    wait(.1);
    }
}
void publishOdometry() {
    // Set initial header values
    odomTransform.header.stamp = nh.now();
    odomTransform.header.frame_id = ODOM_FRAME;
    odomTransform.child_frame_id = BASE_FRAME;

    odomMsg.header.stamp = odomTransform.header.stamp;
    odomMsg.header.frame_id = odomTransform.header.frame_id;
    odomMsg.child_frame_id = odomTransform.child_frame_id;

    geometry_msgs::Quaternion rotation = tf::createQuaternionFromYaw(currentOdom.theta);
    
    odomTransform.transform.translation.x = wheelMsg.x;
    odomTransform.transform.translation.y = wheelMsg.y;
    odomTransform.transform.translation.z = 0.0;
    odomTransform.transform.rotation = rotation;

    odomMsg.pose.pose.position.x = wheelMsg.x;
    odomMsg.pose.pose.position.y = wheelMsg.y;
    odomMsg.pose.pose.position.z = 0.0;
    odomMsg.pose.pose.orientation = rotation;

    // Publish result
    odomPub.publish(&odomMsg);
    odomBroadcaster.sendTransform(odomTransform);
}