File content as of revision 9:e705f028f853:

#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 <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include "geometry_msgs/Vector3.h"
#include "iostream"

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

geometry_msgs::Vector3 odom_base;
ros::Publisher odom_base_pub("/odom_base", &odom_base); 

//for odom_pub
//geometry_msgs::Quaternion odom_quat;
geometry_msgs::TransformStamped odom_trans;
geometry_msgs::TransformStamped odom_trans2;


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;

geometry_msgs::TransformStamped odomTransform2;
tf::TransformBroadcaster odomBroadcaster2;


geometry_msgs::TransformStamped odomTransform3;
tf::TransformBroadcaster odomBroadcaster3;

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;

int prev_leftX =0;
int prev_leftY =0;
int prev_rightX=0;
int prev_rightY=0;
double D= 10;
double x_l= -D/2;
double y_l=0;
double x_r= D/2;
double y_r =0;
double del_x=0;
double del_y=0;
double l_l=0;
double l_r=0;
double x=0;
double y=0;
double theta=0;
double del_theta=0;

int signum(double val){
    if (val > 0) return 1;
    if (val < 0) return -1;
    return 0;
}

int main(){

    nh.initNode();
    nh.advertise(wheelPub);
    nh.advertise(odom_base_pub);
    
    nh.advertise(odomPub);
    odomBroadcaster.init(nh);
    odomBroadcaster2.init(nh);
    odomBroadcaster3.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; 
    
    
    //calculate pose
    int x_l_bar= leftPosX - prev_leftX;
    int y_l_bar= leftPosY - prev_leftY;
    int x_r_bar= rightPosX - prev_rightX;
    int y_r_bar= rightPosY - prev_rightY;
    
    double alpha_l= atan(double(y_l_bar/x_l_bar));
    double alpha_r= atan(double(y_r_bar/x_r_bar));
    cout<<"alpha_l"<<alpha_l;
    
    
    if(alpha_l== 0 || 3.141592){
        l_l=x_l_bar;
    }
    else{
        l_l= y_l_bar/sin(alpha_l);
    }
    if(alpha_r== 0 || 3.141592){
        l_r=x_r_bar;
    }
    else{
        l_r= y_r_bar/sin(alpha_r);
    }
    double gamma= abs( alpha_l - alpha_r);
    double temp1 =y_r-y_l;
    del_theta= signum(temp1)*(1/D)* sqrt(pow(l_l,2) + pow(l_r,2) - (2*cos(gamma)*l_l*l_r));
    if (del_theta==0){
        del_theta=1;
    }
    else{
        continue;
    }
        
    double r_l= l_l/abs(del_theta);
    double r_r= l_r/abs(del_theta);
    x_r= r_r* (sin(alpha_r +del_theta) - sin(alpha_r)) * signum(del_theta) +(D/2);
    y_r= r_r*(cos(alpha_r)-cos(alpha_r +del_theta))* signum(del_theta);
    x_l = r_l*(sin(alpha_l+del_theta) - sin(alpha_l))*signum(del_theta)-(D/2);
    y_l=r_l*(cos(alpha_l)-cos(alpha_l+del_theta))*signum(del_theta);
    
    del_x= (x_r+x_l)*0.5;
    del_y=(y_r+y_l)*0.5;
    
    x= x+ (sqrt(pow(del_x,2)+pow(del_y,2))*cos(theta+atan(del_y/del_x)));
    y= y+ (sqrt(pow(del_x,2)+pow(del_y,2))*sin(theta+atan(del_y/del_x)));
    theta=theta+del_theta;    
    
    //odom_base.x = (leftPosX+rightPosX)*0.05;
    //odom_base.y=(leftPosY + rightPosY)*0.05; 
    odom_base.x =x;
    odom_base.y=y;  
    odom_base.z=theta;
    /*if(rightPosX- leftPosX !=0){
        odom_base.z= atan(double(rightPosY-leftPosY)/double(rightPosX- leftPosX))*(180/3.14);
    }
    else{
        odom_base.z=0.0;
    }*/

    odom_base_pub.publish(&odom_base);
    wheelPub.publish(&wheelMsg);


    publishOdometry();
    
    prev_leftX =leftPosX;
    prev_leftY =leftPosY;
    prev_rightX=rightPosX;
    prev_rightY=rightPosY;
    
    nh.spinOnce();
    wait(.15);
    }
}
void publishOdometry() {
    // Set initial header values
    odomTransform.header.stamp = nh.now();
    odomTransform.header.frame_id = ODOM_FRAME;
    odomTransform.child_frame_id = "mouse1";
        
    odomTransform2.header.stamp = nh.now();
    odomTransform2.header.frame_id = ODOM_FRAME;
    odomTransform2.child_frame_id = "mouse2";
    
    odomTransform3.header.stamp = nh.now();
    odomTransform3.header.frame_id = ODOM_FRAME;
    odomTransform3.child_frame_id = "mouse_base";

    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 = x_l;
    odomTransform.transform.translation.y = y_l;
    odomTransform.transform.translation.z = 0.0;
    odomTransform.transform.rotation = rotation;
    
    odomTransform2.transform.translation.x = x_r;
    odomTransform2.transform.translation.y = y_r;
    odomTransform2.transform.translation.z = 0.0;
    odomTransform2.transform.rotation = rotation;
    
    odomTransform3.transform.translation.x = x;
    odomTransform3.transform.translation.y = y;
    odomTransform3.transform.translation.z = 0.0;
    odomTransform3.transform.rotation = rotation;

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

    // Publish result
    odomPub.publish(&odomMsg);
    odomBroadcaster.sendTransform(odomTransform);
    odomBroadcaster2.sendTransform(odomTransform2);
    odomBroadcaster3.sendTransform(odomTransform3);
    
}